diff --git a/EVB-2/IS_EVB-2/IS_EVB-2.cppproj b/EVB-2/IS_EVB-2/IS_EVB-2.cppproj index 285273156..2da7657d1 100644 --- a/EVB-2/IS_EVB-2/IS_EVB-2.cppproj +++ b/EVB-2/IS_EVB-2/IS_EVB-2.cppproj @@ -30,482 +30,482 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + com.atmel.avrdbg.tool.atmelice - J42700051025 + J41800054309 0xA1020C01 SWD @@ -540,7 +540,7 @@ SWD com.atmel.avrdbg.tool.atmelice - J42700051025 + J41800054309 Atmel-ICE diff --git a/EVB-2/IS_EVB-2/src/communications.cpp b/EVB-2/IS_EVB-2/src/communications.cpp index f3e31b6a8..bef39de4a 100644 --- a/EVB-2/IS_EVB-2/src/communications.cpp +++ b/EVB-2/IS_EVB-2/src/communications.cpp @@ -599,7 +599,7 @@ void update_flash_cfg(evb_flash_cfg_t &newCfg) // Enable flash write nvr_flash_config_write_needed(); - nvr_flash_config_write_enable(true); + nvr_flash_config_write_enable(); } @@ -624,6 +624,7 @@ void handle_data_from_host(is_comm_instance_t *comm, protocol_type_t ptype, uint case SYS_CMD_MANF_UNLOCK: // Unlock process for chip erase manfUnlock = true; + g_status.evbStatus |= EVB_STATUS_MANF_UNLOCKED; break; case SYS_CMD_MANF_CHIP_ERASE: // chip erase and reboot - do NOT reset calibration! diff --git a/EVB-2/IS_EVB-2/src/globals.c b/EVB-2/IS_EVB-2/src/globals.c index e631f4e1b..eaeac8d75 100644 --- a/EVB-2/IS_EVB-2/src/globals.c +++ b/EVB-2/IS_EVB-2/src/globals.c @@ -342,7 +342,7 @@ bool nvr_validate_config_integrity(evb_flash_cfg_t* cfg) { // Reset to defaults *cfg = defaults; nvr_flash_config_write_needed(); - nvr_flash_config_write_enable(true); + nvr_flash_config_write_enable(); } // Disable cbPresets is necessary @@ -482,18 +482,10 @@ void nvr_flash_config_write_needed(void) // Used to enabled flash writes at controlled times. The system may identify when a flash write is needed. // However, this will not occur until flash is enabled (at time EKF can tolerate stutters in RTOS update). -void nvr_flash_config_write_enable(bool enable) +void nvr_flash_config_write_enable(void) { - if (enable) - { - g_nvr_manage_config.flash_write_enable_timeMs = time_msec(); - g_status.evbStatus |= EVB_STATUS_FLASH_WRITE_IN_PROGRESS; - } - else - { - g_nvr_manage_config.flash_write_enable_timeMs = 0; - g_status.evbStatus &= ~EVB_STATUS_FLASH_WRITE_IN_PROGRESS; - } + g_nvr_manage_config.flash_write_enable_timeMs = time_msec(); + g_status.evbStatus |= EVB_STATUS_FLASH_WRITE_IN_PROGRESS; } diff --git a/EVB-2/IS_EVB-2/src/globals.h b/EVB-2/IS_EVB-2/src/globals.h index 9621f19ca..0a4e4cf51 100644 --- a/EVB-2/IS_EVB-2/src/globals.h +++ b/EVB-2/IS_EVB-2/src/globals.h @@ -141,7 +141,7 @@ bool nvr_validate_config_integrity(evb_flash_cfg_t* cfg); void nvr_init(void); bool nvr_slow_maintenance(void); void nvr_flash_config_write_needed(void); -void nvr_flash_config_write_enable(bool enable); +void nvr_flash_config_write_enable(void); int error_check_config(evb_flash_cfg_t *cfg); diff --git a/EVB-2/IS_EVB-2/src/misc/init.c b/EVB-2/IS_EVB-2/src/misc/init.c index 3791c34e1..9a842c265 100644 --- a/EVB-2/IS_EVB-2/src/misc/init.c +++ b/EVB-2/IS_EVB-2/src/misc/init.c @@ -653,6 +653,7 @@ void board_init() // Real-time timer time_init(); + time_delay(1); // Delay to ensure time_msec() returns non-zero. /* Initialize IOPORTs */ ioport_init(); diff --git a/EVB-2/IS_EVB-2/src/user_interface.cpp b/EVB-2/IS_EVB-2/src/user_interface.cpp index 9085e8976..aa8fd8176 100644 --- a/EVB-2/IS_EVB-2/src/user_interface.cpp +++ b/EVB-2/IS_EVB-2/src/user_interface.cpp @@ -64,7 +64,7 @@ static void on_cfg_button_release() com_bridge_apply_preset(g_flashCfg); board_IO_config(); nvr_flash_config_write_needed(); - nvr_flash_config_write_enable(true); + nvr_flash_config_write_enable(); evbUiRefreshLedCfg(); } diff --git a/EVB-2/IS_EVB-2/src/version/repositoryInfo.h b/EVB-2/IS_EVB-2/src/version/repositoryInfo.h index 0f3610cc3..29bec631e 100644 --- a/EVB-2/IS_EVB-2/src/version/repositoryInfo.h +++ b/EVB-2/IS_EVB-2/src/version/repositoryInfo.h @@ -1,5 +1,5 @@ -#define REPO_DESCRIPTION "1.9.0" +#define REPO_DESCRIPTION "1.9.1" #define REPO_VERSION_MAJOR 1 #define REPO_VERSION_MINOR 9 -#define REPO_VERSION_REVIS 0 -#define REPO_HEAD_COUNT 1553 +#define REPO_VERSION_REVIS 1 +#define REPO_HEAD_COUNT 1567 diff --git a/EVB-2/Makefile b/EVB-2/Makefile index 6b078ac0f..d1e0d1a1b 100644 --- a/EVB-2/Makefile +++ b/EVB-2/Makefile @@ -50,7 +50,7 @@ rm = rm -f -r $(1) # Detect if a compiler exists. COMPILER_DIR := /opt/arm-none-eabi-uins3/bin ifeq ("$(wildcard $(COMPILER_DIR)/*)","") -# Use the old arm-none-eabi from before we split arm-none-eabi-uins3/arm-none-eabi-imx5 +# Use the old arm-none-eabi from before we split arm-none-eabi-uins3/arm-none-eabi-stm32 COMPILER_DIR = /opt/arm-none-eabi/bin endif ifeq ("$(wildcard $(COMPILER_DIR)/*)","") diff --git a/ExampleProjects/Communications/ISCommunicationsExample.c b/ExampleProjects/Communications/ISCommunicationsExample.c index 9bb9f8925..fe506dcb8 100644 --- a/ExampleProjects/Communications/ISCommunicationsExample.c +++ b/ExampleProjects/Communications/ISCommunicationsExample.c @@ -120,7 +120,7 @@ int enable_message_broadcasting(serial_port_t *serialPort, is_comm_instance_t *c #if 0 // Ask for IMU message at period of 100ms (1ms source period x 100). This could be as high as 1000 times a second (period multiple of 1) - n = is_comm_get_data(comm, _DID_IMU_DUAL, 0, 0, 100); + n = is_comm_get_data(comm, _DID_IMU, 0, 0, 100); if (n != serialPortWrite(serialPort, comm->buf.start, n)) { printf("Failed to encode and write get IMU message\r\n"); diff --git a/docs/html/search/all_7.js b/docs/html/search/all_7.js index 43db1cc7a..47e951bd3 100644 --- a/docs/html/search/all_7.js +++ b/docs/html/search/all_7.js @@ -195,11 +195,11 @@ var searchData= ['gps_5fstatus_5fflags_5frtk_5fbase_5fposition_5fmoving_192',['GPS_STATUS_FLAGS_RTK_BASE_POSITION_MOVING',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a05a4fbe3aacbb13b3a02ac68c1692a1e',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fbaseline_5fbad_193',['GPS_STATUS_FLAGS_RTK_COMPASSING_BASELINE_BAD',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a5f8b49c236ed80c7884980ba7c8ce05f',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fbaseline_5funset_194',['GPS_STATUS_FLAGS_RTK_COMPASSING_BASELINE_UNSET',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a36a1249cf602a274ba79bf2d50917415',1,'data_sets.h']]], - ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fenabled_195',['GPS_STATUS_FLAGS_RTK_COMPASSING_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108aa21c91f8df371780528a30641bfd5d30',1,'data_sets.h']]], + ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fenabled_195',['GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108aa21c91f8df371780528a30641bfd5d30',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fmask_196',['GPS_STATUS_FLAGS_RTK_COMPASSING_MASK',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a8cb9c8ac63095c85ce5859b1600a0eb7',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fvalid_197',['GPS_STATUS_FLAGS_RTK_COMPASSING_VALID',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a488368f442b65121439e88024eac89de',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5ffix_5fand_5fhold_198',['GPS_STATUS_FLAGS_RTK_FIX_AND_HOLD',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108acb54cb050982f7f05eb07fa215410252',1,'data_sets.h']]], - ['gps_5fstatus_5fflags_5frtk_5fposition_5fenabled_199',['GPS_STATUS_FLAGS_RTK_POSITION_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108abbe3d660b35ede6398183c57efcdf286',1,'data_sets.h']]], + ['gps_5fstatus_5fflags_5frtk_5fposition_5fenabled_199',['GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108abbe3d660b35ede6398183c57efcdf286',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fposition_5fvalid_200',['GPS_STATUS_FLAGS_RTK_POSITION_VALID',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108ac6abf3b8112e63dc36b61c15bf176bdc',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fraw_5fgps_5fdata_5ferror_201',['GPS_STATUS_FLAGS_RTK_RAW_GPS_DATA_ERROR',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a44ba3a1dfea45f96820e17930a3b8cea',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5fstatic_5fmode_202',['GPS_STATUS_FLAGS_STATIC_MODE',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a0fb841078ef66c5390f9901f78009a71',1,'data_sets.h']]], diff --git a/docs/html/search/enumvalues_7.js b/docs/html/search/enumvalues_7.js index ddc776a02..f096d3ad5 100644 --- a/docs/html/search/enumvalues_7.js +++ b/docs/html/search/enumvalues_7.js @@ -58,11 +58,11 @@ var searchData= ['gps_5fstatus_5fflags_5frtk_5fbase_5fposition_5fmoving_55',['GPS_STATUS_FLAGS_RTK_BASE_POSITION_MOVING',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a05a4fbe3aacbb13b3a02ac68c1692a1e',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fbaseline_5fbad_56',['GPS_STATUS_FLAGS_RTK_COMPASSING_BASELINE_BAD',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a5f8b49c236ed80c7884980ba7c8ce05f',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fbaseline_5funset_57',['GPS_STATUS_FLAGS_RTK_COMPASSING_BASELINE_UNSET',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a36a1249cf602a274ba79bf2d50917415',1,'data_sets.h']]], - ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fenabled_58',['GPS_STATUS_FLAGS_RTK_COMPASSING_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108aa21c91f8df371780528a30641bfd5d30',1,'data_sets.h']]], + ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fenabled_58',['GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108aa21c91f8df371780528a30641bfd5d30',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fmask_59',['GPS_STATUS_FLAGS_RTK_COMPASSING_MASK',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a8cb9c8ac63095c85ce5859b1600a0eb7',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fcompassing_5fvalid_60',['GPS_STATUS_FLAGS_RTK_COMPASSING_VALID',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a488368f442b65121439e88024eac89de',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5ffix_5fand_5fhold_61',['GPS_STATUS_FLAGS_RTK_FIX_AND_HOLD',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108acb54cb050982f7f05eb07fa215410252',1,'data_sets.h']]], - ['gps_5fstatus_5fflags_5frtk_5fposition_5fenabled_62',['GPS_STATUS_FLAGS_RTK_POSITION_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108abbe3d660b35ede6398183c57efcdf286',1,'data_sets.h']]], + ['gps_5fstatus_5fflags_5frtk_5fposition_5fenabled_62',['GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108abbe3d660b35ede6398183c57efcdf286',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fposition_5fvalid_63',['GPS_STATUS_FLAGS_RTK_POSITION_VALID',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108ac6abf3b8112e63dc36b61c15bf176bdc',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5frtk_5fraw_5fgps_5fdata_5ferror_64',['GPS_STATUS_FLAGS_RTK_RAW_GPS_DATA_ERROR',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a44ba3a1dfea45f96820e17930a3b8cea',1,'data_sets.h']]], ['gps_5fstatus_5fflags_5fstatic_5fmode_65',['GPS_STATUS_FLAGS_STATIC_MODE',['../data__sets_8h.html#ae6d4500b717d1fde25cfaf3caf0c0108a0fb841078ef66c5390f9901f78009a71',1,'data_sets.h']]], diff --git a/python/logInspector/logInspector.py b/python/logInspector/logInspector.py index aea367776..890db7982 100644 --- a/python/logInspector/logInspector.py +++ b/python/logInspector/logInspector.py @@ -271,18 +271,22 @@ def addButton(self, name, function, multithreaded=True, layout=None): def updatePlot(self): self.plot(self.currently_selected, self.plotargs) + self.updateWindowTitle() def updateWindowTitle(self): - size = self.log.numDev - if size != 0: - info = self.log.data[0,DID_DEV_INFO][0] - if size == 1: - infoStr = 'SN' + str(info[1]) + ', H:' + verArrayToString(info[2]) + ', F:' + verArrayToString(info[3]) + ' build ' + str(info[4]) + ', ' + dateTimeArrayToString(info[8], info[9]) + ', ' + info[10].decode('UTF-8') - else: - infoStr = 'Multiple units - ' - for i in range(size): - infoStr = infoStr + str(self.log.serials[i]) + ' ' - self.setWindowTitle("LogInspector - " + infoStr) + try: + size = self.log.numDev + if size != 0: + info = self.log.data[0,DID_DEV_INFO][0] + if size == 1: + infoStr = 'SN' + str(info[1]) + ', H:' + verArrayToString(info[2]) + ', F:' + verArrayToString(info[3]) + ' build ' + str(info[4]) + ', ' + dateTimeArrayToString(info[8], info[9]) + ', ' + info[10].decode('UTF-8') + else: + infoStr = 'Devices: [' + " ".join([str(x) for x in self.log.serials]) + "]" + if self.log.using_mounting_bias: + infoStr += ', Mounting Corrected' + self.setWindowTitle("LogInspector - " + infoStr) + except: + self.setWindowTitle("DID_DEV_INFO missing") def choose_directory(self): log_dir = config['logs_directory'] @@ -315,7 +319,6 @@ def load(self, directory): # str += 'Comp ' # self.statusLabel.setText(str) self.updatePlot() - self.updateWindowTitle() self.stopLoadingIndicator() def setupUi(self): diff --git a/python/logInspector/logInspectorInternal.py b/python/logInspector/logInspectorInternal.py index c08a34cda..8bde0ed7c 100644 --- a/python/logInspector/logInspectorInternal.py +++ b/python/logInspector/logInspectorInternal.py @@ -140,6 +140,7 @@ def RMS(self): self.log.printRMSReport() self.log.openRMSReport() # self.stopLoadingIndicator() + self.updatePlot() def formatButtonColumn(self): super(logInspectorInternal, self).formatButtonColumn() diff --git a/python/logInspector/logPlotter.py b/python/logInspector/logPlotter.py index 4436c79e8..7341523ec 100644 --- a/python/logInspector/logPlotter.py +++ b/python/logInspector/logPlotter.py @@ -12,6 +12,7 @@ import os from os.path import expanduser from inertialsense_math.pose import * +from datetime import date BLACK = r"\u001b[30m" RED = r"\u001b[31m" @@ -65,12 +66,16 @@ def enableResidualPlot(self, enable): def setActiveSerials(self, serials): self.active_devs = [] + self.active_devs_no_ref = [] for d, ser in enumerate(self.log.serials): if ser in serials: self.active_devs.append(d) + if ser != 'Ref INS': + self.active_devs_no_ref.append(d) - def configureSubplot(self, ax, title, xlabel): + def configureSubplot(self, ax, title, ylabel='', xlabel=''): ax.set_title(title) + ax.set_ylabel(ylabel) ax.set_xlabel(xlabel) def saveFig(self, fig, name, sizeInches=[]): @@ -122,10 +127,18 @@ def posNED(self, fig=None): if(np.shape(self.active_devs)[0]==1): timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')) nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS1_POS, 'lla')) - ax[0].plot(timeGPS, nedGps[:, 0], label='GPS') + ax[0].plot(timeGPS, nedGps[:, 0], label='GPS1') ax[1].plot(timeGPS, nedGps[:, 1]) ax[2].plot(timeGPS, nedGps[:, 2]) + if(np.shape(self.active_devs)[0]==1): + timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS2_POS, 'timeOfWeekMs')) + nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS2_POS, 'lla')) + ax[0].plot(timeGPS, nedGps[:, 0], label='GPS2') + ax[1].plot(timeGPS, nedGps[:, 1]) + ax[2].plot(timeGPS, nedGps[:, 2]) + + ax[0].legend(ncol=2) for a in ax: a.grid(True) @@ -166,8 +179,10 @@ def posNEDMap(self, fig=None): self.drawNEDMapArrow(ax, ned, euler[:, 2]) nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS1_POS, 'lla')) - ax.plot(nedGps[:, 1], nedGps[:, 0], label='GPS') + ax.plot(nedGps[:, 1], nedGps[:, 0], label='GPS1') + nedGps = lla2ned(self.getData(d, DID_INS_2, 'lla')[0], self.getData(d, DID_GPS2_POS, 'lla')) + ax.plot(nedGps[:, 1], nedGps[:, 0], label='GPS2') ax.set_aspect('equal', 'datalim') ax.legend(ncol=2) @@ -189,9 +204,14 @@ def posLLA(self, fig=None): if(np.shape(self.active_devs)[0]==1): timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')) - ax[0].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 0], label='GPS') + ax[0].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 0], label='GPS1') ax[1].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 1]) - ax[2].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 2], label='GPS') + ax[2].plot(timeGPS, self.getData(d, DID_GPS1_POS, 'lla')[:, 2], label='GPS1') + + timeGPS = getTimeFromTowMs(self.getData(d, DID_GPS2_POS, 'timeOfWeekMs')) + ax[0].plot(timeGPS, self.getData(d, DID_GPS2_POS, 'lla')[:, 0], label='GPS2') + ax[1].plot(timeGPS, self.getData(d, DID_GPS2_POS, 'lla')[:, 1]) + ax[2].plot(timeGPS, self.getData(d, DID_GPS2_POS, 'lla')[:, 2], label='GPS2') timeBaro = getTimeFromTow(self.getData(d, DID_BAROMETER, 'time')+ self.getData(d, DID_GPS1_POS, 'towOffset')[-1]) ax[2].plot(timeBaro, self.getData(d, DID_BAROMETER, 'mslBar'), label='Baro') @@ -212,9 +232,15 @@ def llaGps(self, fig=None): fig.suptitle('GPS LLA - ' + os.path.basename(os.path.normpath(self.log.directory))) for d in self.active_devs: time = getTimeFromTowMs(self.getData(d, DID_GPS1_POS, 'timeOfWeekMs')) - ax[0].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,0], label=self.log.serials[d]) + ax[0].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,0], label='GPS1') ax[1].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,1]) ax[2].plot(time, self.getData(d, DID_GPS1_POS, 'lla')[:,2]) + + time = getTimeFromTowMs(self.getData(d, DID_GPS2_POS, 'timeOfWeekMs')) + ax[0].plot(time, self.getData(d, DID_GPS2_POS, 'lla')[:,0], label='GPS2') + ax[1].plot(time, self.getData(d, DID_GPS2_POS, 'lla')[:,1]) + ax[2].plot(time, self.getData(d, DID_GPS2_POS, 'lla')[:,2]) + ax[0].legend(ncol=2) for a in ax: a.grid(True) @@ -304,13 +330,28 @@ def velUVW(self, fig=None): self.configureSubplot(ax[0,0], 'Vel U', 'm/s') self.configureSubplot(ax[1,0], 'Vel V', 'm/s') self.configureSubplot(ax[2,0], 'Vel W', 'm/s') - refTime = None refUvw = None if self.residual: self.configureSubplot(ax[0,1], 'Vel U Residual', 'm/s') self.configureSubplot(ax[1,1], 'Vel V Residual', 'm/s') self.configureSubplot(ax[2,1], 'Vel W Residual', 'm/s') + for d in self.active_devs: + if self.log.serials[d] == 'Ref INS': + refTime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')) + refUvw = self.getData(d, DID_INS_2, 'uvw') + continue + # num_devs = len(self.active_devs) + # if refTime is None and num_devs: + # refTime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')) + # refUvw = np.zeros(np.shape(self.getData(d, DID_INS_2, 'uvw'))) + # for d in self.active_devs: + # time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')) + # uvw = self.getData(d, DID_INS_2, 'uvw') + # for i in range(3): + # refUvw[:,i] += np.interp(refTime, time, uvw[:,i], right=np.nan, left=np.nan) + # refUvw *= (1.0/num_devs) + for d in self.active_devs: time = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')) # Adjust data for attitude bias @@ -319,16 +360,10 @@ def velUVW(self, fig=None): ax[1,0].plot(time, uvw[:,1]) ax[2,0].plot(time, uvw[:,2]) - if self.residual: - if self.log.serials[d] == 'Ref INS': - refUvw = uvw - refTime = time - continue - if refTime is None: - continue + if self.residual and not (refTime is None) and self.log.serials[d] != 'Ref INS': intUvw = np.empty_like(refUvw) for i in range(3): - intUvw[:,i] = np.interp(refTime, time, uvw[:,i], right=np.nan) + intUvw[:,i] = np.interp(refTime, time, uvw[:,i], right=np.nan, left=np.nan) resUvw = intUvw - refUvw ax[0,1].plot(refTime, resUvw[:,0], label=self.log.serials[d]) ax[1,1].plot(refTime, resUvw[:,1]) @@ -337,6 +372,8 @@ def velUVW(self, fig=None): ax[0,0].legend(ncol=2) if self.residual: ax[0,1].legend(ncol=2) + for i in range(3): + self.setPlotYSpanMin(ax[i,1], 1.0) for a in ax: for b in a: b.grid(True) @@ -351,14 +388,20 @@ def attitude(self, fig=None): self.configureSubplot(ax[0,0], 'Roll', 'deg') self.configureSubplot(ax[1,0], 'Pitch', 'deg') self.configureSubplot(ax[2,0], 'Yaw', 'deg') + refTime = None + refEuler = None + unwrapRefEuler = None if self.residual: self.configureSubplot(ax[0,1], 'Roll Residual', 'deg') self.configureSubplot(ax[1,1], 'Pitch Residual', 'deg') self.configureSubplot(ax[2,1], 'Yaw Residual', 'deg') + for d in self.active_devs: + if self.log.serials[d] == 'Ref INS': + quat = self.getData(d, DID_INS_2, 'qn2b') + refEuler = quat2euler(quat) + refTime = getTimeFromTow(self.getData(d, DID_INS_2, 'timeOfWeek')) + unwrapRefEuler = self.vec3_unwrap(refEuler) - refTime = None - refEuler = None - unwrapRefEuler = None for d in self.active_devs: # Adjust data for attitude bias quat = mul_ConjQuat_Quat(self.log.mount_bias_quat[d,:], self.getData(d, DID_INS_2, 'qn2b')) @@ -368,18 +411,11 @@ def attitude(self, fig=None): ax[1,0].plot(time, euler[:,1]*RAD2DEG) ax[2,0].plot(time, euler[:,2]*RAD2DEG) - if self.residual: - if self.log.serials[d] == 'Ref INS': - refEuler = euler - unwrapRefEuler = self.vec3_unwrap(refEuler) - refTime = time - continue - if refTime is None: - continue + if self.residual and not (refTime is None) and self.log.serials[d] != 'Ref INS': unwrapEuler = self.vec3_unwrap(euler) intEuler = np.empty_like(refEuler) for i in range(3): - intEuler[:,i] = np.interp(refTime, time, unwrapEuler[:,i], right=np.nan) + intEuler[:,i] = np.interp(refTime, time, unwrapEuler[:,i], right=np.nan, left=np.nan) resEuler = intEuler - unwrapRefEuler ax[0,1].plot(refTime, resEuler[:,0]*RAD2DEG, label=self.log.serials[d]) ax[1,1].plot(refTime, resEuler[:,1]*RAD2DEG) @@ -388,6 +424,8 @@ def attitude(self, fig=None): ax[0,0].legend(ncol=2) if self.residual: ax[0,1].legend(ncol=2) + for i in range(3): + self.setPlotYSpanMin(ax[i,1], 3.0) for a in ax: for b in a: b.grid(True) @@ -847,15 +885,15 @@ def loadIMU(self, device, accelSensor): # 0 = gyro, 1 = accelerometer else: time = self.getData(device, DID_REFERENCE_PIMU, 'time') - if len(time) != 0: # DID_REFERENCE_PIMU - dt = self.getData(d, DID_REFERENCE_PIMU, 'dt') + if time.size: # DID_REFERENCE_PIMU + dt = self.getData(device, DID_REFERENCE_PIMU, 'dt') if accelSensor == 0: # Gyro - refTheta = self.getData(d, DID_REFERENCE_PIMU, 'theta') + refTheta = self.getData(device, DID_REFERENCE_PIMU, 'theta') ref = refTheta / dt[:,None] else: # Accel - refVel = self.getData(d, DID_REFERENCE_PIMU, 'vel') + refVel = self.getData(device, DID_REFERENCE_PIMU, 'vel') ref = refVel / dt[:,None] imu1 = [] for sample in range(0, len(I)): @@ -901,6 +939,16 @@ def loadIMU(self, device, accelSensor): # 0 = gyro, 1 = accelerometer imu3 = np.array(imu3) imuCount = 3 + if self.log.serials[device] != 'Ref INS': + towOffset = self.getData(device, DID_GPS1_POS, 'towOffset') + if towOffset.size: + time = time + np.mean(towOffset) + # else: # HACK: to correct for improper SPAN INS direction and gyro scalar + # tmp = np.copy(imu1) + # tmp *= 125.0 + # imu1[:,0] = tmp[:,1] + # imu1[:,1] = tmp[:,0] + # imu1[:,2] = -tmp[:,2] return (time, dt, imu1, imu2, imu3, imuCount) @@ -920,8 +968,18 @@ def imuPQR(self, fig=None): fig.suptitle('PQR - ' + os.path.basename(os.path.normpath(self.log.directory))) (time, dt, acc0, acc1, acc2, pqrCount) = self.loadGyros(0) + + plotResidual = pqrCount==1 and self.residual if pqrCount: - ax = fig.subplots(3, pqrCount, sharex=True, squeeze=False) + ax = fig.subplots(3, (2 if plotResidual else pqrCount), sharex=True, squeeze=False) + if plotResidual: + for d in self.active_devs: + if self.log.serials[d] == 'Ref INS': + (time, dt, pqr0, pqr1, pqr2, pqrCount) = self.loadGyros(d) + refTime = time + refPqr = pqr0 + continue + for dev_idx, d in enumerate(self.active_devs): (time, dt, pqr0, pqr1, pqr2, pqrCount) = self.loadGyros(d) if pqrCount: @@ -930,6 +988,7 @@ def imuPQR(self, fig=None): for n, pqr in enumerate([ pqr0, pqr1, pqr2 ]): if pqr != [] and n 0 and len(refTime[d]) > 0: # and dev_idx == 0: # Only plot reference IMU for first device - for i in range(3): - if dev_idx == 0: - plabel = 'reference' - else: - plabel = '' - ax[i, 0].plot(refTime[d], refPqr[d][:, i] * 180.0/np.pi, color='black', linestyle = 'dashed', label = plabel) + if plotResidual and not (refTime is None) and self.log.serials[d] != 'Ref INS': + self.configureSubplot(ax[i,1], 'Residual', 'deg/2') + intPqr = np.empty_like(refPqr) + intPqr[:,i] = np.interp(refTime, time, pqr[:,i], right=np.nan, left=np.nan) + resPqr = intPqr - refPqr + ax[i,1].plot(refTime, resPqr[:,i]*RAD2DEG, label=(self.log.serials[d] if dev_idx==0 else None)) + + if not plotResidual: + for dev_idx, d in enumerate(self.active_devs): + if len(refTime) > 0 and len(refTime[d]) > 0: # and dev_idx == 0: # Only plot reference IMU for first device + for i in range(3): + if dev_idx == 0: + plabel = 'reference' + else: + plabel = '' + ax[i, 0].plot(refTime[d], refPqr[d][:, i] * 180.0/np.pi, color='black', linestyle = 'dashed', label = plabel) for i in range(pqrCount): ax[0][i].legend(ncol=2) - for d in range(3): - ax[d][i].grid(True) + if plotResidual: + ax[0,1].legend(ncol=2) + for i in range(3): + self.setPlotYSpanMin(ax[i,1], 1.0) + for a in ax: + for b in a: + b.grid(True) self.saveFig(fig, 'pqrIMU') def imuAcc(self, fig=None): @@ -971,9 +1042,19 @@ def imuAcc(self, fig=None): fig.suptitle('Accelerometer - ' + os.path.basename(os.path.normpath(self.log.directory))) (time, dt, acc0, acc1, acc2, accCount) = self.loadAccels(0) + + plotResidual = accCount==1 and self.residual if accCount: - ax = fig.subplots(3, accCount, sharex=True, squeeze=False) - for d in self.active_devs: + ax = fig.subplots(3, (2 if plotResidual else accCount), sharex=True, squeeze=False) + if plotResidual: + for d in self.active_devs: + if self.log.serials[d] == 'Ref INS': + (time, dt, acc0, acc1, acc2, accCount) = self.loadAccels(d) + refTime = time + refAcc = acc0 + continue + + for dev_idx, d in enumerate(self.active_devs): (time, dt, acc0, acc1, acc2, accCount) = self.loadAccels(d) if accCount: for i in range(3): @@ -988,22 +1069,34 @@ def imuAcc(self, fig=None): alable += '%d ' % n else: alable += ' ' - self.configureSubplot(ax[i, n], alable + axislable + ' (m/s^2), mean: %.4g, std: %.3g' % (mean, std), 'sec') + self.configureSubplot(ax[i, n], alable + axislable + ' (m/s^2), mean: %.4g, std: %.3g' % (mean, std), 'm/s^2') ax[i, n].plot(time, acc[:, i], label=self.log.serials[d]) - - for dev_idx, d in enumerate(self.active_devs): - if len(refTime) > 0 and len(refTime[d]) > 0: # and dev_idx == 0: # Only plot reference IMU for first device - for i in range(3): - if dev_idx == 0: - plabel = 'reference' - else: - plabel = '' - ax[i, 0].plot(refTime[d], refAcc[d][:, i], color='black', linestyle = 'dashed', label = plabel) + if plotResidual and not (refTime is None) and self.log.serials[d] != 'Ref INS': + self.configureSubplot(ax[i,1], 'Residual', 'm/s^2') + intAcc = np.empty_like(refAcc) + intAcc[:,i] = np.interp(refTime, time, acc[:,i], right=np.nan, left=np.nan) + resAcc = intAcc - refAcc + ax[i,1].plot(refTime, resAcc[:,i], label=(self.log.serials[d] if dev_idx==0 else None)) + + if not plotResidual: + for dev_idx, d in enumerate(self.active_devs): + if len(refTime) > 0 and len(refTime[d]) > 0: # and dev_idx == 0: # Only plot reference IMU for first device + for i in range(3): + if dev_idx == 0: + plabel = 'reference' + else: + plabel = '' + ax[i, 0].plot(refTime[d], refAcc[d][:, i], color='black', linestyle = 'dashed', label = plabel) for i in range(accCount): ax[0][i].legend(ncol=2) - for d in range(3): - ax[d][i].grid(True) + if plotResidual: + ax[0,1].legend(ncol=2) + for i in range(3): + self.setPlotYSpanMin(ax[i,1], 1.0) + for a in ax: + for b in a: + b.grid(True) self.saveFig(fig, 'accIMU') def allanVariancePQR(self, fig=None): @@ -1075,7 +1168,7 @@ def allanVariancePQR(self, fig=None): alable += '%d ' % n else: alable += ' ' - self.configureSubplot(ax[i, n], alable + axislable + ' ($deg/hr$), ARW: %.3g $deg/\sqrt{hr}$, BI: %.3g $deg/hr$' % (np.mean(sumARW[i][n]) + np.std(sumARW[i][n]), np.mean(sumBI[i][n]) + np.std(sumBI[i][n])), 'sec') + self.configureSubplot(ax[i, n], alable + axislable + ' ($deg/hr$), ARW: %.3g $deg/\sqrt{hr}$, BI: %.3g $deg/hr$' % (np.mean(sumARW[i][n]) + np.std(sumARW[i][n]), np.mean(sumBI[i][n]) + np.std(sumBI[i][n])), 'deg/hr') for i in range(pqrCount): for d in range(3): @@ -1083,6 +1176,22 @@ def allanVariancePQR(self, fig=None): ax[d][i].legend(ncol=2) self.saveFig(fig, 'pqrIMU') + with open(self.log.directory + '/allan_variance_pqr.csv', 'w') as f: + f.write('Hardware,Date,SN,BI-P,BI-Q,BI-R,ARW-P,ARW-Q,ARW-R,BI-X\n') + f.write(',,,(deg/hr),(deg/hr),(deg/hr),(deg / rt hr),(deg / rt hr),(deg / rt hr)\n') + today = date.today() + for d in self.active_devs: + hdwVer = self.getData(d, DID_DEV_INFO, 'hardwareVer')[d] + f.write('%d.%d.%d,%s,%d,' % (hdwVer[0], hdwVer[1], hdwVer[2], str(today), self.log.serials[d])) + for n, pqr in enumerate([ pqr0, pqr1, pqr2 ]): + if pqr != [] and n 0: @@ -1414,18 +1543,26 @@ def deltatime(self, fig=None): deltaTimestamp = 0 timeImu = 0 - if len(self.getData(d, DID_PIMU, 'time')): - deltaTimestamp = self.getData(d, DID_PIMU, 'time')[1:] - self.getData(d, DID_PIMU, 'time')[0:-1] + timePimu = self.getData(d, DID_PIMU, 'time') + timeIMU = self.getData(d, DID_IMU, 'time') + timeImu3 = self.getData(d, DID_IMU3_RAW, 'time') + if timePimu.size: + deltaTimestamp = timePimu[1:] - timePimu[0:-1] + deltaTimestamp = deltaTimestamp / self.d + timeImu = getTimeFromTow(timePimu[1:] + towOffset) + elif timeIMU.size: + deltaTimestamp = timeIMU[1:] - timeIMU[0:-1] deltaTimestamp = deltaTimestamp / self.d - timeImu = getTimeFromTow(self.getData(d, DID_PIMU, 'time')[1:] + towOffset) - elif len(self.getData(d, DID_IMU3_RAW, 'time')): - deltaTimestamp = self.getData(d, DID_IMU3_RAW, 'time')[1:] - self.getData(d, DID_IMU3_RAW, 'time')[0:-1] + timeImu = getTimeFromTow(timeIMU[1:] + towOffset) + elif timeImu3.size: + deltaTimestamp = timeImu3[1:] - timeImu3[0:-1] deltaTimestamp = deltaTimestamp / self.d - timeImu = getTimeFromTow(self.getData(d, DID_IMU3_RAW, 'time')[1:] + towOffset) + timeImu = getTimeFromTow(timeImu3[1:] + towOffset) ax[0].plot(timeIns, dtIns, label=self.log.serials[d]) ax[1].plot(timeGps, dtGps) - ax[2].plot(timeImu, integrationPeriod) + if integrationPeriod.size: + ax[2].plot(timeImu, integrationPeriod) ax[3].plot(timeImu, deltaTimestamp) self.setPlotYSpanMin(ax[0], 0.005) @@ -1659,7 +1796,7 @@ def rtkDebug2(self, fig=None): max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1] for r in range(0,6): for c in range(0,4): - self.configureSubplot(ax[r,c], '', '') + self.configureSubplot(ax[r,c]) fig.suptitle('RTK Debug2 - ' + os.path.basename(os.path.normpath(self.log.directory))) for d in self.active_devs: @@ -1689,7 +1826,7 @@ def rtkDebug2Sat(self, fig=None): max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1] for r in range(0,6): for c in range(0,4): - self.configureSubplot(ax[r,c], '', '') + self.configureSubplot(ax[r,c]) fig.suptitle('RTK Debug2 - Sat# - ' + os.path.basename(os.path.normpath(self.log.directory))) for d in self.active_devs: @@ -1719,7 +1856,7 @@ def rtkDebug2Std(self, fig=None): max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1] for r in range(0,6): for c in range(0,4): - self.configureSubplot(ax[r,c], '', '') + self.configureSubplot(ax[r,c]) fig.suptitle('RTK Debug 2 - Sat Bias Std - ' + os.path.basename(os.path.normpath(self.log.directory))) for d in self.active_devs: @@ -1749,7 +1886,7 @@ def rtkDebug2Lock(self, fig=None): max_num_biases = self.getData(0, DID_RTK_DEBUG_2, 'num_biases')[-1] for r in range(0,6): for c in range(0,4): - self.configureSubplot(ax[r,c], '', '') + self.configureSubplot(ax[r,c]) fig.suptitle('RTK Debug 2 - Lock Count - ' + os.path.basename(os.path.normpath(self.log.directory))) for d in self.active_devs: diff --git a/python/logInspector/logReader.py b/python/logInspector/logReader.py index c1c65148c..ab1cc0940 100644 --- a/python/logInspector/logReader.py +++ b/python/logInspector/logReader.py @@ -7,7 +7,7 @@ import yaml import datetime -from os.path import expanduser +from os.path import expanduser, exists from scipy.interpolate import interp1d file_path = os.path.dirname(os.path.realpath(__file__)) @@ -26,9 +26,16 @@ RED = '\u001b[31m' RESET = '\u001b[0m' +INS_STATUS_NAV_MODE = 0x00001000 +GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED = 0x00100000 +GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED = 0x00400000 + class Log: def __init__(self): self.c_log = LogReader() + self.init_vars() + + def init_vars(self): self.data = [] self.serials = [] self.passRMS = 0 # 1 = pass, -1 = fail, 0 = unknown @@ -39,17 +46,20 @@ def __init__(self): self.truth = [] self.refINS = False self.hardware = [] + self.numRef = 0 + self.refINS = False + self.using_mounting_bias = False def load(self, directory, serials=['ALL']): - self.data = [] + self.init_vars() self.c_log.init(self, directory, serials) self.c_log.load() self.serials = self.c_log.getSerialNumbers() self.sanitize() self.data = np.array(self.data, dtype=object) self.directory = directory + self.mount_bias_filepath = directory + '/angular_mount_bias.yml' self.numDev = self.data.shape[0] - self.numRef = 0 if self.numDev == 0: print("No devices found in log or no logs found!!!") @@ -58,7 +68,10 @@ def load(self, directory, serials=['ALL']): self.serials = [self.data[d, DID_DEV_INFO]['serialNumber'][0] for d in range(self.numDev)] for i in range(self.numDev): - self.hardware.append(self.data[i, DID_DEV_INFO]['hardwareVer'][0][0]) + try: + self.hardware.append(self.data[i, DID_DEV_INFO]['hardwareVer'][0][0]) + except: + self.hardware.append(0) if any(self.data[i,DID_FLASH_CONFIG]['sysCfgBits'] & eSysConfigBits.SYS_CFG_USE_REFERENCE_IMU_IN_EKF.value): # Use this INS as reference self.refINS = True @@ -81,9 +94,9 @@ def load(self, directory, serials=['ALL']): self.refSerials.append(self.data[i, DID_DEV_INFO]['serialNumber'][0]) if len(self.data[0, DID_INS_2]) == 0 and len(self.data[0, DID_INS_1]) != 0: self.ins1ToIns2(i) - - self.mount_bias_euler = np.zeros([self.numDev, 3], dtype=float) - self.mount_bias_quat = euler2quat(self.mount_bias_euler) + #If you want to view data of log with only refIns: + if len(self.serials) == 1 and self.refINS == True: + return True if len(self.serials) == len(self.refSerials): self.devIdx = self.refIdx @@ -98,20 +111,30 @@ def load(self, directory, serials=['ALL']): if(len(self.serials) > len(self.refSerials)): self.numIns = self.numIns - self.numRef + self.mount_bias_euler = np.zeros([self.numDev, 3], dtype=float) + if exists(self.mount_bias_filepath): + with open(self.mount_bias_filepath, 'r') as file: + mount_bias = yaml.safe_load(file) + for n, dev in enumerate(self.serials): + if (n < self.numIns) and (int(dev) in mount_bias): + self.mount_bias_euler[n, :] = np.array(mount_bias[int(dev)]) + self.using_mounting_bias = True + self.mount_bias_quat = euler2quat(self.mount_bias_euler) self.compassing = None self.rtk = None self.navMode = None - - if len(self.data[0, DID_DEV_INFO]): - self.compassing = 'Cmp' in str(self.data[0, DID_DEV_INFO]['addInfo'][-1]) - self.rtk = 'Rov' in str(self.data[0, DID_DEV_INFO]['addInfo'][-1]) + ins2 = self.data[0, DID_INS_2] + if len(ins2): + self.navMode = (ins2['insStatus'][-1] & INS_STATUS_NAV_MODE) == INS_STATUS_NAV_MODE + gps1Pos = self.data[0, DID_GPS1_POS] + if len(gps1Pos): + self.rtk = (gps1Pos['status'][-1] & GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED) == GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED + self.compassing = (gps1Pos['status'][-1] & GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED) == GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED # Reference INS like Novatel may not have all status fields. Find a first device that is not reference uINS_device_idx = [n for n in range(self.numDev) if n in self.devIdx and not (n in self.refIdx)] idx = uINS_device_idx[0] - if len(self.data[idx, DID_INS_2]): - self.navMode = (self.data[idx, DID_INS_2]['insStatus'][-1] & 0x1000) == 0x1000 # except: # print(RED + "error loading log" + sys.exc_info()[0] + RESET) return True @@ -240,11 +263,12 @@ def getRMSArray(self): self.min_time = max(time_of_fix_ms) # Use only partial data for RMS calculations - self.min_time = self.max_time - 2.0*(self.max_time - self.min_time)/3.0 # do not use the first 1/3 (alignment) - # self.max_time = self.max_time - (self.max_time - self.min_time)/3.0 # do not use the last 1/3 + self.min_time = self.max_time - (self.max_time - self.min_time)*(2.0/3.0) # do not use the first 1/3 (alignment) + # self.min_time = self.max_time - (self.max_time - self.min_time)*(1.0/2.0) # do not use the first 1/2 (alignment) - # Resample at a steady 100 Hz - dt = 0.01 + # Resample at a steady 10 Hz + dt = 0.1 + # dt = np.average(data[0][1:,0] - data[0][:-1,0]) t = np.arange(1.0, self.max_time - self.min_time - 1.0, dt) for i in range(self.numDev): # Chop off extra data at beginning and end @@ -255,7 +279,7 @@ def getRMSArray(self): data[i][:, 0] -= self.min_time # Interpolate data so that it has all the same timestamps - fi = interp1d(data[i][:, 0], data[i][:, 1:].T, kind='cubic', fill_value='extrapolate', + fi = interp1d(data[i][:, 0], data[i][:, 1:].T, kind='linear', fill_value='extrapolate', bounds_error=False) data[i] = np.hstack((t[:, None], fi(t).T)) @@ -294,9 +318,11 @@ def calculateRMS(self): self.getRMSTruth() self.calcAttitudeError() + # Calculate the Mounting Bias for all devices (assume the mounting bias is the mean of the attitude error) uINS_device_idx = [n for n in range(self.numDev) if n in self.devIdx and not (n in self.refIdx)] self.uvw_error = np.empty_like(self.stateArray[:, :, 4:7]) + mount_bias_output = dict() for n, dev in enumerate(uINS_device_idx): mount_bias = np.mean(self.att_error[n, :, :], axis=0) if self.compassing: @@ -305,20 +331,24 @@ def calculateRMS(self): mount_bias[2] = 0 self.mount_bias_euler[dev, :] = mount_bias # quat2eulerArray(qexp(mount_bias)) self.mount_bias_quat[dev,:] = euler2quat(self.mount_bias_euler[dev, :]) + self.using_mounting_bias = True self.att_error[n, :, :] = self.att_error[n, :, :] - mount_bias[None, :] self.uvw_error[n, :, :] = quatRot(self.mount_bias_quat[dev,:], self.stateArray[n, :, 4:7]) - self.truth[:,3:6] + mount_bias_output[int(self.serials[dev])] = mount_bias.tolist() + # Writing mounting bias to file + with open(self.mount_bias_filepath, 'w') as file: + yaml.dump(mount_bias_output, file) # RMS = sqrt ( 1/N sum(e^2) ) - self.RMS = np.empty((len(self.stateArray), 9)) - self.RMS[:,:3] = np.sqrt(np.mean(np.square(self.stateArray[:, :, 1:4] - self.truth[:,0:3]), axis=1)) # [ pos ] - self.RMS[:,3:6] = np.sqrt(np.mean(np.square(self.uvw_error), axis=1)) # [ vel } - self.RMS[:,6:] = np.sqrt(np.mean(np.square(self.att_error), axis=1)) # [ att } - self.RMS_euler = self.RMS[:, 6:] # quat2eulerArray(qexp(RMS[:,6:])) + self.RMSNED = np.sqrt(np.mean(np.square(self.stateArray[:, :, 1:4] - self.truth[:,0:3]), axis=1)) # [ pos ] + self.RMSUVW = np.sqrt(np.mean(np.square(self.uvw_error), axis=1)) # [ vel } + self.RMSAtt = np.sqrt(np.mean(np.square(self.att_error), axis=1)) # [ att } # Average RMS across devices - self.averageRMS = np.mean(self.RMS, axis=0) - self.averageRMS_euler = self.averageRMS[6:] # quat2eulerArray(qexp(averageRMS[None,6:]))[0] + self.averageRMSNED = np.mean(self.RMSNED, axis=0) + self.averageRMSUVW = np.mean(self.RMSUVW, axis=0) + self.averageRMSAtt = np.mean(self.RMSAtt, axis=0) def pass_fail(self, ratio): if ratio > 1.0: @@ -339,130 +369,135 @@ def printRMSReport(self): # Use default value if not all devices use the same hardware hardware = 0 - # Default thresholds - thresholds = np.array([0.35, 0.35, 0.8, # (m) NED - 0.2, 0.2, 0.2, # (m/s) UVW - 0.11, 0.11, 2.0]) # (deg) ATT (roll, pitch, yaw) - if self.navMode or self.compassing: - thresholds[8] = 0.3 # Higher heading accuracy - else: - thresholds[:6] = np.inf - - if self.compassing: - thresholds[0] = 1.0 - thresholds[1] = 1.0 - thresholds[2] = 1.0 - # Thresholds for uINS-3 - if hardware == 3: - thresholds = np.array([0.35, 0.35, 0.8, # (m) NED - 0.2, 0.2, 0.2, # (m/s) UVW - 0.11, 0.11, 2.0]) # (deg) ATT (roll, pitch, yaw) - if self.navMode or self.compassing: - thresholds[8] = 0.3 # Higher heading accuracy - else: - thresholds[:6] = np.inf + # Nav + thresholdNED = np.array([0.35, 0.35, 0.8]) # (m) NED + thresholdUVW = np.array([0.04, 0.04, 0.07]) # (m/s) UVW + thresholdAtt = np.array([0.11, 0.11, 0.3]) # (deg) Att (roll, pitch, yaw) + if not self.navMode: + # AHRS + thresholdAtt[2] = 2.0 # (deg) Att (yaw) - if self.compassing: - thresholds[0] = 1.0 - thresholds[1] = 1.0 - thresholds[2] = 1.0 # Thresholds for IMX-5 - elif hardware == 5: - thresholds = np.array([0.35, 0.35, 0.8, # (m) NED - 0.2, 0.2, 0.2, # (m/s) UVW - 0.06, 0.06, 1.0]) # (deg) ATT (roll, pitch, yaw) - if self.navMode or self.compassing: - thresholds[8] = 0.2 # Higher heading accuracy - else: - thresholds[:6] = np.inf + if hardware == 5: + # Nav + thresholdNED = np.array([0.35, 0.35, 0.8]) # (m) NED + thresholdUVW = np.array([0.035, 0.035, 0.07]) # (m/s) UVW + thresholdAtt = np.array([0.045, 0.045, 0.16]) # (deg) Att (roll, pitch, yaw) + if not self.navMode: + # AHRS + thresholdAtt[:2] = 0.28 # (deg) Att (roll, pitch) + thresholdAtt[2] = 0.5 # (deg) Att (yaw) - if self.compassing: - thresholds[0] = 1.0 - thresholds[1] = 1.0 - thresholds[2] = 1.0 + if self.compassing: + thresholdNED[:2] = 0.5 + + if self.refINS: # SPAN INS has position offset + thresholdNED[:2] = 2.5 + thresholdNED[2] = 6.0 + elif self.rtk: # RTK positioning w/o ref INS + thresholdNED[:2] = 0.04 + thresholdNED[2] = 0.05 - thresholds[6:] *= DEG2RAD # convert degrees threshold to radians + if not (self.navMode or self.compassing): + thresholdNED[:] = np.inf # Disable NED + thresholdUVW[:] = np.inf # Disable UVW - self.specRatio = self.averageRMS / thresholds + thresholdAtt[:] *= DEG2RAD # convert degrees threshold to radians + + self.specRatioNED = self.averageRMSNED / thresholdNED + self.specRatioUVW = self.averageRMSUVW / thresholdUVW + self.specRatioAtt = self.averageRMSAtt / thresholdAtt f = open(filename, 'w') f.write('***** Performance Analysis Report - %s *****\n' % (self.directory)) f.write('\n') - f.write('Directory: %s\n' % (self.directory)) - mode = "AHRS" - if self.navMode: mode = "NAV" - if self.compassing: mode = "DUAL GNSS" - if self.refINS: mode += " With Reference IMU Data" - f.write("\n") + mode = ('IMX-5' if hardware == 5 else 'uINS-3' ) + mode += (", NAV" if self.navMode else ", AHRS") + if self.rtk: mode += ", RTK" + if self.compassing: mode += ", DUAL GNSS" + if self.refINS: mode += ", Ref INS" # Print Table of RMS accuracies - line = 'Device ' + line = 'Device ' if self.navMode: f.write( - '--------------------------------------------------- RMS Accuracy -------------------------------------------\n') - line = line + 'UVW[ (m/s) (m/s) (m/s) ], NED[ (m) (m) (m) ],' + '------------------------------------------------- RMS Accuracy -------------------------------------------\n') else: # AHRS mode - f.write('-------------- RMS Accuracy --------------\n') - line = line + ' Att [ (deg) (deg) (deg) ]\n' + f.write( + '-------------- RMS Accuracy --------------\n') + line += ' Att[ (deg) (deg) (deg) ]' + if self.navMode: + line += ', UVW[ (m/s) (m/s) (m/s) ], NED[ (m) (m) (m) ]' + line += '\n' f.write(line) for n, dev in enumerate(uINS_device_idx): devInfo = self.data[dev,DID_DEV_INFO][0] line = '%2d SN%d ' % (n, devInfo['serialNumber']) + line += '[ %6.4f %6.4f %6.4f ]' % ( + self.RMSAtt[n, 0] * RAD2DEG, self.RMSAtt[n, 1] * RAD2DEG, self.RMSAtt[n, 2] * RAD2DEG) if self.navMode: - line = line + '[ %6.4f %6.4f %6.4f ], ' % (self.RMS[n, 3], self.RMS[n, 4], self.RMS[n, 5]) - line = line + '[ %6.4f %6.4f %6.4f ], ' % (self.RMS[n, 0], self.RMS[n, 1], self.RMS[n, 2]) - line = line + '[ %6.4f %6.4f %6.4f ]\n' % ( - self.RMS_euler[n, 0] * RAD2DEG, self.RMS_euler[n, 1] * RAD2DEG, self.RMS_euler[n, 2] * RAD2DEG) + line += ', [ %6.4f %6.4f %6.4f ]' % (self.RMSUVW[n, 0], self.RMSUVW[n, 1], self.RMSUVW[n, 2]) + line += ', [ %6.4f %6.4f %6.4f ]' % (self.RMSNED[n, 0], self.RMSNED[n, 1], self.RMSNED[n, 2]) + line += '\n' f.write(line) - line = 'AVERAGE: ' if self.navMode: f.write( - '------------------------------------------------------------------------------------------------------------\n') - line = line + '[%7.4f %7.4f %7.4f ], ' % (self.averageRMS[3], self.averageRMS[4], self.averageRMS[5]) - line = line + '[%7.4f %7.4f %7.4f ], ' % (self.averageRMS[0], self.averageRMS[1], self.averageRMS[2]) + '----------------------------------------------------------------------------------------------------------\n') else: # AHRS mode - f.write('------------------------------------------\n') - line = line + '[%7.4f %7.4f %7.4f ]\n' % ( - self.averageRMS_euler[0] * RAD2DEG, self.averageRMS_euler[1] * RAD2DEG, self.averageRMS_euler[2] * RAD2DEG) + f.write( + '------------------------------------------\n') + line = 'AVERAGE: ' + line += '[%7.4f %7.4f %7.4f ]' % ( + self.averageRMSAtt[0] * RAD2DEG, self.averageRMSAtt[1] * RAD2DEG, self.averageRMSAtt[2] * RAD2DEG) + if self.navMode: + line += ', [%7.4f %7.4f %7.4f ]' % (self.averageRMSUVW[0], self.averageRMSUVW[1], self.averageRMSUVW[2]) + line += ', [%7.4f %7.4f %7.4f ]' % (self.averageRMSNED[0], self.averageRMSNED[1], self.averageRMSNED[2]) + line += '\n' f.write(line) line = 'THRESHOLD: ' + line += '[%7.4f %7.4f %7.4f ]' % ( + thresholdAtt[0] * RAD2DEG, thresholdAtt[1] * RAD2DEG, thresholdAtt[2] * RAD2DEG) if self.navMode: - line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[3], thresholds[4], thresholds[5]) - line = line + '[%7.4f %7.4f %7.4f ], ' % (thresholds[0], thresholds[1], thresholds[2]) - line = line + '[%7.4f %7.4f %7.4f ]\n' % ( - thresholds[6] * RAD2DEG, thresholds[7] * RAD2DEG, thresholds[8] * RAD2DEG) + line += ', [%7.4f %7.4f %7.4f ]' % (thresholdUVW[0], thresholdUVW[1], thresholdUVW[2]) + line += ', [%7.4f %7.4f %7.4f ]' % (thresholdNED[0], thresholdNED[1], thresholdNED[2]) + line += '\n' f.write(line) - line = 'RATIO: ' if self.navMode: f.write( - '------------------------------------------------------------------------------------------------------------\n') - line = line + '[%7.4f %7.4f %7.4f ], ' % (self.specRatio[3], self.specRatio[4], self.specRatio[5]) - line = line + '[%7.4f %7.4f %7.4f ], ' % (self.specRatio[0], self.specRatio[1], self.specRatio[2]) + '----------------------------------------------------------------------------------------------------------\n') else: # AHRS mode f.write('------------------------------------------\n') - line = line + '[%7.4f %7.4f %7.4f ]\n' % (self.specRatio[6], self.specRatio[7], self.specRatio[8]) + line = 'RATIO: ' + line += '[%7.4f %7.4f %7.4f ]' % (self.specRatioAtt[0], self.specRatioAtt[1], self.specRatioAtt[2]) + if self.navMode: + line += ', [%7.4f %7.4f %7.4f ]' % (self.specRatioUVW[0], self.specRatioUVW[1], self.specRatioUVW[2]) + line += ', [%7.4f %7.4f %7.4f ]' % (self.specRatioNED[0], self.specRatioNED[1], self.specRatioNED[2]) + line += '\n' f.write(line) line = 'PASS/FAIL: ' + line += '[ %s %s %s ]' % ( + self.pass_fail(self.specRatioAtt[0]), + self.pass_fail(self.specRatioAtt[1]), + self.pass_fail(self.specRatioAtt[2])) # ATT if self.navMode: - line = line + '[ %s %s %s ], ' % ( - self.pass_fail(self.specRatio[3]), self.pass_fail(self.specRatio[4]), self.pass_fail(self.specRatio[5])) # LLA - line = line + '[ %s %s %s ], ' % ( - self.pass_fail(self.specRatio[0]), self.pass_fail(self.specRatio[1]), self.pass_fail(self.specRatio[2])) # UVW - line = line + '[ %s %s %s ]\n' % ( - self.pass_fail(self.specRatio[6]), self.pass_fail(self.specRatio[7]), self.pass_fail(self.specRatio[8])) # ATT + line += ', [ %s %s %s ]' % ( + self.pass_fail(self.specRatioUVW[0]), + self.pass_fail(self.specRatioUVW[1]), + self.pass_fail(self.specRatioUVW[2])) # UVW + line += ', [ %s %s %s ]' % ( + self.pass_fail(self.specRatioNED[0]), + self.pass_fail(self.specRatioNED[1]), + self.pass_fail(self.specRatioNED[2])) # NED + line += '\n' f.write(line) - if self.navMode: - f.write(' ') - else: # AHRS mode - f.write(' ') - f.write('(' + mode + ')\n\n') + f.write('MODE: (' + mode + ')\n\n') # Print Mounting Biases f.write('--------------- Angular Mounting Biases ----------------\n') @@ -470,7 +505,9 @@ def printRMSReport(self): for dev in uINS_device_idx: devInfo = self.data[dev, DID_DEV_INFO][0] f.write('%2d SN%d [ %7.4f %7.4f %7.4f ]\n' % ( - n, devInfo['serialNumber'], self.mount_bias_euler[dev, 0] * RAD2DEG, self.mount_bias_euler[dev, 1] * RAD2DEG, + n, devInfo['serialNumber'], + self.mount_bias_euler[dev, 0] * RAD2DEG, + self.mount_bias_euler[dev, 1] * RAD2DEG, self.mount_bias_euler[dev, 2] * RAD2DEG)) f.write('\n') diff --git a/python/pylib/ISToolsDataSorted.py b/python/pylib/ISToolsDataSorted.py index 4c91567bb..8f19414e7 100644 --- a/python/pylib/ISToolsDataSorted.py +++ b/python/pylib/ISToolsDataSorted.py @@ -395,7 +395,7 @@ def __init__(self, index, directory, serialNumber, refIns=None): ('gps2AntOffset', (f32, 3)), ('zeroVelRotation', (f32, 3)), ('zeroVelOffset', (f32, 3)), - ('magInclination', f32), + ('gpsTimeUserDelay', f32), ('magDeclination', f32), ('gpsTimeSyncPulsePeriodMs', u32), ('startupGPSDtMs', u32), diff --git a/src/DataKML.cpp b/src/DataKML.cpp index 0f7e63d44..c6073913f 100644 --- a/src/DataKML.cpp +++ b/src/DataKML.cpp @@ -67,6 +67,7 @@ int cDataKML::WriteDataToFile(std::vector& list, const p_data_hdr_t uDatasets& d = (uDatasets&)(*dataBuf); ixEuler theta; sKmlLogData data; + bool deadreckoning = false; #ifdef USE_IS_INTERNAL // uInternalDatasets &i = (uInternalDatasets&)(*dataBuf); @@ -79,15 +80,18 @@ int cDataKML::WriteDataToFile(std::vector& list, const p_data_hdr_t return 0; case DID_INS_1: - data = sKmlLogData(d.ins1.timeOfWeek, d.ins1.lla, d.ins1.theta, !(d.ins1.insStatus & INS_STATUS_GPS_AIDING_POS)); + deadreckoning = !(d.ins1.insStatus & INS_STATUS_GPS_AIDING_POS); + data = sKmlLogData(d.ins1.timeOfWeek, d.ins1.lla, d.ins1.theta, deadreckoning); break; case DID_INS_2: quat2euler(d.ins2.qn2b, theta); - data = sKmlLogData(d.ins2.timeOfWeek, d.ins2.lla, theta, !(d.ins2.insStatus & INS_STATUS_GPS_AIDING_POS)); + deadreckoning = !(d.ins2.insStatus & INS_STATUS_GPS_AIDING_POS); + data = sKmlLogData(d.ins2.timeOfWeek, d.ins2.lla, theta, deadreckoning); break; case DID_INS_3: quat2euler(d.ins3.qn2b, theta); - data = sKmlLogData(d.ins3.timeOfWeek, d.ins3.lla, theta, !(d.ins3.insStatus & INS_STATUS_GPS_AIDING_POS)); + deadreckoning = !(d.ins3.insStatus & INS_STATUS_GPS_AIDING_POS); + data = sKmlLogData(d.ins3.timeOfWeek, d.ins3.lla, theta, deadreckoning); break; case DID_GPS1_POS: case DID_GPS1_UBX_POS: diff --git a/src/DataKML.h b/src/DataKML.h index 92b602f89..663586391 100644 --- a/src/DataKML.h +++ b/src/DataKML.h @@ -32,7 +32,7 @@ struct sKmlLogData float theta[3]; bool deadReckoning; sKmlLogData() {} - sKmlLogData(double _time, double _lla[3], float _theta[3], bool _deadReckoning) + sKmlLogData(double _time, double _lla[3], float _theta[3], bool _deadReckoning=false) { time = _time; lla[0] = _lla[0]; @@ -51,7 +51,7 @@ struct sKmlLogData lla[1] = _lla[1]; lla[2] = _lla[2]; theta[0] = theta[1] = theta[2] = 0; - deadReckoning = 0; + deadReckoning = false; } }; @@ -75,7 +75,8 @@ class cDataKML switch (did) { default: return -1; // Unused - case DID_INS_1: return KID_INS; + case DID_INS_1: + case DID_INS_2: return KID_INS; case DID_GPS1_POS: return KID_GPS; case DID_GPS1_UBX_POS: return KID_GPS1; case DID_GPS2_POS: return KID_GPS2; @@ -90,6 +91,7 @@ class cDataKML default: return -1; // Unused case KID_INS: + case KID_REF: return 130; case KID_GPS: case KID_GPS1: diff --git a/src/DeviceLog.cpp b/src/DeviceLog.cpp index 81e51a0c0..eea1a6da1 100644 --- a/src/DeviceLog.cpp +++ b/src/DeviceLog.cpp @@ -39,6 +39,7 @@ cDeviceLog::cDeviceLog() m_fileCount = 0; memset(&m_devInfo, 0, sizeof(dev_info_t)); m_altClampToGround = true; + m_enableGpsLogging = true; m_showTracks = true; m_showPointTimestamps = true; m_pointUpdatePeriodSec = 1.0f; diff --git a/src/DeviceLog.h b/src/DeviceLog.h index 12bb495dc..b77f9caa9 100644 --- a/src/DeviceLog.h +++ b/src/DeviceLog.h @@ -51,8 +51,10 @@ class cDeviceLog uint64_t FileSize() { return m_fileSize; } uint64_t LogSize() { return m_logSize; } uint32_t FileCount() { return m_fileCount; } - void SetKmlConfig(bool showTracks =true, bool showPoints =true, bool showPointTimestamps =true, double pointUpdatePeriodSec=1.0, bool altClampToGround=true) + std::string GetNewFileName(uint32_t serialNumber, uint32_t fileCount, const char* suffix); + void SetKmlConfig(bool gpsData =true, bool showTracks =true, bool showPoints =true, bool showPointTimestamps =true, double pointUpdatePeriodSec=1.0, bool altClampToGround=true) { + m_enableGpsLogging = gpsData; m_showTracks = showTracks; m_showPoints = showPoints; m_showPointTimestamps = showPointTimestamps; @@ -63,7 +65,6 @@ class cDeviceLog protected: bool OpenNewSaveFile(); bool OpenNextReadFile(); - std::string GetNewFileName(uint32_t serialNumber, uint32_t fileCount, const char* suffix); void OnReadData(p_data_t* data); std::vector m_fileNames; @@ -80,6 +81,7 @@ class cDeviceLog uint64_t m_maxDiskSpace; uint32_t m_maxFileSize; bool m_altClampToGround; + bool m_enableGpsLogging; bool m_showTracks; bool m_showPoints; bool m_showPointTimestamps; diff --git a/src/DeviceLogKML.cpp b/src/DeviceLogKML.cpp index 04bb335cd..b7fa9fe7d 100644 --- a/src/DeviceLogKML.cpp +++ b/src/DeviceLogKML.cpp @@ -30,7 +30,15 @@ using namespace std; void cDeviceLogKML::InitDeviceForWriting(int pHandle, std::string timestamp, std::string directory, uint64_t maxDiskSpace, uint32_t maxFileSize) { - memset(&m_Log, 0, sizeof(m_Log)); + for (int kid=0; kidid) { + case DID_DEV_INFO: + if (d.devInfo.serialNumber == 99999 || + d.devInfo.serialNumber == 10101) + { + m_isRefIns = true; + } + break; + +#if 0 // This code is not needed as cDeviceLogKML::WriteDateToFile() saves both DID_INS_1 and DID_INS_2 case DID_INS_2: ins_1_t ins1; ins1.week = d.ins2.week; @@ -450,6 +490,7 @@ bool cDeviceLogKML::SaveData(p_data_hdr_t *dataHdr, const uint8_t *dataBuf) return false; } break; +#endif } return true; @@ -466,6 +507,19 @@ bool cDeviceLogKML::WriteDateToFile(const p_data_hdr_t *dataHdr, const uint8_t* return true; } + switch (kid) + { + case cDataKML::KID_GPS: + case cDataKML::KID_GPS1: + case cDataKML::KID_GPS2: + case cDataKML::KID_RTK: + if (!m_enableGpsLogging) + { + return true; + } + break; + } + // Reference current log sKmlLog &log = m_Log[kid]; diff --git a/src/DeviceLogKML.h b/src/DeviceLogKML.h index ca4ca18bb..3d4a53083 100644 --- a/src/DeviceLogKML.h +++ b/src/DeviceLogKML.h @@ -58,6 +58,7 @@ class cDeviceLogKML : public cDeviceLog cDataKML m_kml; sKmlLog m_Log[cDataKML::MAX_NUM_KID]; + bool m_isRefIns; }; #endif // DEVICE_LOG_KML_H diff --git a/src/ISBootloaderAPP.cpp b/src/ISBootloaderAPP.cpp index 63612d5a7..0cb648e05 100644 --- a/src/ISBootloaderAPP.cpp +++ b/src/ISBootloaderAPP.cpp @@ -95,7 +95,9 @@ eImageSignature cISBootloaderAPP::check_is_compatible() case DID_EVB_DEV_INFO: evb_dev_info = (dev_info_t*)comm.dataPtr; if (evb_dev_info->hardwareVer[0] == 2) - { /** EVB-2 */ + { /** EVB-2 - all firmwares are valid except for STM32 bootloader (no VCP support) */ + valid_signatures |= IS_IMAGE_SIGN_UINS_5; + valid_signatures |= IS_IMAGE_SIGN_UINS_3_16K | IS_IMAGE_SIGN_UINS_3_24K; valid_signatures |= IS_IMAGE_SIGN_EVB_2_16K | IS_IMAGE_SIGN_EVB_2_24K; valid_signatures |= IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K; } @@ -124,7 +126,7 @@ is_operation_result cISBootloaderAPP::reboot_down(uint8_t major, char minor, boo (void)minor; (void)major; - m_info_callback(this, "(APP) Rebooting down into ISB mode...", IS_LOG_LEVEL_INFO); + m_info_callback(this, "(APP) Rebooting to ISB mode...", IS_LOG_LEVEL_INFO); // In case we are in program mode, try and send the commands to go into bootloader mode uint8_t c = 0; diff --git a/src/ISBootloaderBase.cpp b/src/ISBootloaderBase.cpp index 8e8110efb..12310b907 100644 --- a/src/ISBootloaderBase.cpp +++ b/src/ISBootloaderBase.cpp @@ -211,6 +211,7 @@ is_operation_result cISBootloaderBase::mode_device_app strncpy((obj)->m_app.enable_command, "EBLE", 5); (obj)->reboot_down(); delete obj; + SLEEP_MS(3000); // Delay 3 seconds to avoid port being re-used return IS_OP_CLOSED; } else if ((device & IS_IMAGE_SIGN_APP) & fw_IMX_5) @@ -219,6 +220,7 @@ is_operation_result cISBootloaderBase::mode_device_app strncpy((obj)->m_app.enable_command, "BLEN", 5); (obj)->reboot_down(); delete obj; + SLEEP_MS(3000); return IS_OP_CLOSED; } else if ((device & IS_IMAGE_SIGN_APP) & fw_uINS_3) @@ -227,24 +229,17 @@ is_operation_result cISBootloaderBase::mode_device_app strncpy((obj)->m_app.enable_command, "BLEN", 5); (obj)->reboot_down(); delete obj; + SLEEP_MS(3000); return IS_OP_CLOSED; } - else - { - strncpy((obj)->m_app.enable_command, "BLEN", 5); - (obj)->reboot_down(); - delete obj; - return IS_OP_CLOSED; - } - - delete obj; - return IS_OP_CANCELLED; - } - else - { - delete obj; } + char msg[120] = { 0 }; + SNPRINTF(msg, sizeof(msg), " | (%s) Incompatible device.", handle->port); + statusfn(NULL, msg, IS_LOG_LEVEL_ERROR); + + delete obj; + SLEEP_MS(3000); return IS_OP_OK; } @@ -314,29 +309,6 @@ is_operation_result cISBootloaderBase::get_device_isb_version( delete obj; return IS_OP_CLOSED; } - - if((obj)->isb_mightUpdate) - { - if(major != 0 && minor != 0) - { - if(major < (obj)->m_isb_major) - { - (obj)->isb_mightUpdate = false; - } - else if(major == (obj)->m_isb_major) - { - if(minor < (obj)->m_isb_minor) - { - (obj)->isb_mightUpdate = false; - } - else if(minor == (obj)->m_isb_minor) - { - (obj)->isb_mightUpdate = false; - } - - } - } - } } else { @@ -369,9 +341,9 @@ is_operation_result cISBootloaderBase::mode_device_isb char minor; uint32_t device = IS_IMAGE_SIGN_NONE; - uint32_t fw_uINS_3 = get_image_signature(filenames.fw_uINS_3.path) & (IS_IMAGE_SIGN_UINS_3_16K | IS_IMAGE_SIGN_UINS_3_24K); - //uint32_t bl_uINS_3 = get_image_signature(filenames.bl_uINS_3.path, &major, &minor) & (IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K); - uint32_t fw_IMX_5 = get_image_signature(filenames.fw_IMX_5.path) & IS_IMAGE_SIGN_UINS_5; + //uint32_t fw_uINS_3 = get_image_signature(filenames.fw_uINS_3.path) & (IS_IMAGE_SIGN_UINS_3_16K | IS_IMAGE_SIGN_UINS_3_24K); + uint32_t bl_uINS_3 = get_image_signature(filenames.bl_uINS_3.path, &major, &minor) & (IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K); + //uint32_t fw_IMX_5 = get_image_signature(filenames.fw_IMX_5.path) & IS_IMAGE_SIGN_UINS_5; uint32_t bl_IMX_5 = get_image_signature(filenames.bl_IMX_5.path, &major, &minor) & IS_IMAGE_SIGN_ISB_STM32L4; //uint32_t fw_EVB_2 = get_image_signature(filenames.fw_EVB_2.path) & (IS_IMAGE_SIGN_EVB_2_16K | IS_IMAGE_SIGN_EVB_2_24K); uint32_t bl_EVB_2 = get_image_signature(filenames.bl_EVB_2.path, &major, &minor) & (IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K); @@ -385,7 +357,7 @@ is_operation_result cISBootloaderBase::mode_device_isb } else if(device) { // Firmware for a device must be specified to update its bootloader - if ((device & IS_IMAGE_SIGN_ISB) & bl_EVB_2) + if ((device & IS_IMAGE_SIGN_ISB) & bl_EVB_2) // & ((device & IS_IMAGE_SIGN_APP) & fw_EVB_2)) { (obj)->m_filename = filenames.bl_EVB_2.path; is_operation_result op = (obj)->reboot_down(major, minor, force); @@ -401,7 +373,7 @@ is_operation_result cISBootloaderBase::mode_device_isb return IS_OP_CLOSED; } } - else if (((device & IS_IMAGE_SIGN_ISB) & bl_IMX_5) && ((device & IS_IMAGE_SIGN_APP) & fw_IMX_5)) + else if ((device & IS_IMAGE_SIGN_ISB) & bl_IMX_5) // && ((device & IS_IMAGE_SIGN_APP) & fw_IMX_5)) { (obj)->m_filename = filenames.bl_IMX_5.path; is_operation_result op = (obj)->reboot_down(major, minor, force); @@ -417,7 +389,7 @@ is_operation_result cISBootloaderBase::mode_device_isb return IS_OP_CLOSED; } } - else if ((device & IS_IMAGE_SIGN_APP) & fw_uINS_3) + else if ((device & IS_IMAGE_SIGN_ISB) & bl_uINS_3) // && ((device & IS_IMAGE_SIGN_APP) & fw_uINS_3)) { (obj)->m_filename = filenames.bl_uINS_3.path; is_operation_result op = (obj)->reboot_down(major, minor, force); @@ -433,6 +405,12 @@ is_operation_result cISBootloaderBase::mode_device_isb return IS_OP_CLOSED; } } + else if (bl_uINS_3 | bl_IMX_5 | bl_EVB_2) + { + (obj)->m_info_callback(obj, "(ISB) Bootloader upgrade not supported on this port. Trying APP update...", IS_LOG_LEVEL_INFO); + delete obj; + return IS_OP_CLOSED; + } else { delete obj; @@ -504,7 +482,8 @@ is_operation_result cISBootloaderBase::update_device pfnBootloadProgress verifyProgress, std::vector& contexts, std::mutex* addMutex, - cISBootloaderBase** new_context + cISBootloaderBase** new_context, + uint32_t baud ) { cISBootloaderBase* obj; @@ -521,7 +500,7 @@ is_operation_result cISBootloaderBase::update_device { obj = new cISBootloaderSAMBA(updateProgress, verifyProgress, statusfn, handle); obj->m_port_name = std::string(handle->port); - device = (obj)->check_is_compatible(); + device = obj->check_is_compatible(); if (device) { if((device & IS_IMAGE_SIGN_SAMBA) & bl_EVB_2) @@ -574,7 +553,7 @@ is_operation_result cISBootloaderBase::update_device } else { - statusfn(NULL, " | (SAMBA) Firmware image incompatible with SAMBA device", IS_LOG_LEVEL_ERROR); + statusfn(NULL, " | (SAM-BA) Firmware image incompatible with SAM-BA device", IS_LOG_LEVEL_ERROR); delete obj; return IS_OP_CANCELLED; } @@ -585,10 +564,28 @@ is_operation_result cISBootloaderBase::update_device } } + char* name = handle->port; + serialPortClose(handle); + if (!serialPortOpenRetry(handle, name, baud, 1)) + { + char msg[120] = { 0 }; + SNPRINTF(msg, sizeof(msg), " | (%s) Unable to open port at %d baud", handle->port, baud); + statusfn(NULL, msg, IS_LOG_LEVEL_ERROR); + return IS_OP_ERROR; + } + obj = new cISBootloaderISB(updateProgress, verifyProgress, statusfn, handle); (obj)->m_port_name = std::string(handle->port); device = (obj)->check_is_compatible(); - if(device == IS_IMAGE_SIGN_ERROR) + if (device == IS_IMAGE_SIGN_NONE) + { + delete obj; + char msg[120] = { 0 }; + SNPRINTF(msg, sizeof(msg), " | (%s) Device response missing.", handle->port); + statusfn(NULL, msg, IS_LOG_LEVEL_ERROR); + return IS_OP_ERROR; + } + else if(device == IS_IMAGE_SIGN_ERROR) { delete obj; } @@ -679,8 +676,8 @@ is_operation_result cISBootloaderBase::update_device delete obj; } - char msg[100] = {0}; - SNPRINTF(msg, 100, " | (%s) Incompatible device selected", handle->port); + char msg[120] = {0}; + SNPRINTF(msg, sizeof(msg), " | (%s) Incompatible device selected", handle->port); statusfn(NULL, msg, IS_LOG_LEVEL_ERROR); return IS_OP_ERROR; } diff --git a/src/ISBootloaderBase.h b/src/ISBootloaderBase.h index 68056f861..c71ef969d 100644 --- a/src/ISBootloaderBase.h +++ b/src/ISBootloaderBase.h @@ -263,7 +263,8 @@ class cISBootloaderBase pfnBootloadProgress verifyProgress, std::vector& contexts, std::mutex* addMutex, - cISBootloaderBase** new_context + cISBootloaderBase** new_context, + uint32_t baud = BAUDRATE_921600 ); static is_operation_result update_device( firmwares_t filenames, diff --git a/src/ISBootloaderDFU.cpp b/src/ISBootloaderDFU.cpp index 4547ce42d..17938ca63 100644 --- a/src/ISBootloaderDFU.cpp +++ b/src/ISBootloaderDFU.cpp @@ -148,7 +148,7 @@ is_operation_result cISBootloaderDFU::list_devices(is_dfu_list* list) // Open the device ret_libusb = libusb_open(dev, &dev_handle); if (ret_libusb < LIBUSB_SUCCESS) continue; - + // Add to list std::string uidstr; get_serial_number_libusb(&dev_handle, list->id[list->present].sn, uidstr, desc.iSerialNumber); @@ -191,19 +191,19 @@ is_operation_result cISBootloaderDFU::get_serial_number_libusb(libusb_device_han SLEEP_MS(100); // Reset the device - ret_libusb = libusb_reset_device(*handle); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } +// ret_libusb = libusb_reset_device(*handle); +// if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } ret_libusb = libusb_claim_interface(*handle, 0); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } // Cancel any existing operations ret_libusb = dfu_ABORT(handle); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } // Reset status to good ret_dfu = dfu_wait_for_state(handle, DFU_STATE_IDLE); - if (ret_dfu < DFU_ERROR_NONE) { return IS_OP_ERROR; } + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } // Get the string containing the serial number from the device unsigned char uid[IS_DFU_UID_MAX_SIZE]; @@ -218,20 +218,20 @@ is_operation_result cISBootloaderDFU::get_serial_number_libusb(libusb_device_han // Set the address pointer (command is 0x21) uint8_t txBuf[] = { 0x21, 0x00, 0x70, 0xFF, 0x1F }; ret_libusb = dfu_DNLOAD(handle, 0, txBuf, sizeof(txBuf)); - if(ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if(ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } // Address pointer takes effect after GETSTATUS command ret_libusb = dfu_GETSTATUS(handle, &status, &waitTime, &state, &stringIdx); - if(ret_libusb < LIBUSB_SUCCESS || status != DFU_STATUS_OK || state != DFU_STATE_DNBUSY) return IS_OP_ERROR; + if(ret_libusb < LIBUSB_SUCCESS || status != DFU_STATUS_OK || state != DFU_STATE_DNBUSY) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } ret_libusb = dfu_GETSTATUS(handle, &status, &waitTime, &state, &stringIdx); - if(ret_libusb < LIBUSB_SUCCESS || status != DFU_STATUS_OK) return IS_OP_ERROR; + if(ret_libusb < LIBUSB_SUCCESS || status != DFU_STATUS_OK) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } // Get out of download mode dfu_ABORT(handle); // Read the full OTP page ret_libusb = dfu_UPLOAD(handle, 2, rxBuf, 1024); - if(ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if(ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(*handle, 0); return IS_OP_ERROR; } } int index = 0; @@ -244,7 +244,7 @@ is_operation_result cISBootloaderDFU::get_serial_number_libusb(libusb_device_han while(memcmp(cmp, otp_mem, OTP_SECTION_SIZE) != 0) { otp_mem += OTP_SECTION_SIZE; index++; - if(index >= OTP_NUM_SECTIONS) + if(index >= (int)OTP_NUM_SECTIONS) { foundSn = false; break; // No more room in OTP } @@ -263,6 +263,7 @@ is_operation_result cISBootloaderDFU::get_serial_number_libusb(libusb_device_han return IS_OP_OK; } + libusb_release_interface(*handle, 0); return IS_OP_ERROR; } @@ -282,28 +283,28 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) size_t image_sections; // Reset the device - ret_libusb = libusb_reset_device(m_dfu.handle_libusb); - if(ret_libusb < LIBUSB_SUCCESS) - { - return IS_OP_ERROR; - } +// ret_libusb = libusb_reset_device(m_dfu.handle_libusb); +// if(ret_libusb < LIBUSB_SUCCESS) +// { +// return IS_OP_ERROR; +// } SLEEP_MS(100); ret_libusb = libusb_claim_interface(m_dfu.handle_libusb, 0); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Cancel any existing operations ret_libusb = dfu_ABORT(&m_dfu.handle_libusb); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Reset status to good ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_IDLE); - if (ret_dfu < DFU_ERROR_NONE) { return IS_OP_ERROR; } + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Load the firmware image image_sections = ihex_load_sections(filename.c_str(), image, MAX_NUM_IHEX_SECTIONS); - if(image_sections <= 0) { return IS_OP_ERROR; } + if(image_sections <= 0) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } int image_total_len = 0; for(size_t i = 0; i < image_sections; i++) @@ -347,18 +348,12 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) memcpy(&eraseCommand[1], &pageAddress, 4); ret_libusb = dfu_DNLOAD(&m_dfu.handle_libusb, 0, eraseCommand, 5); - // if (ret_libusb < LIBUSB_SUCCESS) - // { - // ihex_unload_sections(image, image_sections); - // return IS_OP_ERROR; - // } + if (ret_libusb < LIBUSB_SUCCESS) + { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_DNLOAD_IDLE); - // if (ret_dfu < DFU_ERROR_NONE) - // { - // ihex_unload_sections(image, image_sections); - // return IS_OP_ERROR; - // } + if (ret_dfu < DFU_ERROR_NONE) + { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } byteInSection += STM32_PAGE_SIZE; bytes_written_total += STM32_PAGE_SIZE; @@ -381,6 +376,7 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) if (ret_dfu < DFU_ERROR_NONE) { ihex_unload_sections(image, image_sections); + libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } @@ -405,6 +401,7 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) if (ret_libusb < LIBUSB_SUCCESS) { ihex_unload_sections(image, image_sections); + libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } @@ -412,6 +409,7 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) if (ret_dfu < DFU_ERROR_NONE) { ihex_unload_sections(image, image_sections); + libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } @@ -428,11 +426,11 @@ is_operation_result cISBootloaderDFU::download_image(std::string filename) // Cancel any existing operations ret_libusb = dfu_ABORT(&m_dfu.handle_libusb); - if (ret_libusb < LIBUSB_SUCCESS) { return IS_OP_ERROR; } + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Reset status to good ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_IDLE); - if (ret_dfu < DFU_ERROR_NONE) { return IS_OP_ERROR; } + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } libusb_release_interface(m_dfu.handle_libusb, 0); @@ -444,7 +442,7 @@ is_operation_result cISBootloaderDFU::reboot_up() int ret_libusb; dfu_error ret_dfu; - m_info_callback(this, "(DFU) Rebooting up into ISB mode...", IS_LOG_LEVEL_INFO); + m_info_callback(this, "(DFU) Rebooting to ISB mode...", IS_LOG_LEVEL_INFO); // Option bytes // This hard-coded array sets mostly defaults, but without PH3 enabled and @@ -457,20 +455,19 @@ is_operation_result cISBootloaderDFU::reboot_up() 0xff,0xff,0x00,0xff, 0x00,0x00,0xff,0x00 }; + ret_libusb = libusb_claim_interface(m_dfu.handle_libusb, 0); + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } + // Cancel any existing operations ret_libusb = dfu_ABORT(&m_dfu.handle_libusb); - if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Reset status to good ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_IDLE); - if (ret_dfu < DFU_ERROR_NONE) return IS_OP_ERROR; - - // Select the alt setting for option bytes -// ret_libusb = libusb_set_interface_alt_setting(m_dfu.handle_libusb, 0, STM32_DFU_INTERFACE_OPTIONS); -// if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } ret_dfu = dfu_set_address_pointer(&m_dfu.handle_libusb, 0x1FFF7800); - if (ret_dfu < DFU_ERROR_NONE) return IS_OP_ERROR; + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } ret_libusb = dfu_DNLOAD(&m_dfu.handle_libusb, 2, bytes, sizeof(bytes)); dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_DNLOAD_IDLE); @@ -478,6 +475,8 @@ is_operation_result cISBootloaderDFU::reboot_up() // ret_libusb = libusb_reset_device(dev_handle); // if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + libusb_release_interface(m_dfu.handle_libusb, 0); + return IS_OP_OK; } @@ -486,29 +485,34 @@ is_operation_result cISBootloaderDFU::reboot() int ret_libusb; dfu_error ret_dfu; + ret_libusb = libusb_claim_interface(m_dfu.handle_libusb, 0); + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } + // Cancel any existing operations ret_libusb = dfu_ABORT(&m_dfu.handle_libusb); - if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Reset status to good ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_IDLE); - if (ret_dfu < DFU_ERROR_NONE) return IS_OP_ERROR; + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Set address pointer to flash ret_dfu = dfu_set_address_pointer(&m_dfu.handle_libusb, 0x08000000); - if (ret_dfu < DFU_ERROR_NONE) return IS_OP_ERROR; + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Request DFU leave ret_libusb = dfu_DNLOAD(&m_dfu.handle_libusb, 0, NULL, 0); - if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Execute DFU leave ret_dfu = dfu_wait_for_state(&m_dfu.handle_libusb, DFU_STATE_MANIFEST); - if (ret_dfu < DFU_ERROR_NONE) return IS_OP_ERROR; + if (ret_dfu < DFU_ERROR_NONE) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } // Reset USB device ret_libusb = libusb_reset_device(m_dfu.handle_libusb); - if (ret_libusb < LIBUSB_SUCCESS) return IS_OP_ERROR; + if (ret_libusb < LIBUSB_SUCCESS) { libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_ERROR; } + + libusb_release_interface(m_dfu.handle_libusb, 0); return IS_OP_OK; } @@ -596,7 +600,7 @@ cISBootloaderDFU::dfu_error cISBootloaderDFU::dfu_wait_for_state(libusb_device_h if (ret_libusb < LIBUSB_SUCCESS) return DFU_ERROR_LIBUSB; } - SLEEP_MS(_MAX(waitTime, 10)); // TODO: Windows + SLEEP_MS(_MAX(waitTime, 10)); ret_libusb = dfu_GETSTATUS(dev_handle, &status, &waitTime, &state, &stringIndex); if (ret_libusb < LIBUSB_SUCCESS) diff --git a/src/ISBootloaderISB.cpp b/src/ISBootloaderISB.cpp index 8e60b2f95..975489136 100644 --- a/src/ISBootloaderISB.cpp +++ b/src/ISBootloaderISB.cpp @@ -36,7 +36,7 @@ std::mutex cISBootloaderISB::rst_serial_list_mutex; /** uINS bootloader baud rate */ #define IS_BAUD_RATE_BOOTLOADER 921600 -#define BOOTLOADER_RETRIES 10 +#define BOOTLOADER_RETRIES 100 #define BOOTLOADER_RESPONSE_DELAY 10 #define BOOTLOADER_REFRESH_DELAY 500 #define MAX_VERIFY_CHUNK_SIZE 1024 @@ -61,9 +61,12 @@ is_operation_result cISBootloaderISB::match_test(void* param) eImageSignature cISBootloaderISB::check_is_compatible() { serialPortFlush(m_port); - // if( - sync(m_port); - // != IS_OP_OK) + //if ( + sync(m_port); + //!= IS_OP_OK) + //{ + //return IS_IMAGE_SIGN_ERROR; + //} // { // for(int i = 0; i < 10; i++) // { @@ -81,25 +84,35 @@ eImageSignature cISBootloaderISB::check_is_compatible() SLEEP_MS(100); - // Send command - serialPortFlush(m_port); - serialPortWrite(m_port, (uint8_t*)":020000041000EA", 15); - uint8_t buf[14] = { 0 }; + int count = 0; - // Read Version, SAM-BA Available, serial number (in version 6+) and ok (.\r\n) response - int count = serialPortReadTimeout(m_port, buf, 14, 100); + for (int retry=0;; retry++) + { + // Send command + serialPortFlush(m_port); + serialPortWrite(m_port, (uint8_t*)":020000041000EA", 15); - uint32_t valid_signatures = 0; + // Read Version, SAM-BA Available, serial number (in version 6+) and ok (.\r\n) response +#define READ_DELAY_MS 500 + count = serialPortReadTimeout(m_port, buf, 14, READ_DELAY_MS); - if (count < 8 || buf[0] != 0xAA || buf[1] != 0x55) - { // Bad read - return IS_IMAGE_SIGN_NONE; + if (count >= 8 && buf[0] == 0xAA && buf[1] == 0x55) + { + break; + } + + if (retry*READ_DELAY_MS > 4000) + { // No response + return IS_IMAGE_SIGN_NONE; + } } + uint32_t valid_signatures = 0; + m_isb_major = buf[2]; m_isb_minor = (char)buf[3]; -// bool rom_available = buf[4]; + bool rom_available = buf[4]; uint8_t processor = 0xFF; m_isb_props.is_evb = false; m_sn = 0; @@ -116,18 +129,18 @@ eImageSignature cISBootloaderISB::check_is_compatible() if(processor == IS_PROCESSOR_SAMx70) { valid_signatures |= m_isb_props.is_evb ? IS_IMAGE_SIGN_EVB_2_24K : IS_IMAGE_SIGN_UINS_3_24K; - valid_signatures |= IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K; + if (rom_available) valid_signatures |= IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K; } else if(processor == IS_PROCESSOR_STM32L4) { valid_signatures |= IS_IMAGE_SIGN_UINS_5; - valid_signatures |= IS_IMAGE_SIGN_ISB_STM32L4; + if (rom_available) valid_signatures |= IS_IMAGE_SIGN_ISB_STM32L4; } } else { valid_signatures |= IS_IMAGE_SIGN_EVB_2_16K | IS_IMAGE_SIGN_UINS_3_16K; - valid_signatures |= IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K; + if (rom_available) valid_signatures |= IS_IMAGE_SIGN_ISB_SAMx70_16K | IS_IMAGE_SIGN_ISB_SAMx70_24K; } return (eImageSignature)valid_signatures; @@ -135,10 +148,11 @@ eImageSignature cISBootloaderISB::check_is_compatible() is_operation_result cISBootloaderISB::reboot_up() { - m_info_callback(this, "(ISB) Rebooting up into APP mode...", IS_LOG_LEVEL_INFO); + m_info_callback(this, "(ISB) Rebooting to APP mode...", IS_LOG_LEVEL_INFO); // send the "reboot to program mode" command and the device should start in program mode serialPortWrite(m_port, (unsigned char*)":020000040300F7", 15); + SLEEP_MS(1000); serialPortClose(m_port); return IS_OP_OK; } @@ -146,46 +160,27 @@ is_operation_result cISBootloaderISB::reboot_up() is_operation_result cISBootloaderISB::reboot_down(uint8_t major, char minor, bool force) { char message[100] = {0}; + int n = SNPRINTF(message, 100, "(ISB) Bootloader version: file %c%c, device %c%c. ", major + '0', (minor ? minor : '0'), m_isb_major + '0', m_isb_minor); if(!force) { - if(major != 0 && minor != 0) + if(major == 0 || minor == 0) { - if(major < m_isb_major) - { - SNPRINTF(message, 100, "(ISB) Not updating bootloader: file %c%c, device %c%c", major + '0', minor, m_isb_major + '0', m_isb_minor); - m_info_callback(this, message, IS_LOG_LEVEL_INFO); - return IS_OP_OK; - } - else if(major == m_isb_major) - { - if(minor < m_isb_minor) - { - SNPRINTF(message, 100, "(ISB) Not updating bootloader: file %c%c, device %c%c", major + '0', minor, m_isb_major + '0', m_isb_minor); - m_info_callback(this, message, IS_LOG_LEVEL_INFO); - return IS_OP_OK; - } - else if(minor == m_isb_minor) - { - SNPRINTF(message, 100, "(ISB) Not updating bootloader: file %c%c, device %c%c", major + '0', minor, m_isb_major + '0', m_isb_minor); - m_info_callback(this, message, IS_LOG_LEVEL_INFO); - return IS_OP_OK; - } - - } + return IS_OP_ERROR; } - else + + if(major < m_isb_major || + (major == m_isb_major && minor <= m_isb_minor)) { - return IS_OP_ERROR; + SNPRINTF(message+n, sizeof(message)-n, "No update."); + m_info_callback(this, message, IS_LOG_LEVEL_INFO); + return IS_OP_OK; } } - else - { - SNPRINTF(message, 100, "(ISB) Updating bootloader: file %c%c >> device %c%c", major + '0', minor, m_isb_major + '0', m_isb_minor); - m_info_callback(this, message, IS_LOG_LEVEL_INFO); - } - m_info_callback(this, "(ISB) Rebooting down into DFU/SAMBA mode...", IS_LOG_LEVEL_INFO); + SNPRINTF(message+n, sizeof(message)-n, "Update needed..."); + m_info_callback(this, message, IS_LOG_LEVEL_INFO); + m_info_callback(this, "(ISB) Rebooting to DFU/SAM-BA mode...", IS_LOG_LEVEL_INFO); // USE WITH CAUTION! This will put in bootloader ROM mode allowing a new bootloader to be put on // In some cases, the device may become unrecoverable because of interference on its ports. @@ -193,6 +188,8 @@ is_operation_result cISBootloaderISB::reboot_down(uint8_t major, char minor, boo // restart bootloader assist command serialPortWrite(m_port, (unsigned char*)":020000040700F3", 15); + serialPortSleep(m_port, 500); + return IS_OP_OK; } @@ -302,6 +299,7 @@ is_operation_result cISBootloaderISB::sync(serial_port_t* s) { static const uint8_t handshakerChar = 'U'; + // Bootloader sync requires at least 6 'U' characters to be sent every 10ms. // write a 'U' to handshake with the boot loader - once we get a 'U' back we are ready to go for (int i = 0; i < BOOTLOADER_RETRIES; i++) { diff --git a/src/ISBootloaderSAMBA.cpp b/src/ISBootloaderSAMBA.cpp index 92cec2cb1..492fa3395 100644 --- a/src/ISBootloaderSAMBA.cpp +++ b/src/ISBootloaderSAMBA.cpp @@ -31,7 +31,6 @@ using namespace ISBootloader; // Resources for SAM-BA protocol: // - Datasheet ROM boot section -// - https://github.com/atmelcorp/sam-ba/tree/master/src/plugins/connection/serial // - https://sourceforge.net/p/lejos/wiki-nxt/SAM-BA%20Protocol/ #define UART_XMODEM_SOH 0x01 @@ -106,13 +105,12 @@ is_operation_result cISBootloaderSAMBA::reboot() { // RSTC_CR, RSTC_CR_KEY_PASSWD | RSTC_CR_PROCRST write_word(0x400e1800, 0xa5000001); - serialPortClose(m_port); return IS_OP_OK; } is_operation_result cISBootloaderSAMBA::reboot_up() { - m_info_callback(this, "(SAMBA) Rebooting up into ISB mode...", IS_LOG_LEVEL_INFO); + m_info_callback(this, "(SAM-BA) Rebooting to ISB mode...", IS_LOG_LEVEL_INFO); // EEFC.FCR, EEFC_FCR_FKEY_PASSWD | EEFC_FCR_FARG_BOOT | EEFC_FCR_FCMD_SGPB if (write_word(0x400e0c04, 0x5a00010b) == IS_OP_OK) @@ -121,6 +119,8 @@ is_operation_result cISBootloaderSAMBA::reboot_up() reboot(); + serialPortSleep(m_port, 500); + return IS_OP_OK; } return IS_OP_ERROR; @@ -134,7 +134,7 @@ is_operation_result cISBootloaderSAMBA::download_image(std::string filename) // https://sourceforge.net/p/lejos/wiki-nxt/SAM-BA%20Protocol/ uint8_t buf[SAMBA_PAGE_SIZE]; - SAMBA_ERROR_CHECK(erase_flash(), "(SAMBA) Failed to erase flash memory"); + SAMBA_ERROR_CHECK(erase_flash(), "(SAM-BA) Failed to erase flash memory"); // try non-USB and then USB mode (0 and 1) for(int isUSB = 0; isUSB < 2; isUSB++) @@ -155,7 +155,7 @@ is_operation_result cISBootloaderSAMBA::download_image(std::string filename) if (file == 0) { - SAMBA_STATUS("(SAMBA) Unable to load bootloader file", IS_LOG_LEVEL_ERROR); + SAMBA_STATUS("(SAM-BA) Unable to load bootloader file", IS_LOG_LEVEL_ERROR); // serialPortClose(port); return IS_OP_ERROR; } @@ -167,12 +167,12 @@ is_operation_result cISBootloaderSAMBA::download_image(std::string filename) if (size != SAMBA_BOOTLOADER_SIZE_24K) { - SAMBA_STATUS("(SAMBA) Invalid or old (v5 or earlier) bootloader file", IS_LOG_LEVEL_ERROR); + SAMBA_STATUS("(SAM-BA) Invalid or old (v5 or earlier) bootloader file", IS_LOG_LEVEL_ERROR); // serialPortClose(port); return IS_OP_ERROR; } - if(isUSB == 0) SAMBA_STATUS("(SAMBA) Writing ISB bootloader...", IS_LOG_LEVEL_INFO); + if(isUSB == 0) SAMBA_STATUS("(SAM-BA) Writing ISB bootloader...", IS_LOG_LEVEL_INFO); uint32_t offset = 0; size_t len; @@ -206,7 +206,7 @@ is_operation_result cISBootloaderSAMBA::download_image(std::string filename) is_operation_result cISBootloaderSAMBA::erase_flash() { - SAMBA_STATUS("(SAMBA) Erasing flash memory...", IS_LOG_LEVEL_INFO); + SAMBA_STATUS("(SAM-BA) Erasing flash memory...", IS_LOG_LEVEL_INFO); // Erase 3 sectors of 16 pares each (8K) if (write_word(0x400e0c04, 0x5a000207) == IS_OP_OK) @@ -301,7 +301,7 @@ is_operation_result cISBootloaderSAMBA::wait_eefc_ready(bool waitReady) is_operation_result cISBootloaderSAMBA::write_uart_modem(uint8_t* buf, size_t len) { int ret; - uint8_t eot = UART_XMODEM_EOT; // "X" comes from xModem, not sure where that came from + uint8_t eot = UART_XMODEM_EOT; uint8_t answer; xmodem_chunk_t chunk = {0}; chunk.block = 1; @@ -316,6 +316,8 @@ is_operation_result cISBootloaderSAMBA::write_uart_modem(uint8_t* buf, size_t le } } while (answer != 'C'); + serialPortFlush(m_port); + // write up to one sector while (len) { @@ -360,7 +362,12 @@ is_operation_result cISBootloaderSAMBA::write_uart_modem(uint8_t* buf, size_t le { return IS_OP_ERROR; } - // serialPortReadChar(&ctx->handle.port, &eot); + ret = serialPortReadCharTimeout(m_port, &eot, SAMBA_TIMEOUT_DEFAULT); + if (ret == 0 || eot != UART_XMODEM_ACK) + { + return IS_OP_ERROR; + } + return IS_OP_OK; } @@ -370,22 +377,20 @@ is_operation_result cISBootloaderSAMBA::flash_erase_write_page(size_t offset, ui uint8_t buf[32]; int count; + count = SNPRINTF((char*)buf, sizeof(buf), "S%08x,%08x#", + (unsigned int)(SAMBA_FLASH_START_ADDRESS + offset), + (unsigned int)SAMBA_PAGE_SIZE); + serialPortWrite(m_port, buf, count); + // Copy data into latch buffer prior to write if (isUSB) { - count = SNPRINTF((char*)buf, sizeof(buf), "S%08x,%08x#", - (unsigned int)(SAMBA_FLASH_START_ADDRESS + offset), - (unsigned int)SAMBA_PAGE_SIZE); - serialPortWrite(m_port, buf, count); serialPortWrite(m_port, data, SAMBA_PAGE_SIZE); } else { - count = SNPRINTF((char*)buf, sizeof(buf), "S%08x,#", (unsigned int)(SAMBA_FLASH_START_ADDRESS + offset)); - serialPortWrite(m_port, buf, count); - // send page data - if (write_uart_modem(buf, SAMBA_PAGE_SIZE) != IS_OP_OK) + if (write_uart_modem(data, SAMBA_PAGE_SIZE) != IS_OP_OK) { return IS_OP_ERROR; } @@ -404,27 +409,30 @@ is_operation_result cISBootloaderSAMBA::verify_image(std::string filename) uint32_t checksum2 = 0; uint32_t nextAddress; - uint8_t buf[SAMBA_PAGE_SIZE]; + uint8_t buf[SAMBA_PAGE_SIZE] = {0}; + uint8_t cmd[42] = { 0 }; int count; serialPortFlush(m_port); - SAMBA_STATUS("(SAMBA) Verifying ISB bootloader...", IS_LOG_LEVEL_INFO); + SAMBA_STATUS("(SAM-BA) Verifying ISB bootloader...", IS_LOG_LEVEL_INFO); + + while (serialPortRead(m_port, buf, 1)); for (uint32_t address = SAMBA_FLASH_START_ADDRESS; address < (SAMBA_FLASH_START_ADDRESS + SAMBA_BOOTLOADER_SIZE_24K); ) { - serialPortFlush(m_port); + int index = 0; nextAddress = address + SAMBA_PAGE_SIZE; - serialPortFlush(m_port); while (address < nextAddress) { - count = SNPRINTF((char*)buf, sizeof(buf), "w%08x,#", address); - serialPortWrite(m_port, buf, count); - serialPortSleep(m_port, 1); + count = SNPRINTF((char*)cmd, sizeof(cmd), "w%08x,#", address); + serialPortWrite(m_port, (const uint8_t*)"#", 2); + serialPortWrite(m_port, cmd, count); + index += serialPortReadTimeout(m_port, buf + index, sizeof(uint32_t), SAMBA_TIMEOUT_DEFAULT); address += sizeof(uint32_t); } - count = serialPortReadTimeout(m_port, buf, SAMBA_PAGE_SIZE, SAMBA_TIMEOUT_DEFAULT); - if (count == SAMBA_PAGE_SIZE) + /*count = serialPortReadTimeout(m_port, buf, SAMBA_PAGE_SIZE, SAMBA_TIMEOUT_DEFAULT);*/ + if (index == SAMBA_PAGE_SIZE) { for (uint32_t* ptr = (uint32_t*)buf, *ptrEnd = (uint32_t*)(buf + sizeof(buf)); ptr < ptrEnd; ptr++) { @@ -443,15 +451,6 @@ is_operation_result cISBootloaderSAMBA::verify_image(std::string filename) return IS_OP_OK; } -/** - * @brief Software reset the chip - * - * @param ctx device context with open serial port registered under `handler` - * @return is_operation_result - */ - - - uint16_t cISBootloaderSAMBA::crc_update(uint16_t crc_in, int incr) { uint16_t crc = crc_in >> 15; diff --git a/src/ISBootloaderSAMBA.h b/src/ISBootloaderSAMBA.h index e2a99bc25..21e41d102 100644 --- a/src/ISBootloaderSAMBA.h +++ b/src/ISBootloaderSAMBA.h @@ -1,7 +1,7 @@ /** * @file ISBootloaderSAMBA.h * @author Dave Cutting (davidcutting42@gmail.com) - * @brief Inertial Sense routines for updating ISB images using SAMBA protocol. + * @brief Inertial Sense routines for updating ISB images using SAM-BA protocol. * */ diff --git a/src/ISBootloaderThread.cpp b/src/ISBootloaderThread.cpp index 0a0584d71..17e7895ce 100644 --- a/src/ISBootloaderThread.cpp +++ b/src/ISBootloaderThread.cpp @@ -26,6 +26,10 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI #include +#if !PLATFORM_IS_WINDOWS +#include +#endif + using namespace std; using namespace ISBootloader; @@ -62,6 +66,37 @@ void cISBootloaderThread::mgmt_thread_libusb(void* context) cISBootloaderDFU::m_DFUmutex.lock(); + m_libusb_thread_mutex.lock(); + cISBootloaderDFU::list_devices(&dfu_list); + for (size_t i = 0; i < dfu_list.present; i++) + { // Create contexts for devices in DFU mode + bool found = false; + + for (size_t j = 0; j < ctx.size(); j++) + { + m_ctx_mutex.lock(); + if (!(ctx[j]->is_serial_device()) && ctx[j]->match_test((void*)dfu_list.id[i].uid) == IS_OP_OK) + { // We found the device in the context list + found = true; + break; + } + m_ctx_mutex.unlock(); + } + + if (!found) + { // If we didn't find the device + thread_libusb_t* new_thread = (thread_libusb_t*)malloc(sizeof(thread_libusb_t)); + new_thread->ctx = NULL; + new_thread->done = false; + new_thread->handle = dfu_list.id[i].handle_libusb; + m_libusb_threads.push_back(new_thread); + m_libusb_threads[m_libusb_threads.size() - 1]->thread = threadCreateAndStart(update_thread_libusb, m_libusb_threads[m_libusb_threads.size() - 1]); + + m_libusb_devicesActive++; + } + } + m_libusb_thread_mutex.unlock(); + while(m_continue_update) { m_libusb_thread_mutex.lock(); @@ -83,36 +118,6 @@ void cISBootloaderThread::mgmt_thread_libusb(void* context) } } - cISBootloaderDFU::list_devices(&dfu_list); // TODO: Put this in a separate thread since it takes a long time - - for (size_t i = 0; i < dfu_list.present; i++) - { // Create contexts for devices in DFU mode - bool found = false; - - for (size_t j = 0; j < ctx.size(); j++) - { - m_ctx_mutex.lock(); - if (!(ctx[j]->is_serial_device()) && ctx[j]->match_test((void*)dfu_list.id[i].uid) == IS_OP_OK) - { // We found the device in the context list - found = true; - break; - } - m_ctx_mutex.unlock(); - } - - if (!found) - { // If we didn't find the device - thread_libusb_t* new_thread = (thread_libusb_t*)malloc(sizeof(thread_libusb_t)); - new_thread->ctx = NULL; - new_thread->done = false; - new_thread->handle = dfu_list.id[i].handle_libusb; - m_libusb_threads.push_back(new_thread); - m_libusb_threads[m_libusb_threads.size() - 1]->thread = threadCreateAndStart(update_thread_libusb, m_libusb_threads[m_libusb_threads.size() - 1]); - - m_libusb_devicesActive++; - } - } - m_libusb_thread_mutex.unlock(); SLEEP_MS(100); @@ -153,7 +158,7 @@ void cISBootloaderThread::mode_thread_serial_app(void* context) serialPortClose(&port); m_serial_thread_mutex.lock(); - thread_info->reuse_port = true; + thread_info->reuse_port = false; thread_info->done = true; m_serial_thread_mutex.unlock(); } @@ -242,8 +247,9 @@ void cISBootloaderThread::update_thread_serial(void* context) thread_info->reuse_port = false; m_serial_thread_mutex.unlock(); + // Start at 115200 always, we will switch to user specified rate after we check for SAM-BA devices serialPortSetPort(&port, serial_name); - if (!serialPortOpenRetry(&port, serial_name, m_baudRate, 1)) + if (!serialPortOpenRetry(&port, serial_name, BAUDRATE_115200, 1)) { serialPortClose(&port); m_serial_thread_mutex.lock(); @@ -252,7 +258,7 @@ void cISBootloaderThread::update_thread_serial(void* context) return; } - is_operation_result result = cISBootloaderBase::update_device(m_firmware, &port, m_infoProgress, m_uploadProgress, m_verifyProgress, ctx, &m_ctx_mutex, &new_context); + is_operation_result result = cISBootloaderBase::update_device(m_firmware, &port, m_infoProgress, m_uploadProgress, m_verifyProgress, ctx, &m_ctx_mutex, &new_context, m_baudRate); if (result == IS_OP_OK) { @@ -268,7 +274,7 @@ void cISBootloaderThread::update_thread_serial(void* context) } else if(result == IS_OP_CLOSED) { - // Device is resetting (may have updated if it was a SAMBA device) + // Device is resetting (may have updated if it was a SAM-BA device) m_serial_thread_mutex.lock(); thread_info->reuse_port = true; m_serial_thread_mutex.unlock(); @@ -348,8 +354,9 @@ vector cISBootloaderThread::set_mode_an m_baudRate = baudRate; m_waitAction = waitAction; - vector ports; // List of ports connected + vector ports; // List of all ports connected, including ignored ports vector ports_user_ignore; // List of ports that were connected at startup but not selected. Will ignore in update. + vector updatesPending; m_serial_threads.clear(); @@ -366,12 +373,12 @@ vector cISBootloaderThread::set_mode_an m_continue_update = true; m_timeStart = current_timeMs(); + m_infoProgress(NULL, "Initializing devices for update...", IS_LOG_LEVEL_INFO); + //////////////////////////////////////////////////////////////////////////// // Run `mode_thread_serial_app` to put all APP devices into ISB mode //////////////////////////////////////////////////////////////////////////// - m_infoProgress(NULL, "Resetting devices into ISB mode... (5 seconds)", IS_LOG_LEVEL_INFO); - // Put all devices in the correct mode while(m_continue_update) { @@ -426,13 +433,13 @@ vector cISBootloaderThread::set_mode_an } } - m_serial_thread_mutex.unlock(); - // Break after 5 seconds if (current_timeMs() - m_timeStart > 5000) { m_continue_update = false; } + + m_serial_thread_mutex.unlock(); } m_continue_update = true; @@ -441,8 +448,6 @@ vector cISBootloaderThread::set_mode_an //////////////////////////////////////////////////////////////////////////// // Join and free //////////////////////////////////////////////////////////////////////////// - - m_infoProgress(NULL, "Waiting for devices to re-enumerate... (5 seconds max.)", IS_LOG_LEVEL_INFO); // Join and free all mode threads while (m_continue_update) @@ -485,8 +490,6 @@ vector cISBootloaderThread::set_mode_an // Run `get_device_isb_version_thread` to get version from ISB bootloaders //////////////////////////////////////////////////////////////////////////// - m_infoProgress(NULL, "Finding devices in ISB mode... (3 seconds)", IS_LOG_LEVEL_INFO); - // Put all devices in the correct mode while(m_continue_update) { @@ -541,13 +544,13 @@ vector cISBootloaderThread::set_mode_an } } - m_serial_thread_mutex.unlock(); - - // Break after 5 seconds + // Break after 3 seconds if (current_timeMs() - m_timeStart > 3000) { m_continue_update = false; } + + m_serial_thread_mutex.unlock(); } m_continue_update = true; @@ -556,8 +559,6 @@ vector cISBootloaderThread::set_mode_an //////////////////////////////////////////////////////////////////////////// // Join threads //////////////////////////////////////////////////////////////////////////// - - m_infoProgress(NULL, "Waiting for devices to re-enumerate... (5 seconds max.)", IS_LOG_LEVEL_INFO); // Join and free all mode threads while (m_continue_update) @@ -585,7 +586,7 @@ vector cISBootloaderThread::set_mode_an SLEEP_MS(100); // Timeout after 5 seconds - if (current_timeMs() - m_timeStart > 5000) + if (current_timeMs() - m_timeStart > 3000) { m_continue_update = false; } @@ -593,8 +594,6 @@ vector cISBootloaderThread::set_mode_an m_serial_thread_mutex.unlock(); } - vector updatesPending; - m_ctx_mutex.lock(); for(size_t i = 0; i < ctx.size(); i++) @@ -658,12 +657,8 @@ is_operation_result cISBootloaderThread::update( m_continue_update = true; m_timeStart = current_timeMs(); - m_infoProgress(NULL, "Resetting devices into ISB mode... (5 seconds)", IS_LOG_LEVEL_INFO); - //////////////////////////////////////////////////////////////////////////// - // Run `mode_thread_serial_isb` to put all ISB devices into DFU/SAMBA mode - // - // Only happens if + // Run `mode_thread_serial_isb` to put all ISB devices into DFU/SAM-BA mode //////////////////////////////////////////////////////////////////////////// while(m_continue_update) @@ -722,8 +717,6 @@ is_operation_result cISBootloaderThread::update( m_continue_update = true; m_timeStart = current_timeMs(); - - m_infoProgress(NULL, "Waiting for devices to re-enumerate... (5 seconds max.)", IS_LOG_LEVEL_INFO); //////////////////////////////////////////////////////////////////////////// // Join and free @@ -759,6 +752,8 @@ is_operation_result cISBootloaderThread::update( m_serial_thread_mutex.unlock(); } + m_infoProgress(NULL, "Updating... (120 seconds max.)", IS_LOG_LEVEL_INFO); + //////////////////////////////////////////////////////////////////////////// // Run `mgmt_thread_libusb` to update DFU devices //////////////////////////////////////////////////////////////////////////// @@ -771,8 +766,6 @@ is_operation_result cISBootloaderThread::update( m_timeStart = current_timeMs(); uint32_t timeoutLong = current_timeMs(); - m_infoProgress(NULL, "Updating devices... (60 seconds max.)", IS_LOG_LEVEL_INFO); - //////////////////////////////////////////////////////////////////////////// // Run `update_thread_serial` to update devices //////////////////////////////////////////////////////////////////////////// @@ -863,15 +856,13 @@ is_operation_result cISBootloaderThread::update( m_serial_thread_mutex.unlock(); // Timeout after 60 seconds - if (current_timeMs() - timeoutLong > 60000) + if (current_timeMs() - timeoutLong > 120000) { m_continue_update = false; } } threadJoinAndFree(libusb_thread); - - m_infoProgress(NULL, "Resetting devices into final mode...", IS_LOG_LEVEL_INFO); // Reset all serial devices up a level into APP or ISB mode for (size_t i = 0; i < ctx.size(); i++) @@ -893,7 +884,7 @@ is_operation_result cISBootloaderThread::update( serialPortClose(&port); } - m_waitAction(); + if (m_waitAction) m_waitAction(); } // Clear the ctx list @@ -907,9 +898,7 @@ is_operation_result cISBootloaderThread::update( m_update_in_progress = false; m_update_mutex.unlock(); - m_infoProgress(NULL, "Done!", IS_LOG_LEVEL_INFO); - - m_waitAction(); // Final UI update + if(m_waitAction) m_waitAction(); // Final UI update return IS_OP_OK; } diff --git a/src/ISConstants.h b/src/ISConstants.h index 315abf2a6..2c8392ec8 100644 --- a/src/ISConstants.h +++ b/src/ISConstants.h @@ -779,6 +779,8 @@ extern void vPortFree(void* pv); #define FLOAT2DOUBLE (double) // Used to prevent warning when compiling with -Wdouble-promotion in Linux +#define REF_INS_SERIAL_NUMBER 99999 // 10101 was prior value + typedef float f_t; typedef int i_t; typedef double ixVector2d[2]; // V = | 0 1 | diff --git a/src/ISDataMappings.cpp b/src/ISDataMappings.cpp index 4c95ca5b8..080016b86 100644 --- a/src/ISDataMappings.cpp +++ b/src/ISDataMappings.cpp @@ -930,7 +930,7 @@ static void PopulateFlashConfigMappings(map_name_to_info_t mappings[DID_COUNT]) ADD_MAP(m, totalSize, "lastLlaUpdateDistance", lastLlaUpdateDistance, 0, DataTypeFloat, float, 0); ADD_MAP(m, totalSize, "ioConfig", ioConfig, 0, DataTypeUInt32, uint32_t, DataFlagsDisplayHex); ADD_MAP(m, totalSize, "platformConfig", platformConfig, 0, DataTypeUInt32, uint32_t, DataFlagsDisplayHex); - ADD_MAP(m, totalSize, "magInclination", magInclination, 0, DataTypeFloat, float, 0); + ADD_MAP(m, totalSize, "gpsTimeUserDelay", gpsTimeUserDelay, 0, DataTypeFloat, float, 0); ADD_MAP(m, totalSize, "magDeclination", magDeclination, 0, DataTypeFloat, float, 0); ADD_MAP(m, totalSize, "gps2AntOffset[0]", gps2AntOffset[0], 0, DataTypeFloat, float&, 0); ADD_MAP(m, totalSize, "gps2AntOffset[1]", gps2AntOffset[1], 0, DataTypeFloat, float&, 0); @@ -1365,8 +1365,8 @@ static void PopulateStrobeInTimeMappings(map_name_to_info_t mappings[DID_COUNT]) ADD_MAP(m, totalSize, "week", week, 0, DataTypeUInt32, uint32_t, 0); ADD_MAP(m, totalSize, "timeOfWeekMs", timeOfWeekMs, 0, DataTypeUInt32, uint32_t, 0); - ADD_MAP(m, totalSize, "pin", pin, 0, DataTypeUInt32, uint32_t, 0); - ADD_MAP(m, totalSize, "count", count, 0, DataTypeUInt32, uint32_t, 0); + ADD_MAP(m, totalSize, "pin", pin, 0, DataTypeUInt16, uint16_t, 0); + ADD_MAP(m, totalSize, "count", count, 0, DataTypeUInt16, uint16_t, 0); ASSERT_SIZE(totalSize); } diff --git a/src/ISDisplay.cpp b/src/ISDisplay.cpp index be8baae9c..a777a1106 100644 --- a/src/ISDisplay.cpp +++ b/src/ISDisplay.cpp @@ -38,14 +38,14 @@ THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLI using namespace std; -#define PRINTV3_P1 "%8.1f,%8.1f,%8.1f\n" -#define PRINTV3_P2 " %8.2f,%8.2f,%8.2f\n" -#define PRINTV3_P3 " %8.3f,%8.3f,%8.3f\n" -#define PRINTV4_P1 "%8.1f,%8.1f,%8.1f,%8.1f\n" -#define PRINTV4_P2 " %8.2f,%8.2f,%8.2f,%8.2f\n" -#define PRINTV4_P3 " %8.3f,%8.3f,%8.3f,%8.3f\n" -#define PRINTV3_LLA "%13.7f,%13.7f,%7.1f ellipsoid\n" -#define PRINTV3_LLA_MSL "%13.7f,%13.7f,%7.1f MSL\n" +#define PRINTV3_P1 "%8.1f,%8.1f,%8.1f" +#define PRINTV3_P2 " %8.2f,%8.2f,%8.2f" +#define PRINTV3_P3 " %8.3f,%8.3f,%8.3f" +#define PRINTV4_P1 "%8.1f,%8.1f,%8.1f,%8.1f" +#define PRINTV4_P2 " %8.2f,%8.2f,%8.2f,%8.2f" +#define PRINTV4_P3 " %8.3f,%8.3f,%8.3f,%8.3f" +#define PRINTV3_LLA "%13.7f,%13.7f,%7.1f ellipsoid" +#define PRINTV3_LLA_MSL "%13.7f,%13.7f,%7.1f MSL" #define BUF_SIZE 8192 #define DATASET_VIEW_NUM_ROWS 25 @@ -733,17 +733,17 @@ string cInertialSenseDisplay::DataToStringINS1(const ins_1_t &ins1, const p_data else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tEuler\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2 "\n", ins1.theta[0] * C_RAD2DEG_F, // Roll ins1.theta[1] * C_RAD2DEG_F, // Pitch ins1.theta[2] * C_RAD2DEG_F); // Yaw ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tUWV\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1 "\n", ins1.uvw[0], // U body velocity ins1.uvw[1], // V body velocity ins1.uvw[2]); // W body velocity ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tLLA\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA "\n", ins1.lla[0], // INS Latitude ins1.lla[1], // INS Longitude ins1.lla[2]); // INS Ellipsoid altitude (meters) @@ -781,7 +781,7 @@ string cInertialSenseDisplay::DataToStringINS2(const ins_2_t &ins2, const p_data else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tQn2b\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3, // Quaternion attitude rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3 "\n", // Quaternion attitude rotation ins2.qn2b[0], // W ins2.qn2b[1], // X ins2.qn2b[2], // Y @@ -789,17 +789,17 @@ string cInertialSenseDisplay::DataToStringINS2(const ins_2_t &ins2, const p_data float theta[3]; quat2euler(ins2.qn2b, theta); ptr += SNPRINTF(ptr, ptrEnd - ptr, "\t(Euler)\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2, // Convert quaternion to euler rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2 "\n", // Convert quaternion to euler rotation theta[0] * C_RAD2DEG_F, // Roll theta[1] * C_RAD2DEG_F, // Pitch theta[2] * C_RAD2DEG_F); // Yaw ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tUWV\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1 "\n", ins2.uvw[0], // U body velocity ins2.uvw[1], // V body velocity ins2.uvw[2]); // W body velocity ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tLLA\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA "\n", ins2.lla[0], // INS Latitude ins2.lla[1], // INS Longitude ins2.lla[2]); // INS Ellipsoid altitude (meters) @@ -837,7 +837,7 @@ string cInertialSenseDisplay::DataToStringINS3(const ins_3_t &ins3, const p_data else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tQn2b\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3, // Quaternion attitude rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3 "\n", // Quaternion attitude rotation ins3.qn2b[0], // W ins3.qn2b[1], // X ins3.qn2b[2], // Y @@ -845,17 +845,17 @@ string cInertialSenseDisplay::DataToStringINS3(const ins_3_t &ins3, const p_data float theta[3]; quat2euler(ins3.qn2b, theta); ptr += SNPRINTF(ptr, ptrEnd - ptr, "\t(Euler)\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2, // Convert quaternion to euler rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2 "\n", // Convert quaternion to euler rotation theta[0] * C_RAD2DEG_F, // Roll theta[1] * C_RAD2DEG_F, // Pitch theta[2] * C_RAD2DEG_F); // Yaw ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tUWV\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1 "\n", ins3.uvw[0], // U body velocity ins3.uvw[1], // V body velocity ins3.uvw[2]); // W body velocity ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tLLA\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA_MSL, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA_MSL "\n", ins3.lla[0], // INS Latitude ins3.lla[1], // INS Longitude ins3.lla[2]); // INS Ellipsoid altitude (meters) @@ -893,7 +893,7 @@ string cInertialSenseDisplay::DataToStringINS4(const ins_4_t &ins4, const p_data else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tQe2b\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3, // Quaternion attitude rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV4_P3 "\n", // Quaternion attitude rotation ins4.qe2b[0], // W ins4.qe2b[1], // X ins4.qe2b[2], // Y @@ -901,17 +901,17 @@ string cInertialSenseDisplay::DataToStringINS4(const ins_4_t &ins4, const p_data float theta[3]; qe2b2EulerNedEcef(theta, (float*)ins4.qe2b, (double*)ins4.ecef); ptr += SNPRINTF(ptr, ptrEnd - ptr, "\t(Euler)\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2, // Convert quaternion to euler rotation + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2 "\n", // Convert quaternion to euler rotation theta[0] * C_RAD2DEG_F, // Roll theta[1] * C_RAD2DEG_F, // Pitch theta[2] * C_RAD2DEG_F); // Yaw ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tVE\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3 "\n", ins4.ve[0], // X ECEF velocity ins4.ve[1], // Y ECEF velocity ins4.ve[2]); // Z ECEF velocity ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tECEF\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3 "\n", ins4.ecef[0], // X ECEF position ins4.ecef[1], // Y ECEF position ins4.ecef[2]); // Z ECEF position @@ -951,12 +951,12 @@ string cInertialSenseDisplay::DataToStringIMU(const imu_t &imu, const p_data_hdr { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n"); ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tPQR\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1 "\n", imu.I.pqr[0] * C_RAD2DEG_F, // P angular rate imu.I.pqr[1] * C_RAD2DEG_F, // Q angular rate imu.I.pqr[2] * C_RAD2DEG_F); // R angular rate ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tAcc\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P1 "\n", imu.I.acc[0], // X acceleration imu.I.acc[1], // Y acceleration imu.I.acc[2]); // Z acceleration @@ -993,12 +993,12 @@ string cInertialSenseDisplay::DataToStringPreintegratedImu(const pimu_t &imu, co else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tIMU1 theta\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3 "\n", imu.theta[0] * C_RAD2DEG_F, // IMU1 P angular rate imu.theta[1] * C_RAD2DEG_F, // IMU1 Q angular rate imu.theta[2] * C_RAD2DEG_F); // IMU1 R angular rate ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tIMU1 vel\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3 "\n", imu.vel[0], // IMU1 X acceleration imu.vel[1], // IMU1 Y acceleration imu.vel[2]); // IMU1 Z acceleration @@ -1064,7 +1064,7 @@ string cInertialSenseDisplay::DataToStringMagnetometer(const magnetometer_t &mag else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\tmag\t"); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P2 "\n", mag.mag[0], // X magnetometer mag.mag[1], // Y magnetometer mag.mag[2]); // Z magnetometer @@ -1145,29 +1145,28 @@ string cInertialSenseDisplay::DataToStringGpsPos(const gps_pos_t &gps, const p_d case GPS_STATUS_FIX_RTK_FLOAT: ptr += SNPRINTF(ptr, ptrEnd - ptr, "RTK Float"); break; case GPS_STATUS_FIX_RTK_FIX: ptr += SNPRINTF(ptr, ptrEnd - ptr, "RTK FIX"); break; } - ptr += SNPRINTF(ptr, ptrEnd - ptr, "),\thAcc: %.3f m cno: %3.1f dBHz\n", gps.hAcc, gps.cnoMean); // Position accuracy + ptr += SNPRINTF(ptr, ptrEnd - ptr, ") \thAcc: %.3f m cno: %3.1f dBHz\n", gps.hAcc, gps.cnoMean); // Position accuracy ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tLLA: "); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA " ", gps.lla[0], // GPS Latitude gps.lla[1], // GPS Longitude gps.lla[2]); // GPS Ellipsoid altitude (meters) + bool comma = false; if (gps.status&GPS_STATUS_FLAGS_GPS1_RTK_POSITION_ENABLED) - { - if (gps.status&GPS_STATUS_FLAGS_GPS1_RTK_RAW_GPS_DATA_ERROR) { ptr += SNPRINTF(ptr, ptrEnd - ptr, "Raw error, "); } + { + if (gps.status&GPS_STATUS_FLAGS_GPS1_RTK_RAW_GPS_DATA_ERROR) { AddCommaToString(comma, ptr, ptrEnd); ptr += SNPRINTF(ptr, ptrEnd - ptr, "Raw error"); } switch (gps.status&GPS_STATUS_FLAGS_ERROR_MASK) { - case GPS_STATUS_FLAGS_GPS1_RTK_BASE_DATA_MISSING: ptr += SNPRINTF(ptr, ptrEnd - ptr, "Base missing, "); break; - case GPS_STATUS_FLAGS_GPS1_RTK_BASE_POSITION_MOVING: ptr += SNPRINTF(ptr, ptrEnd - ptr, "Moving base, "); break; - case GPS_STATUS_FLAGS_GPS1_RTK_BASE_POSITION_INVALID: ptr += SNPRINTF(ptr, ptrEnd - ptr, "Moving invalid, "); break; + case GPS_STATUS_FLAGS_GPS1_RTK_BASE_DATA_MISSING: { AddCommaToString(comma, ptr, ptrEnd); ptr += SNPRINTF(ptr, ptrEnd - ptr, "Base missing"); } break; + case GPS_STATUS_FLAGS_GPS1_RTK_BASE_POSITION_MOVING: { AddCommaToString(comma, ptr, ptrEnd); ptr += SNPRINTF(ptr, ptrEnd - ptr, "Moving base"); } break; + case GPS_STATUS_FLAGS_GPS1_RTK_BASE_POSITION_INVALID: { AddCommaToString(comma, ptr, ptrEnd); ptr += SNPRINTF(ptr, ptrEnd - ptr, "Moving invalid, "); } break; } } if (gps.status&GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED) { - if (gps.status&GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED) - { - ptr += SNPRINTF(ptr, ptrEnd - ptr, "Compassing, "); - } + if (gps.status&GPS_STATUS_FLAGS_GPS2_RTK_COMPASS_ENABLED) { AddCommaToString(comma, ptr, ptrEnd); ptr += SNPRINTF(ptr, ptrEnd - ptr, "Compassing"); } } + ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n"); } @@ -1201,7 +1200,7 @@ string cInertialSenseDisplay::DataToStringRtkRel(const gps_rtk_rel_t &rel, const else { // Spacious format ptr += SNPRINTF(ptr, ptrEnd - ptr, "\tbaseToRover: "); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_P3 "\n", rel.baseToRoverVector[0], // Vector to base in ECEF rel.baseToRoverVector[1], // Vector to base in ECEF rel.baseToRoverVector[2]); // Vector to base in ECEF @@ -1280,7 +1279,7 @@ string cInertialSenseDisplay::DataToStringSurveyIn(const survey_in_t &survey, co if (m_displayMode != DMODE_SCROLL) { ptr += SNPRINTF(ptr, ptrEnd - ptr, "\n\thAcc: %4.3f\tlla:", survey.hAccuracy); - ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA, + ptr += SNPRINTF(ptr, ptrEnd - ptr, PRINTV3_LLA "\n", survey.lla[0], // latitude survey.lla[1], // longitude survey.lla[2]); // altitude diff --git a/src/ISDisplay.h b/src/ISDisplay.h index 668bfd7ec..2138b340b 100644 --- a/src/ISDisplay.h +++ b/src/ISDisplay.h @@ -113,6 +113,7 @@ class cInertialSenseDisplay std::string DataToStringSensorsADC(const sys_sensors_adc_t &sensorsADC, const p_data_hdr_t& hdr); std::string DataToStringWheelEncoder(const wheel_encoder_t &enc, const p_data_hdr_t& hdr); std::string DataToStringGeneric(const p_data_t* data); + void AddCommaToString(bool &comma, char* &ptr, char* &ptrEnd){ if (comma) { ptr += SNPRINTF(ptr, ptrEnd - ptr, ", "); } comma = true; }; std::string DatasetToString(const p_data_t* data); diff --git a/src/ISLogger.cpp b/src/ISLogger.cpp index 25210f30b..fec602630 100644 --- a/src/ISLogger.cpp +++ b/src/ISLogger.cpp @@ -543,6 +543,16 @@ uint32_t cISLogger::FileCount( unsigned int device ) return m_devices[device]->FileCount(); } +std::string cISLogger::GetNewFileName(unsigned int device, uint32_t serialNumber, uint32_t fileCount, const char* suffix) +{ + if (device < m_devices.size()) + { + return m_devices[device]->GetNewFileName(serialNumber, fileCount, suffix); + } + + return std::string(""); +} + bool cISLogger::SetDeviceInfo(const dev_info_t *info, unsigned int device ) { if (device >= m_devices.size() || info == NULL) @@ -587,7 +597,7 @@ bool cISLogger::CopyLog(cISLogger& log, const string& timestamp, const string &o #endif // Set KML configuration - m_devices[dev]->SetKmlConfig(m_showPath, m_showSample, m_showTimeStamp, m_iconUpdatePeriodSec, m_altClampToGround); + m_devices[dev]->SetKmlConfig(m_gpsData, m_showPath, m_showSample, m_showTimeStamp, m_iconUpdatePeriodSec, m_altClampToGround); // Copy data for (g_copyReadCount = 0; (data = log.ReadData(dev)); g_copyReadCount++) diff --git a/src/ISLogger.h b/src/ISLogger.h index 53824ab47..be6105ccf 100644 --- a/src/ISLogger.h +++ b/src/ISLogger.h @@ -87,6 +87,7 @@ class cISLogger float LogSizeMB(unsigned int device = 0); float FileSizeMB(unsigned int device = 0); uint32_t FileCount(unsigned int device = 0); + std::string GetNewFileName(unsigned int device, uint32_t serialNumber, uint32_t fileCount, const char* suffix); uint32_t GetDeviceCount() { return (uint32_t)m_devices.size(); } bool SetDeviceInfo(const dev_info_t *info, unsigned int device = 0); const dev_info_t* GetDeviceInfo(unsigned int device = 0); @@ -127,8 +128,9 @@ class cISLogger // the map contains device id (serial number) key and a vector containing log data for each data id, which will be an empty vector if no log data for that id static bool ReadAllLogDataIntoMemory(const std::string& directory, std::map>>& data); - void SetKmlConfig(bool showPath = true, bool showSample = false, bool showTimeStamp = true, double updatePeriodSec = 1.0, bool altClampToGround = true) + void SetKmlConfig(bool gpsData = true, bool showPath = true, bool showSample = false, bool showTimeStamp = true, double updatePeriodSec = 1.0, bool altClampToGround = true) { + m_gpsData = gpsData; m_showPath = showPath; m_showSample = showSample; m_showTimeStamp = showTimeStamp; @@ -198,6 +200,7 @@ class cISLogger #endif bool m_altClampToGround; + bool m_gpsData; bool m_showSample; bool m_showPath; bool m_showTimeStamp; diff --git a/src/InertialSense.cpp b/src/InertialSense.cpp index d50789404..5831026ca 100644 --- a/src/InertialSense.cpp +++ b/src/InertialSense.cpp @@ -742,6 +742,23 @@ is_operation_result InertialSense::BootloadFile( cISSerialPort::GetComPorts(all_ports); + // On non-Windows systems, try to interpret each user-specified port as a symlink and find what it is pointing to + // TODO: This only works for "/dev/" ports +#if !PLATFORM_IS_WINDOWS + for(unsigned int k = 0; k < comPorts.size(); k++) + { + char buf[PATH_MAX]; + int newsize = readlink(comPorts[k].c_str(), buf, sizeof(buf)-1); + if(newsize < 0) + { + continue; + } + + buf[newsize] = '\0'; + comPorts[k] = "/dev/" + string(buf); + } +#endif + // Get the list of ports to ignore during the bootloading process sort(all_ports.begin(), all_ports.end()); sort(comPorts.begin(), comPorts.end()); @@ -761,6 +778,8 @@ is_operation_result InertialSense::BootloadFile( #if !PLATFORM_IS_WINDOWS fputs("\e[?25l", stdout); // Turn off cursor during firmare update #endif + + printf("\n\r"); ISBootloader::firmwares_t files; files.fw_uINS_3.path = fileName; @@ -784,6 +803,8 @@ is_operation_result InertialSense::BootloadFile( cISBootloaderThread::update(update_ports, forceBootloaderUpdate, baudRate, files, uploadProgress, verifyProgress, infoProgress, waitAction); + printf("\n\r"); + #if !PLATFORM_IS_WINDOWS fputs("\e[?25h", stdout); // Turn cursor back on #endif diff --git a/src/data_sets.h b/src/data_sets.h index be15fdabe..d77e7bdd7 100644 --- a/src/data_sets.h +++ b/src/data_sets.h @@ -209,8 +209,8 @@ enum eInsStatusFlags /** Nav mode (set) = estimating velocity and position. AHRS mode (cleared) = NOT estimating velocity and position */ INS_STATUS_NAV_MODE = (int)0x00001000, - /** User should not move (keep system motionless) to assist on-board processing. */ - INS_STATUS_DO_NOT_MOVE = (int)0x00002000, + /** INS in stationary mode. If initiated by zero velocity command, user should not move (keep system motionless) to assist on-board processing. */ + INS_STATUS_STATIONARY_MODE = (int)0x00002000, /** Velocity aided by GPS velocity */ INS_STATUS_GPS_AIDING_VEL = (int)0x00004000, /** Vehicle kinematic calibration is good */ @@ -1186,6 +1186,8 @@ enum eGenFaultCodes GFC_INIT_BAROMETER = 0x00200000, /*! Fault: I2C initialization */ GFC_INIT_I2C = 0x00800000, + /*! Fault: Chip erase line toggled but did not meet required hold time. This is caused by noise/transient on chip erase pin. */ + GFC_CHIP_ERASE_INVALID = 0x01000000, }; @@ -1202,24 +1204,39 @@ typedef struct PACKED enum eSystemCommand { - SYS_CMD_SAVE_PERSISTENT_MESSAGES = 1, - SYS_CMD_ENABLE_BOOTLOADER_AND_RESET = 2, - SYS_CMD_ENABLE_SENSOR_STATS = 3, - SYS_CMD_ENABLE_RTOS_STATS = 4, - SYS_CMD_ZERO_MOTION = 5, - - SYS_CMD_ENABLE_GPS_LOW_LEVEL_CONFIG = 10, - - SYS_CMD_SAVE_FLASH = 97, - SYS_CMD_SAVE_GPS_ASSIST_TO_FLASH_RESET = 98, - SYS_CMD_SOFTWARE_RESET = 99, - SYS_CMD_MANF_UNLOCK = 1122334455, - SYS_CMD_MANF_FACTORY_RESET = 1357924680, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. - SYS_CMD_MANF_CHIP_ERASE = 1357924681, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. - SYS_CMD_MANF_DOWNGRADE_CALIBRATION = 1357924682, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. + SYS_CMD_SAVE_PERSISTENT_MESSAGES = 1, + SYS_CMD_ENABLE_BOOTLOADER_AND_RESET = 2, + SYS_CMD_ENABLE_SENSOR_STATS = 3, + SYS_CMD_ENABLE_RTOS_STATS = 4, + SYS_CMD_ZERO_MOTION = 5, + SYS_CMD_REF_POINT_STATIONARY = 6, + SYS_CMD_REF_POINT_MOVING = 7, + + SYS_CMD_ENABLE_GPS_LOW_LEVEL_CONFIG = 10, + SYS_CMD_ENABLE_SERIAL_PORT_BRIDGE_USB_TO_GPS1 = 11, + SYS_CMD_ENABLE_SERIAL_PORT_BRIDGE_USB_TO_GPS2 = 12, + SYS_CMD_ENABLE_SERIAL_PORT_BRIDGE_USB_TO_SER0 = 13, + SYS_CMD_ENABLE_SERIAL_PORT_BRIDGE_USB_TO_SER1 = 14, + SYS_CMD_ENABLE_SERIAL_PORT_BRIDGE_USB_TO_SER2 = 15, + + SYS_CMD_SAVE_FLASH = 97, + SYS_CMD_SAVE_GPS_ASSIST_TO_FLASH_RESET = 98, + SYS_CMD_SOFTWARE_RESET = 99, + SYS_CMD_MANF_UNLOCK = 1122334455, + SYS_CMD_MANF_FACTORY_RESET = 1357924680, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. + SYS_CMD_MANF_CHIP_ERASE = 1357924681, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. + SYS_CMD_MANF_DOWNGRADE_CALIBRATION = 1357924682, // SYS_CMD_MANF_RESET_UNLOCK must be sent prior to this command. }; - +enum eSerialPortBridge +{ + SERIAL_PORT_BRIDGE_DISABLED = 0, + SERIAL_PORT_BRIDGE_USB_TO_GPS1 = 1, + SERIAL_PORT_BRIDGE_USB_TO_GPS2 = 2, + SERIAL_PORT_BRIDGE_USB_TO_SER0 = 3, + SERIAL_PORT_BRIDGE_USB_TO_SER1 = 4, + SERIAL_PORT_BRIDGE_USB_TO_SER2 = 5, +}; /** (DID_ASCII_BCAST_PERIOD) ASCII broadcast periods. This data structure is zeroed out on stop_all_broadcasts */ typedef struct PACKED @@ -1882,6 +1899,8 @@ enum eSysConfigBits /** Use reference IMU in EKF instead of onboard IMU */ SYS_CFG_USE_REFERENCE_IMU_IN_EKF = (int)0x01000000, + /** Reference point stationary on strobe input */ + SYS_CFG_EKF_REF_POINT_STATIONARY_ON_STROBE_INPUT = (int)0x02000000, }; @@ -2142,6 +2161,7 @@ enum eIoConfig { /** Strobe (input and output) trigger on rising edge (0 = falling edge) */ IO_CONFIG_STROBE_TRIGGER_HIGH = (int)0x00000001, + // G1,G2 - STROBE, CAN, Ser2, I2C (future) /** G1,G2 - STROBE input on G2 */ IO_CONFIG_G1G2_STROBE_INPUT_G2 = (int)0x00000002, @@ -2194,7 +2214,9 @@ enum eIoConfig /** G5,G8 - Default */ IO_CONFIG_G5G8_DEFAULT = (int)0, - /** Unused bits */ + /** G15 (GPS PPS) - STROBE */ + IO_CONFIG_G15_STROBE_INPUT = (int)0x00000800, + // IO_CONFIG_ = (int)0x00001000, /** External GPS TIMEPULSE source */ IO_CFG_GPS_TIMEPUSE_SOURCE_BITMASK = (int)0x0000E000, @@ -2267,6 +2289,7 @@ enum eIoConfig IO_CONFIG_IMU_3_DISABLE = (int)0x40000000, /** Unused bits */ + // IO_CONFIG_ = (int)0x80000000, }; #define IO_CONFIG_DEFAULT (IO_CONFIG_G1G2_DEFAULT | IO_CONFIG_G5G8_DEFAULT | IO_CONFIG_G6G7_DEFAULT | IO_CONFIG_G9_DEFAULT | (IO_CONFIG_GPS_SOURCE_ONBOARD_1<port, &sb) != 0) - { - error_message("error: serial port not open\r\n\r\n"); + { // Serial port not open return 0; } int count = write(handle->fd, buffer, writeCount); if(count != writeCount) - { - error_message("write error %d\r\n\r\n", errno); + { // Write error return 0; } int error = 0; - // if(handle->blocking) error = tcdrain(handle->fd); + if(handle->blocking) error = tcdrain(handle->fd); if (error != 0) - { - error_message("error %d from tcdrain\r\n\r\n", errno); + { // Drain error return 0; }