diff --git a/src/Makefile b/src/Makefile index 17b4e43d..3552ffda 100644 --- a/src/Makefile +++ b/src/Makefile @@ -134,6 +134,8 @@ CSRC := $(PORTSRC) \ $(SWIFTNAV_ROOT)/src/decode.o \ $(SWIFTNAV_ROOT)/src/decode_gps_l1.o \ $(SWIFTNAV_ROOT)/src/signal.o \ + $(SWIFTNAV_ROOT)/src/ndb.o \ + $(SWIFTNAV_ROOT)/src/ndb_p1.o \ main.c # C++ sources that can be compiled in ARM or THUMB mode depending on the global diff --git a/src/base_obs.c b/src/base_obs.c index 7604d58c..e58b75ae 100644 --- a/src/base_obs.c +++ b/src/base_obs.c @@ -35,6 +35,7 @@ #include "base_obs.h" #include "ephemeris.h" #include "signal.h" +#include "ndb.h" extern bool disable_raim; @@ -300,9 +301,9 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context) /* Check if we have an ephemeris for this satellite, we will need this to * fill in satellite position etc. parameters. */ - ephemeris_lock(); - ephemeris_t *e = ephemeris_get(sid); - if (ephemeris_valid(e, &t)) { + ephemeris_t ephe; + if((ndb_ephemeris_read(sid, &ephe) == NDB_ERR_NONE) && + (ephemeris_valid(&ephe, &t))) { /* Unpack the observation into a navigation_measurement_t. */ unpack_obs_content( &obs[i], @@ -315,7 +316,7 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context) double clock_err; double clock_rate_err; /* Calculate satellite parameters using the ephemeris. */ - calc_sat_state(e, &t, + calc_sat_state(&ephe, &t, base_obss_rx.nm[base_obss_rx.n].sat_pos, base_obss_rx.nm[base_obss_rx.n].sat_vel, &clock_err, &clock_rate_err); @@ -328,7 +329,6 @@ static void obs_callback(u16 sender_id, u8 len, u8 msg[], void* context) base_obss_rx.nm[base_obss_rx.n].tot = t; base_obss_rx.n++; } - ephemeris_unlock(); } /* If we can, and all the obs have been received, update to using the new diff --git a/src/decode_gps_l1.c b/src/decode_gps_l1.c index b4b21577..c3f9be9f 100644 --- a/src/decode_gps_l1.c +++ b/src/decode_gps_l1.c @@ -18,6 +18,7 @@ #include "sbp.h" #include "sbp_utils.h" #include "decode.h" +#include "ndb.h" #define NUM_GPS_L1_DECODERS 12 @@ -112,10 +113,11 @@ static void decoder_gps_l1_process(const decoder_channel_info_t *channel_info, /* Decoded a new ephemeris. */ ephemeris_new(&e); - ephemeris_t *eph = ephemeris_get(channel_info->sid); - if (!eph->valid) { + u8 v, h; + ndb_ephemeris_info(channel_info->sid, &v, &h, NULL, NULL); + if (!v) { char buf[SID_STR_LEN_MAX]; sid_to_string(buf, sizeof(buf), channel_info->sid); - log_info("%s ephemeris is invalid", buf); + log_info("%s ephemeris wasn't stored", buf); } } diff --git a/src/ephemeris.c b/src/ephemeris.c index 31133572..912b6a42 100644 --- a/src/ephemeris.c +++ b/src/ephemeris.c @@ -10,7 +10,6 @@ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. */ - #include #include #include @@ -23,56 +22,9 @@ #include "timing.h" #include "ephemeris.h" #include "signal.h" +#include "ndb.h" -#define EPHEMERIS_TRANSMIT_EPOCH_SPACING_ms (15 * 1000) -#define EPHEMERIS_MESSAGE_SPACING_ms (200) - -MUTEX_DECL(es_mutex); -static ephemeris_t es[PLATFORM_SIGNAL_COUNT] _CCM; -static ephemeris_t es_candidate[PLATFORM_SIGNAL_COUNT] _CCM; - -static WORKING_AREA_CCM(wa_ephemeris_thread, 1400); - -static msg_t ephemeris_thread(void *arg); - -static msg_t ephemeris_thread(void *arg) -{ - (void)arg; - chRegSetThreadName("ephemeris"); - - systime_t tx_epoch = chTimeNow(); - while (1) { - - for (u32 i=0; isid); - gps_time_t t = get_current_time(); + if (!e->valid) { + log_error("Invalid ephemeris for %s", buf); + return; + } + u32 index = sid_to_global_index(e->sid); - if (!ephemeris_valid(&es[index], &t)) { - /* Our currently used ephemeris is bad, so we assume this is better. */ - log_info("New untrusted ephemeris for %s", buf); - ephemeris_lock(); - es[index] = es_candidate[index] = *e; - ephemeris_unlock(); - } else if (ephemeris_equal(&es_candidate[index], e)) { + if(ephemeris_equal(&es_candidate[index], e)) { /* The received ephemeris matches our candidate, so we trust it. */ log_info("New trusted ephemeris for %s", buf); - ephemeris_lock(); - es[index] = *e; - ephemeris_unlock(); + if (ndb_ephemeris_store(e, NDB_DS_RECEIVER) != NDB_ERR_NONE) + log_error("Error storing ephemeris for %s", buf); } else { /* This is our first reception of this new ephemeris, so treat it with * suspicion and call it the new candidate. */ log_info("New ephemeris candidate for %s", buf); - ephemeris_lock(); es_candidate[index] = *e; - ephemeris_unlock(); } } @@ -121,42 +68,18 @@ static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* contex log_warn("Ignoring ephemeris for invalid sat"); return; } - - ephemeris_new(&e); + /* We trust epehemeris that we received over SBP, so save it to NDB right + * away. If we receive new one from the sky twice it will replace it. */ + ndb_ephemeris_store(&e, NDB_DS_SBP); } void ephemeris_setup(void) { memset(es_candidate, 0, sizeof(es_candidate)); - memset(es, 0, sizeof(es)); - for (u32 i=0; i void ephemeris_setup(void); -void ephemeris_lock(void); -void ephemeris_unlock(void); void ephemeris_new(ephemeris_t *e); -ephemeris_t *ephemeris_get(gnss_signal_t sid); - #endif diff --git a/src/main.c b/src/main.c index e16a4106..4b216c5e 100644 --- a/src/main.c +++ b/src/main.c @@ -43,6 +43,7 @@ #include "decode.h" #include "decode_gps_l1.h" #include "signal.h" +#include "ndb.h" extern void ext_setup(void); @@ -223,6 +224,7 @@ int main(void) READ_ONLY_PARAMETER("system_info", "nap_fft_index_bits", nap_acq_fft_index_bits, TYPE_INT); ephemeris_setup(); + ndb_init(); /* Send message to inform host we are up and running. */ u32 startup_flags = 0; diff --git a/src/manage.c b/src/manage.c index 527f62e3..dad6f3a6 100644 --- a/src/manage.c +++ b/src/manage.c @@ -43,6 +43,7 @@ #include "./system_monitor.h" #include "settings.h" #include "signal.h" +#include "ndb.h" /** \defgroup manage Manage * Manage acquisition and tracking. @@ -112,9 +113,6 @@ typedef struct { static tracking_startup_fifo_t tracking_startup_fifo; static MUTEX_DECL(tracking_startup_mutex); - -static almanac_t almanac[PLATFORM_SIGNAL_COUNT]; - static float elevation_mask = 0.0; /* degrees */ static bool sbas_enabled = false; @@ -194,8 +192,6 @@ void manage_acq_setup() acq_status[i].sid = sid_from_global_index(i); track_mask[i] = false; - almanac[i].valid = 0; - if (!sbas_enabled && (sid_to_constellation(acq_status[i].sid) == CONSTELLATION_SBAS)) { acq_status[i].masked = true; @@ -250,10 +246,11 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t, /* Do we have a suitable ephemeris for this sat? If so, use that in preference to the almanac. */ - const ephemeris_t *e = ephemeris_get(sid); - if (ephemeris_valid(e, t)) { + ephemeris_t ephe; + enum ndb_op_code oc = ndb_ephemeris_read(sid, &ephe); + if((NDB_ERR_NONE == oc) && ephemeris_valid(&ephe, t)) { double sat_pos[3], sat_vel[3], el_d; - calc_sat_state(e, t, sat_pos, sat_vel, &_, &_); + calc_sat_state(&ephe, t, sat_pos, sat_vel, &_, &_); wgsecef2azel(sat_pos, position_solution.pos_ecef, &_, &el_d); el = (float)(el_d) * R2D; if (el < elevation_mask) @@ -269,14 +266,15 @@ static u16 manage_warm_start(gnss_signal_t sid, const gps_time_t* t, if (time_quality >= TIME_FINE) dopp_uncertainty = DOPP_UNCERT_EPHEM; } else { - const almanac_t *a = &almanac[sid_to_global_index(sid)]; - if (a->valid) { - calc_sat_az_el_almanac(a, t->tow, t->wn-1024, + almanac_t alma; + oc = ndb_almanac_read(sid, &alma); + if((NDB_ERR_NONE == oc) && (alma.valid)) { + calc_sat_az_el_almanac(&alma, t->tow, t->wn-1024, position_solution.pos_ecef, &_, &el_d); el = (float)(el_d) * R2D; if (el < elevation_mask) return SCORE_BELOWMASK; - dopp_hint = -calc_sat_doppler_almanac(a, t->tow, t->wn, + dopp_hint = -calc_sat_doppler_almanac(&alma, t->tow, t->wn, position_solution.pos_ecef); } else { return SCORE_COLDSTART; /* Couldn't determine satellite state. */ @@ -532,9 +530,9 @@ static void manage_track() } /* Is ephemeris or alert flag marked unhealthy?*/ - const ephemeris_t *e = ephemeris_get(sid); - /* TODO: check alert flag */ - if (e->valid && !satellite_healthy(e)) { + u8 v, h; + enum ndb_op_code oc = ndb_ephemeris_info(sid, &v, &h, NULL, NULL); + if((NDB_ERR_NONE == oc) && v && !h) { log_info("%s unhealthy, dropping", buf); drop_channel(i); acq->state = ACQ_PRN_UNHEALTHY; @@ -608,9 +606,9 @@ s8 use_tracking_channel(u8 i) /* Channel time of week has been decoded. */ && (tracking_channel_tow_ms_get(i) != TOW_INVALID) /* Nav bit polarity is known, i.e. half-cycles have been resolved. */ - && tracking_channel_bit_polarity_resolved(i)) + && tracking_channel_bit_polarity_resolved(i)) { /* TODO: Alert flag is not set */ - { + /* Ephemeris must be valid, not stale. Satellite must be healthy. This also acts as a sanity check on the channel TOW.*/ gps_time_t t = { @@ -619,9 +617,14 @@ s8 use_tracking_channel(u8 i) .wn = WN_UNKNOWN, .tow = 1e-3 * tracking_channel_tow_ms_get(i) }; - ephemeris_t *e = ephemeris_get(tracking_channel_sid_get(i)); - return ephemeris_valid(e, &t) && satellite_healthy(e); - } else return 0; + gnss_signal_t sid = tracking_channel_sid_get(i); + u8 v, h, fit_interval; + gps_time_t toe; + ndb_ephemeris_info(sid, &v, &h, &toe, &fit_interval); + return ephemeris_params_valid(v, fit_interval, &toe, &t) && h; + } + + return 0; } u8 tracking_channels_ready() diff --git a/src/ndb.c b/src/ndb.c new file mode 100644 index 00000000..2f1bd47e --- /dev/null +++ b/src/ndb.c @@ -0,0 +1,342 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ +#include +#include +#include +#include +#include "libsbp/piksi.h" +#include "sbp.h" +#include "ndb.h" + +typedef Thread *ndb_thread_t; + +static ndb_thread_t ndb_thread; +#define NDB_THREAD_PRIORITY (LOWPRIO) +static WORKING_AREA_CCM(ndb_thread_wa, 1756); +static msg_t ndb_service_thread(void*); +MUTEX_DECL(data_access); + +ndb_element_metadata_t *wq_first = NULL; +ndb_element_metadata_t *wq_last = NULL; +CONDVAR_DECL(wq_new_data); +MUTEX_DECL(wq_access); + +static enum ndb_op_code ndb_wq_put(ndb_element_metadata_t* md); +static enum ndb_op_code ndb_wq_get(ndb_element_metadata_t** md); + +static enum ndb_op_code ndb_open_file(ndb_file_t *file, char *v); +static enum ndb_op_code ndb_read(ndb_file_t *f, void *first_element, size_t s); + +static enum ndb_op_code do_nv_writes(bool *data_write_ok, bool *md_write_ok); +static void do_sbp_updates(); + +enum ndb_op_code ndb_p1_init(); +enum ndb_op_code ndb_p3_init() NDB_WEAK; + +void ndb_p1_sbp_update(); +void ndb_p3_sbp_update() NDB_WEAK; + +enum ndb_op_code ndb_gps_l2cm_l2c_cap_read(u32 *l2c_cap) NDB_WEAK; +enum ndb_op_code ndb_gps_l2cm_l2c_cap_store(u32 *l2c_cap, + enum ndb_data_source src) NDB_WEAK; + +enum ndb_op_code ndb_wq_put(ndb_element_metadata_t* md) +{ + chMtxLock(&wq_access); + if (NULL == wq_last) { + wq_first = wq_last = md; + } else { + wq_last->next = md; + wq_last = md; + } + md->next = NULL; + chCondSignal(&wq_new_data); + chMtxUnlock(); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_wq_get(ndb_element_metadata_t** md) +{ + chMtxLock(&wq_access); + if (NULL == wq_first) { + /* NOTE: chCondwaitTimeout does not care of + locking mutex again when timeout occurs*/ + if (chCondWaitTimeout(&wq_new_data, + NV_WRITE_REQ_TIMEOUT) == RDY_TIMEOUT) { + *md = NULL; + return NDB_ERR_TIMEOUT; + } + } + + *md = wq_first; + wq_first = wq_first->next; + + if (NULL == wq_first) { + wq_last = NULL; + } + + chMtxUnlock(); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_init() +{ + ndb_p1_init(); + ndb_p3_init(); + + ndb_thread = chThdCreateStatic(ndb_thread_wa, sizeof(ndb_thread_wa), + NDB_THREAD_PRIORITY, + ndb_service_thread, + NULL); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_load_data(ndb_file_t *f, void *b, + ndb_element_metadata_t* b_md, size_t el_size, + size_t n) +{ + size_t ds = el_size * n; + size_t mds = sizeof(ndb_element_metadata_nv_t) * n; + ndb_element_metadata_nv_t md_nv[n]; + + if (ndb_open_file(f, ndb_file_version) == NDB_ERR_NONE) { + if (cfs_seek(f->fh, 0, CFS_SEEK_SET) != 0) { + cfs_close(f->fh); + f->fh = -1; + return NDB_ERR_FILE_IO; + } + if (ndb_read(f, b, ds) == NDB_ERR_NONE) { + if (ndb_read(f, md_nv, mds) == NDB_ERR_NONE) { + u32 i; + for (i = 0; i < n; i++) { + b_md[i].nv_data = md_nv[i]; + } + return NDB_ERR_NONE; + } + } + + memset(b, 0, ds); + memset(md_nv, 0, mds); + memset(b_md, 0, sizeof(ndb_element_metadata_t) * n); + + if (ndb_write_file_data(f, 0, b, ds) == NDB_ERR_NONE) { + if (ndb_write_file_data(f, ds, md_nv, mds) == NDB_ERR_NONE) { + return NDB_ERR_INIT_DONE; + } + } + } + + return NDB_ERR_FILE_IO; +} + +/** + * Returns time stamp in seconds. + */ +ndb_timestamp_t ndb_get_timestamp() +{ + return chTimeNow(); +} + +static msg_t ndb_service_thread(void* p) +{ + (void) (p); + chRegSetThreadName("ndb"); + bool data_write_ok = false; + bool md_write_ok = false; + + while (true) { + if (NDB_ERR_TIMEOUT == do_nv_writes(&data_write_ok, &md_write_ok)) { + do_sbp_updates(); + } else { + if (data_write_ok && md_write_ok) + log_info("Data and metadata were written to the NDB file"); + else { + if (!data_write_ok) + log_error("Error writing data to the NDB file"); + if (!md_write_ok) + log_error("Error writing metadata to the NDB file"); + } + } + } + return 0; +} + +enum ndb_op_code do_nv_writes(bool *data_write_ok, bool *md_write_ok) +{ + ndb_element_metadata_t* md = NULL; + enum ndb_op_code ret; + int changed; + + if (NDB_ERR_TIMEOUT == ndb_wq_get(&md)) { + return NDB_ERR_TIMEOUT; + } else { + ndb_element_metadata_t md_copy; + ndb_element_t buf; + ndb_lock(1); + memcpy(&buf, md->data, md->data_size); + memcpy(&md_copy, md, sizeof(ndb_element_metadata_t)); + ndb_lock(0); + ret = ndb_write_file_data(md->file, md->data_size * md->index, &buf, + md->data_size); + *data_write_ok = NDB_ERR_NONE == ret; + if (data_write_ok) { + ndb_lock(1); + changed = memcmp(md->data, &buf, md->data_size); + ndb_lock(0); + if (!changed) { + /* + * Data hasn't changed while i/o operation was running - + * mark as saved and write metadata to NVM. + * */ + md->nv_data.state &= ~NDB_IE_DIRTY; + cfs_offset_t offset = md->data_size * md->n_elements + + sizeof(ndb_element_metadata_nv_t) * md->index; + ret = ndb_write_file_data(md->file, offset, &md_copy, + sizeof(ndb_element_metadata_nv_t)); + *md_write_ok = NDB_ERR_NONE == ret; + } else { + /* Data has changed - schedule saving once again */ + md->nv_data.state |= NDB_IE_DIRTY; + ndb_wq_put(md); + } + } + } + + return NDB_ERR_NONE; +} + +void do_sbp_updates() +{ + ndb_p1_sbp_update(); + ndb_p3_sbp_update(); +} + +/** + * Opens NDB file that stores information elements of certain type. + * This function checks if version of the file matches the passed one + * and if it doesn't creates empty file automatically. + */ +enum ndb_op_code ndb_open_file(ndb_file_t *file, char *version) +{ + char ver[MAX_NDB_FILE_VERSION_LEN]; + + file->fh = cfs_open(file->name, CFS_READ + CFS_WRITE); + if (file->fh < 0) { + cfs_coffee_reserve(file->name, file->expected_size); + cfs_coffee_configure_log(file->name, 256 * 2, 256); + file->fh = cfs_open(file->name, CFS_READ + CFS_WRITE); + if (file->fh < 0) { + return NDB_ERR_FILE_IO; + } + } + + if (cfs_read(file->fh, ver, sizeof(ndb_file_version)) + == sizeof(ndb_file_version)) { + if (memcmp(&ver, version, sizeof(ndb_file_version)) == 0) { + return NDB_ERR_NONE; + } else { + cfs_close(file->fh); + cfs_remove(file->name); + return ndb_open_file(file, version); + } + } + + if (cfs_seek(file->fh, 0, CFS_SEEK_SET) != 0) { + cfs_close(file->fh); + file->fh = -1; + return NDB_ERR_FILE_IO; + } + + if (cfs_write(file->fh, version, sizeof(ndb_file_version)) + != sizeof(ndb_file_version)) { + cfs_close(file->fh); + file->fh = -1; + return NDB_ERR_FILE_IO; + } + + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_write_file_data(ndb_file_t *f, cfs_offset_t o, void *b, + size_t l) +{ + int offset = sizeof(ndb_file_version) + o; + int ret = cfs_seek(f->fh, offset, CFS_SEEK_SET); + + if (ret == offset) { + if (cfs_write(f->fh, b, l) == (int) l) { + return NDB_ERR_NONE; + } + } + return NDB_ERR_FILE_IO; +} + +/** + * Reading from NDB file. + * This function is to be called only during initialization of NDB. + * It is intended to read all information elements collection from + * file into memory. + */ +enum ndb_op_code ndb_read(ndb_file_t *file, void *first_element, size_t size) +{ + if (file->fh < 0) { + return NDB_ERR_FILE_IO; + } + + if (cfs_seek(file->fh, sizeof(ndb_file_version), CFS_SEEK_SET) + == (cfs_offset_t) -1) { + cfs_close(file->fh); + file->fh = -1; + return NDB_ERR_FILE_IO; + } + return + cfs_read(file->fh, first_element, size) == (int) size ? + NDB_ERR_NONE : NDB_ERR_FILE_IO; +} + +void ndb_lock(u8 lock) +{ + lock ? chMtxLock(&data_access) : chMtxUnlock(); +} + +void ndb_retrieve(void* out, void* cached, size_t size) +{ + ndb_lock(1); + memcpy(out, cached, size); + ndb_lock(0); +} + +enum ndb_op_code ndb_update(void* cached, void* new, size_t size, + enum ndb_data_source src, + ndb_element_metadata_t *md, u8 and_state_mask, + u8 or_state_mask) +{ + ndb_lock(1); + if (memcmp(new, cached, size) == 0) { + ndb_lock(0); + return NDB_ERR_NONE; + } + memcpy(cached, new, size); + md->update_c++; + if (!md->update_c) + md->update_c++; /* should never be 0 */ + md->nv_data.state &= and_state_mask; + md->nv_data.state |= or_state_mask; + md->nv_data.received_at = ndb_get_timestamp(); + md->nv_data.state |= NDB_IE_DIRTY; + md->nv_data.source = src; + + ndb_wq_put(md); + + ndb_lock(0); + return NDB_ERR_NONE; +} diff --git a/src/ndb.h b/src/ndb.h new file mode 100644 index 00000000..bc9ab497 --- /dev/null +++ b/src/ndb.h @@ -0,0 +1,143 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#ifndef SRC_NDB_H_ +#define SRC_NDB_H_ + +#include +#include +#include +#include +#include +#include +#include +#include "signal.h" + +enum ndb_op_code +{ + NDB_ERR_NONE = 0, /**< No error */ + NDB_ERR_MISSING_IE, /**< DB doesn't contain value of this IE */ + NDB_ERR_UNSUPPORTED, + NDB_ERR_FILE_IO, + NDB_ERR_INIT_DONE, + NDB_ERR_BAD_PARAM, + NDB_ERR_TIMEOUT +}; + +enum ndb_data_source +{ + NDB_DS_UNDEFINED = 0, + NDB_DS_INIT, + NDB_DS_RECEIVER, + NDB_DS_SBP +}; + +#define MAX_NDB_FILE_VERSION_LEN 64 +#define ndb_file_version GIT_VERSION + +/** NDB IE update counter type */ +typedef u8 ndb_update_counter_t; +/** NDB file handler */ +typedef int ndb_file_handle_t; +/** NDB Timestamp */ +typedef systime_t ndb_timestamp_t; +/* Information element size */ +typedef u8 ndb_ie_size_t; +/* Information element index in the array */ +typedef u8 ndb_ie_index_t; + +/** NDB File */ +typedef struct +{ + char* name; /**< Name of the file */ + ndb_file_handle_t fh; /**< File handle */ + u32 expected_size; /**< Expected file size */ +} ndb_file_t; + +typedef union +{ + ephemeris_t ephe; + almanac_t alma; + u32 l2c_capabilities; +} ndb_element_t; + +/* Maximum waiting time for write request, milliseconds */ +#define NV_WRITE_REQ_TIMEOUT 100 + +#define NDB_IE_DIRTY (1 << 0) /**< needs to be written to NVM */ +#define NDB_IE_VALID (1 << 1) /**< value in RAM has been set */ +#define NDB_IE_IS_VALID(md_ptr) (md_ptr->nv_data.state & NDB_IE_VALID) + +typedef struct __attribute__((packed)) ndb_element_metadata_nv +{ + ndb_timestamp_t received_at; + enum ndb_data_source source; + u8 state; +} ndb_element_metadata_nv_t; + +typedef struct __attribute__((packed)) ndb_element_metadata +{ + ndb_element_metadata_nv_t nv_data; + ndb_update_counter_t update_c; + void *data; + ndb_ie_size_t data_size; + ndb_ie_index_t index; + u16 n_elements; + ndb_file_t *file; + struct ndb_element_metadata *next; +} ndb_element_metadata_t; + +#define NDB_WEAK __attribute__ ((weak, alias ("ndb_not_implemented"))) +enum ndb_op_code ndb_not_implemented() __attribute__ ((weak)); +inline enum ndb_op_code ndb_not_implemented() +{ + return NDB_ERR_UNSUPPORTED; +} + +void ndb_lock(u8 lock); +ndb_timestamp_t ndb_get_timestamp(); +enum ndb_op_code ndb_not_implemented(); +enum ndb_op_code ndb_init(); +enum ndb_op_code ndb_load_data(ndb_file_t *f, void *b, + ndb_element_metadata_t* b_md, size_t el_size, + size_t n); + +enum ndb_op_code ndb_update(void* cached, void* new, size_t size, + enum ndb_data_source src, + ndb_element_metadata_t *md, + u8 and_state_mask, u8 or_state_mask); +void ndb_retrieve(void* out, void* cached, size_t size); +enum ndb_op_code ndb_write_file_data(ndb_file_t *f, cfs_offset_t o, void *b, + size_t l); + +enum ndb_op_code ndb_invalidate(ndb_element_metadata_t *md); + +enum ndb_op_code ndb_ephemeris_read(gnss_signal_t sid, ephemeris_t *e); +enum ndb_op_code ndb_ephemeris_store(ephemeris_t *e, enum ndb_data_source); +enum ndb_op_code ndb_ephemeris_info(gnss_signal_t sid, u8* v, u8* h, + gps_time_t* toe, u8* fit_interval); +enum ndb_op_code ndb_update_cache_ephemeris(ephemeris_t *cached_e, + ndb_update_counter_t *uc); + +enum ndb_op_code ndb_almanac_read(gnss_signal_t sid, almanac_t *a); +enum ndb_op_code ndb_almanac_store(almanac_t *a, enum ndb_data_source); +enum ndb_op_code ndb_update_cache_almanac(almanac_t *cached_a, + ndb_update_counter_t *uc); + +/** + * L2CM specific + */ +enum ndb_op_code ndb_gps_l2cm_l2c_cap_read(u32 *l2c_cap); +enum ndb_op_code ndb_gps_l2cm_l2c_cap_store(u32 *l2c_cap, + enum ndb_data_source src); + +#endif /* SRC_NDB_H_ */ diff --git a/src/ndb_p1.c b/src/ndb_p1.c new file mode 100644 index 00000000..6a7e2065 --- /dev/null +++ b/src/ndb_p1.c @@ -0,0 +1,212 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ +#include +#include "ndb.h" +#include "libswiftnav/logging.h" +#include "timing.h" +#include "sbp.h" +#include "sbp_utils.h" + +ephemeris_t ndb_ephemeris[PLATFORM_SIGNAL_COUNT] _CCM; +ndb_element_metadata_t ndb_ephemeris_md[PLATFORM_SIGNAL_COUNT] _CCM; +almanac_t ndb_almanac[PLATFORM_SIGNAL_COUNT] _CCM; +ndb_element_metadata_t ndb_almanac_md[PLATFORM_SIGNAL_COUNT] _CCM; + +#define NDB_EPHE_FILE_NAME "ephe" +#define NDB_ALMA_FILE_NAME "alma" + +ndb_file_t ndb_ephe_file = { .name = NDB_EPHE_FILE_NAME, .fh = -1, + .expected_size = sizeof(ndb_file_version) + + sizeof(ephemeris_t) * PLATFORM_SIGNAL_COUNT + + sizeof(ndb_element_metadata_nv_t) * PLATFORM_SIGNAL_COUNT }; + +ndb_file_t ndb_alma_file = { .name = NDB_ALMA_FILE_NAME, .fh = -1, + .expected_size = sizeof(ndb_file_version) + + sizeof(almanac_t) * PLATFORM_SIGNAL_COUNT + + sizeof(ndb_element_metadata_nv_t) * PLATFORM_SIGNAL_COUNT }; + +enum ndb_op_code ndb_p1_init() +{ + u32 i; + + memset(ndb_ephemeris, 0, sizeof(ndb_ephemeris)); + for (u32 i = 0; i < PLATFORM_SIGNAL_COUNT; i++) { + ndb_ephemeris[i].sid = sid_from_global_index(i); + } + + if (ndb_load_data(&ndb_ephe_file, ndb_ephemeris, ndb_ephemeris_md, + sizeof(ephemeris_t), PLATFORM_SIGNAL_COUNT) + != NDB_ERR_NONE) { + log_info("No ephemeris file present in flash, create an empty one"); + } else + log_info("Ephemerides loaded from flash"); + + for (i = 0; i < PLATFORM_SIGNAL_COUNT; i++) { + ndb_ephemeris_md[i].data = &ndb_ephemeris[i]; + ndb_ephemeris_md[i].data_size = sizeof(ephemeris_t); + ndb_ephemeris_md[i].index = i; + ndb_ephemeris_md[i].n_elements = PLATFORM_SIGNAL_COUNT; + ndb_ephemeris_md[i].file = &ndb_ephe_file; + ndb_ephemeris_md[i].next = NULL; + ndb_ephemeris_md[i].update_c = 1; + } + + memset(ndb_almanac, 0, sizeof(ndb_almanac)); + for (u32 i = 0; i < PLATFORM_SIGNAL_COUNT; i++) { + ndb_almanac[i].sid = sid_from_global_index(i); + } + + if (ndb_load_data(&ndb_alma_file, ndb_almanac, ndb_almanac_md, + sizeof(almanac_t), PLATFORM_SIGNAL_COUNT) != NDB_ERR_NONE) { + log_info("No almanac file present in flash, create an empty one"); + } else + log_info("Almanacs loaded from flash"); + + for (i = 0; i < PLATFORM_SIGNAL_COUNT; i++) { + ndb_almanac_md[i].data = &ndb_almanac[i]; + ndb_almanac_md[i].data_size = sizeof(almanac_t); + ndb_almanac_md[i].index = i; + ndb_almanac_md[i].n_elements = PLATFORM_SIGNAL_COUNT; + ndb_almanac_md[i].file = &ndb_alma_file; + ndb_almanac_md[i].next = NULL; + ndb_almanac_md[i].update_c = 1; + } + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_ephemeris_read(gnss_signal_t sid, ephemeris_t *e) +{ + u16 idx = sid_to_global_index(sid); + ndb_retrieve(e, &ndb_ephemeris[idx], sizeof(ephemeris_t)); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_update_cache_ephemeris(ephemeris_t *cached_e, + ndb_update_counter_t *uc) +{ + u16 idx = sid_to_global_index(cached_e->sid); + ndb_element_metadata_t *md = &ndb_ephemeris_md[idx]; + + if (md->update_c == *uc) { + return NDB_ERR_NONE; + } + + enum ndb_op_code r = ndb_ephemeris_read(cached_e->sid, cached_e); + if (NDB_ERR_NONE != r) { + return r; + } + + *uc = md->update_c; + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_ephemeris_store(ephemeris_t *e, enum ndb_data_source src) +{ + if (!e->valid) { + return NDB_ERR_BAD_PARAM; + } + u16 idx = sid_to_global_index(e->sid); + return ndb_update(&ndb_ephemeris[idx], e, sizeof(ephemeris_t), src, + &ndb_ephemeris_md[idx], 0xff, NDB_IE_VALID); +} + +enum ndb_op_code ndb_ephemeris_info(gnss_signal_t sid, u8* v, u8* h, + gps_time_t* toe, u8* fit_interval) +{ + u16 idx = sid_to_global_index(sid); + ndb_lock(1); + *v = ndb_ephemeris[idx].valid; + *h = ndb_ephemeris[idx].healthy; + if (NULL != toe) { + *toe = ndb_ephemeris[idx].toe; + } + if (NULL != fit_interval) { + *fit_interval = ndb_ephemeris[idx].fit_interval; + } + ndb_lock(0); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_almanac_read(gnss_signal_t sid, almanac_t *a) +{ + u16 idx = sid_to_global_index(sid); + ndb_retrieve(a, &ndb_almanac[idx], sizeof(almanac_t)); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_almanac_store(almanac_t *a, enum ndb_data_source src) +{ + u16 idx = sid_to_global_index(a->sid); + return ndb_update(&ndb_almanac[idx], a, sizeof(almanac_t), src, + &ndb_almanac_md[idx], 0xff, NDB_IE_VALID); +} + +enum ndb_op_code ndb_update_cache_almanac(almanac_t *cached_a, + ndb_update_counter_t *uc) +{ + u16 idx = sid_to_global_index(cached_a->sid); + ndb_element_metadata_t *md = &ndb_almanac_md[idx]; + + if (md->update_c == *uc) { + return NDB_ERR_NONE; + } + + enum ndb_op_code r = ndb_almanac_read(cached_a->sid, cached_a); + if (NDB_ERR_NONE != r) { + return r; + } + + *uc = md->update_c; + return NDB_ERR_NONE; +} + +#define EPHEMERIS_MESSAGE_SPACING_cycle (200 / NV_WRITE_REQ_TIMEOUT) +#define EPHEMERIS_TRANSMIT_EPOCH_SPACING_cycle (15 * 1000 / NV_WRITE_REQ_TIMEOUT) + +/** The function sends ephemeris if valid + * Function called every NV_WRITE_REQ_TIMEOUT ms from NDB thread*/ +void ndb_p1_sbp_update() +{ + static u32 count = 0; + static u32 i = 0; + static bool tx_en = true; /* initially enable SBP TX */ + + if (tx_en) { + if (!(count % EPHEMERIS_MESSAGE_SPACING_cycle)) { + /* every 200 ms send eph of a SV */ + ephemeris_t e; + gps_time_t t = get_current_time(); + gnss_signal_t sid = sid_from_global_index(i); + ndb_ephemeris_read(sid, &e); + if (ephemeris_valid(&e, &t)) { + msg_ephemeris_t msg; + pack_ephemeris(&e, &msg); + sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *) &msg); + } + i++; + if (i == PLATFORM_SIGNAL_COUNT) { + /* no eph to send */ + i = 0; + tx_en = false; + } + } + } else { + if (!(count % EPHEMERIS_TRANSMIT_EPOCH_SPACING_cycle)) { + /* every 15 sec enable tx again */ + count = 0; + tx_en = true; + return; + } + } + + count++; +} diff --git a/src/ndb_p3.c b/src/ndb_p3.c new file mode 100644 index 00000000..ac1193a6 --- /dev/null +++ b/src/ndb_p3.c @@ -0,0 +1,86 @@ +/* + * Copyright (C) 2016 Swift Navigation Inc. + * Contact: Roman Gezikov + * + * This source is subject to the license found in the file 'LICENSE' which must + * be be distributed together with this source. All other rights reserved. + * + * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND, + * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED + * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE. + */ + +#include +#include "ndb.h" + +#define GPS_L2C_CAPB_FILE_NAME "l2capb" +static u32 gps_l2c_capabilities; +static ndb_element_metadata_t gps_l2c_capabilities_md; + +static ndb_file_t gps_l2c_capb_file = { + .name = GPS_L2C_CAPB_FILE_NAME, + .fh = -1, + .expected_size = sizeof(ndb_file_version) + + sizeof(gps_l2c_capabilities) + + sizeof(ndb_element_metadata_nv_t) +}; + +enum ndb_op_code ndb_p3_init() +{ + enum ndb_op_code r; + + r = ndb_load_data(&gps_l2c_capb_file, &gps_l2c_capabilities, + &gps_l2c_capabilities_md, + sizeof(gps_l2c_capabilities), 1); + gps_l2c_capabilities_md.data = &gps_l2c_capabilities_md; + gps_l2c_capabilities_md.data_size = sizeof(gps_l2c_capabilities_md); + gps_l2c_capabilities_md.index = 0; + gps_l2c_capabilities_md.n_elements = 1; + gps_l2c_capabilities_md.file = &gps_l2c_capb_file; + gps_l2c_capabilities_md.next = NULL; + gps_l2c_capabilities_md.update_c = 1; + + switch(r) { + case NDB_ERR_NONE: + /* Do nothing, everything is OK */ + break; + case NDB_ERR_INIT_DONE: + { + /* Reading from file failed, value was initialized with 0. + * Change value to all ones and initiate writing to file */ + u32 new_val = 0xffffffff; + if (ndb_update(&gps_l2c_capabilities, + &new_val, + sizeof(gps_l2c_capabilities), + NDB_DS_INIT, + &gps_l2c_capabilities_md, 0xff, NDB_IE_VALID) + != NDB_ERR_NONE) { + /* TODO: log failure */ + } + } + break; + default: + /* TODO: log failure */ + break; + } + + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_gps_l2cm_l2c_cap_read(u32 *l2c_cap) +{ + ndb_retrieve(l2c_cap, &gps_l2c_capabilities, sizeof(gps_l2c_capabilities)); + return NDB_ERR_NONE; +} + +enum ndb_op_code ndb_gps_l2cm_l2c_cap_store(u32 *l2c_cap, + enum ndb_data_source src) +{ + return ndb_update(&gps_l2c_capabilities, l2c_cap, + sizeof(gps_l2c_capabilities), src, &gps_l2c_capabilities_md, + 0xff, NDB_IE_VALID); +} + +void ndb_p3_sbp_update() +{ +} diff --git a/src/pps.c b/src/pps.c index 31dfa739..692f1633 100644 --- a/src/pps.c +++ b/src/pps.c @@ -30,7 +30,7 @@ /** Number of microseconds the PPS will remain high (default: 200000). */ u32 pps_width_microseconds = PPS_WIDTH_MICROSECONDS; -static WORKING_AREA_CCM(wa_pps_thread, 256); +static WORKING_AREA(wa_pps_thread, 256); static msg_t pps_thread(void *arg) { (void)arg; diff --git a/src/solution.c b/src/solution.c index 4d8ffe77..77dbe624 100644 --- a/src/solution.c +++ b/src/solution.c @@ -26,6 +26,7 @@ #include #include +#include "ndb.h" #include "board/leds.h" #include "position.h" #include "nmea.h" @@ -375,7 +376,7 @@ static void update_sat_elevations(const navigation_measurement_t nav_meas[], } } -static WORKING_AREA_CCM(wa_solution_thread, 8000); +static WORKING_AREA_CCM(wa_solution_thread, 8960); static msg_t solution_thread(void *arg) { (void)arg; @@ -417,20 +418,19 @@ static msg_t solution_thread(void *arg) static u8 n_ready_old = 0; u64 nav_tc = nap_timing_count(); static navigation_measurement_t nav_meas[MAX_CHANNELS]; - - const channel_measurement_t *p_meas[n_ready]; - navigation_measurement_t *p_nav_meas[n_ready]; - const ephemeris_t *p_e_meas[n_ready]; + ephemeris_t ephe_cache[MAX_CHANNELS]; + const channel_measurement_t *p_meas[MAX_CHANNELS]; + static navigation_measurement_t *p_nav_meas[MAX_CHANNELS]; + static ephemeris_t *p_e_meas[MAX_CHANNELS]; for (u8 i=0; i= 4) { output_baseline(num_sdiffs, sdiffs, &position_solution.time); } @@ -677,11 +678,11 @@ static msg_t time_matched_obs_thread(void *arg) ); chMtxUnlock(); - u16 *sds_lock_counters[n_sds]; + u16 *sds_lock_counters[MAX_CHANNELS]; for (u32 i=0; i 0) { diff --git a/src/system_monitor.c b/src/system_monitor.c index 20768a17..0df0641d 100644 --- a/src/system_monitor.c +++ b/src/system_monitor.c @@ -237,7 +237,7 @@ static void debug_threads() } } -static WORKING_AREA_CCM(wa_watchdog_thread, 1024); +static WORKING_AREA(wa_watchdog_thread, 1024); static msg_t watchdog_thread(void *arg) { (void)arg; diff --git a/src/track.c b/src/track.c index 4ada8d57..1dd20fb7 100644 --- a/src/track.c +++ b/src/track.c @@ -83,7 +83,7 @@ typedef struct { tracker_t *tracker; } tracker_channel_t; -static tracker_channel_t tracker_channels[NUM_TRACKER_CHANNELS]; +static tracker_channel_t tracker_channels[NUM_TRACKER_CHANNELS] _CCM; static const tracker_interface_t tracker_interface_default = { .code = CODE_INVALID,