From 343f093ead9380347681aed5decb524281083746 Mon Sep 17 00:00:00 2001 From: Kamil Tomaszewski Date: Tue, 16 Jun 2020 20:41:02 +0200 Subject: [PATCH] spresense: Add support for GNSS --- ports/cxd56/common-hal/gnss/GNSS.c | 152 ++++++++++++++++++ ports/cxd56/common-hal/gnss/GNSS.h | 41 +++++ ports/cxd56/common-hal/gnss/PositionFix.c | 0 ports/cxd56/common-hal/gnss/SatelliteSystem.c | 0 ports/cxd56/common-hal/gnss/__init__.c | 0 ports/cxd56/mpconfigport.mk | 1 + 6 files changed, 194 insertions(+) create mode 100644 ports/cxd56/common-hal/gnss/GNSS.c create mode 100644 ports/cxd56/common-hal/gnss/GNSS.h create mode 100644 ports/cxd56/common-hal/gnss/PositionFix.c create mode 100644 ports/cxd56/common-hal/gnss/SatelliteSystem.c create mode 100644 ports/cxd56/common-hal/gnss/__init__.c diff --git a/ports/cxd56/common-hal/gnss/GNSS.c b/ports/cxd56/common-hal/gnss/GNSS.c new file mode 100644 index 0000000000..0e5655534b --- /dev/null +++ b/ports/cxd56/common-hal/gnss/GNSS.c @@ -0,0 +1,152 @@ +/* + * This file is part of the MicroPython project, http://micropython.org/ + * + * The MIT License (MIT) + * + * Copyright 2020 Sony Semiconductor Solutions Corporation + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#include +#include +#include +#include + +#include "py/runtime.h" + +#include "shared-bindings/gnss/GNSS.h" + +typedef struct { + const char* devpath; + int fd; +} gnss_dev_t; + +STATIC gnss_dev_t gnss_dev = {"/dev/gps", -1}; + +static unsigned long satellitesystem_type_to_bit_fields(gnss_satellitesystem_t system) { + switch (system) { + case SATELLITESYSTEM_GPS: + return CXD56_GNSS_SAT_GPS; + case SATELLITESYSTEM_GLONASS: + return CXD56_GNSS_SAT_GLONASS; + case SATELLITESYSTEM_SBAS: + return CXD56_GNSS_SAT_SBAS; + case SATELLITESYSTEM_QZSS_L1CA: + return CXD56_GNSS_SAT_QZ_L1CA; + case SATELLITESYSTEM_QZSS_L1S: + return CXD56_GNSS_SAT_QZ_L1S; + case SATELLITESYSTEM_NONE: + default: + return CXD56_GNSS_SAT_NONE; + } +} + +static gnss_positionfix_t fix_to_positionfix_type(uint8_t fix) { + switch (fix) { + case CXD56_GNSS_PVT_POSFIX_2D: + return POSITIONFIX_2D; + case CXD56_GNSS_PVT_POSFIX_3D: + return POSITIONFIX_3D; + case CXD56_GNSS_PVT_POSFIX_INVALID: + default: + return POSITIONFIX_INVALID; + } +} + +void common_hal_gnss_construct(gnss_obj_t *self) { + if (gnss_dev.fd < 0) { + gnss_dev.fd = open(gnss_dev.devpath, O_RDONLY); + if (gnss_dev.fd < 0) { + mp_raise_ValueError(translate("Could not initialize GNSS")); + } + } + + self->satellite_system = 0; + self->fix = POSITIONFIX_INVALID; +} + +void common_hal_gnss_deinit(gnss_obj_t *self) { + if (common_hal_gnss_deinited(self)) { + return; + } + + close(gnss_dev.fd); + gnss_dev.fd = -1; +} + +bool common_hal_gnss_deinited(gnss_obj_t *self) { + return gnss_dev.fd < 0; +} + +void common_hal_gnss_select(gnss_obj_t *self, gnss_satellitesystem_t system) { + unsigned long selection = self->satellite_system | satellitesystem_type_to_bit_fields(system); + + ioctl(gnss_dev.fd, CXD56_GNSS_IOCTL_SELECT_SATELLITE_SYSTEM, selection); + + self->satellite_system = selection; +} + +void common_hal_gnss_deselect(gnss_obj_t *self, gnss_satellitesystem_t system) { + unsigned long selection = self->satellite_system & ~satellitesystem_type_to_bit_fields(system); + + ioctl(gnss_dev.fd, CXD56_GNSS_IOCTL_SELECT_SATELLITE_SYSTEM, selection); + + self->satellite_system = selection; +} + +void common_hal_gnss_start(gnss_obj_t *self) { + if (self->satellite_system == 0) { + mp_raise_ValueError(translate("Cannot start without selecting at least one satellite system.")); + } + ioctl(gnss_dev.fd, CXD56_GNSS_IOCTL_START, CXD56_GNSS_STMOD_COLD); +} + +void common_hal_gnss_stop(gnss_obj_t *self) { + ioctl(gnss_dev.fd, CXD56_GNSS_IOCTL_STOP, 0); +} + +void common_hal_gnss_update(gnss_obj_t *self) { + struct cxd56_gnss_positiondata_s positiondata; + + read(gnss_dev.fd, &positiondata, sizeof(struct cxd56_gnss_positiondata_s)); + + if (positiondata.receiver.pos_dataexist) { + self->fix = positiondata.receiver.pos_fixmode; + self->latitude = positiondata.receiver.latitude; + self->longitude = positiondata.receiver.longitude; + self->altitude = positiondata.receiver.altitude; + } +} + +mp_float_t common_hal_gnss_get_latitude(gnss_obj_t *self) { + return (mp_float_t) self->latitude; +} + +mp_float_t common_hal_gnss_get_longitude(gnss_obj_t *self) { + return (mp_float_t) self->longitude; +} + +mp_float_t common_hal_gnss_get_altitude(gnss_obj_t *self) { + return (mp_float_t) self->altitude; +} + +gnss_positionfix_t common_hal_gnss_get_fix(gnss_obj_t *self) { + return fix_to_positionfix_type(self->fix); +} diff --git a/ports/cxd56/common-hal/gnss/GNSS.h b/ports/cxd56/common-hal/gnss/GNSS.h new file mode 100644 index 0000000000..fd44bcb5cd --- /dev/null +++ b/ports/cxd56/common-hal/gnss/GNSS.h @@ -0,0 +1,41 @@ +/* + * This file is part of the MicroPython project, http://micropython.org/ + * + * The MIT License (MIT) + * + * Copyright 2020 Sony Semiconductor Solutions Corporation + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + * THE SOFTWARE. + */ + +#ifndef MICROPY_INCLUDED_CXD56_COMMON_HAL_GNSS_GNSS_H +#define MICROPY_INCLUDED_CXD56_COMMON_HAL_GNSS_GNSS_H + +#include "py/obj.h" + +typedef struct { + mp_obj_base_t base; + unsigned long satellite_system; + uint8_t fix; + double latitude; + double longitude; + double altitude; +} gnss_obj_t; + +#endif // MICROPY_INCLUDED_CXD56_COMMON_HAL_GNSS_GNSS_H diff --git a/ports/cxd56/common-hal/gnss/PositionFix.c b/ports/cxd56/common-hal/gnss/PositionFix.c new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ports/cxd56/common-hal/gnss/SatelliteSystem.c b/ports/cxd56/common-hal/gnss/SatelliteSystem.c new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ports/cxd56/common-hal/gnss/__init__.c b/ports/cxd56/common-hal/gnss/__init__.c new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ports/cxd56/mpconfigport.mk b/ports/cxd56/mpconfigport.mk index 8aeae1de56..6c59e12993 100644 --- a/ports/cxd56/mpconfigport.mk +++ b/ports/cxd56/mpconfigport.mk @@ -14,6 +14,7 @@ CIRCUITPY_I2CSLAVE = 0 CIRCUITPY_ROTARYIO = 0 CIRCUITPY_TOUCHIO = 0 CIRCUITPY_GAMEPAD = 0 +CIRCUITPY_GNSS = 1 CIRCUITPY_NEOPIXEL_WRITE = 0 CIRCUITPY_NVM = 0 CIRCUITPY_DISPLAYIO = 0