Add support for the Deepblu Cosmiq+
The Deepblu Cosmiq+ uses a plaintext and line based communication protocol over BLE, where the binary payload data is encoded as hexadecimal characters. Based-on-code-by: Linus Torvalds <torvalds@linux-foundation.org>
This commit is contained in:
Jef Driesen
parent
3ce34a0b6d
commit
86e9cc3443
@ -96,6 +96,7 @@ static const backend_table_t g_backends[] = {
|
||||
{"sp2", DC_FAMILY_SPORASUB_SP2, 0},
|
||||
{"excursion", DC_FAMILY_DEEPSIX_EXCURSION, 0},
|
||||
{"screen", DC_FAMILY_SEAC_SCREEN, 0},
|
||||
{"cosmiq", DC_FAMILY_DEEPBLU_COSMIQ, 0},
|
||||
};
|
||||
|
||||
static const transport_table_t g_transports[] = {
|
||||
|
||||
@ -114,6 +114,8 @@ typedef enum dc_family_t {
|
||||
DC_FAMILY_DEEPSIX_EXCURSION = (19 << 16),
|
||||
/* Seac Screen */
|
||||
DC_FAMILY_SEAC_SCREEN = (20 << 16),
|
||||
/* Deepblu Cosmiq */
|
||||
DC_FAMILY_DEEPBLU_COSMIQ = (21 << 16),
|
||||
} dc_family_t;
|
||||
|
||||
#ifdef __cplusplus
|
||||
|
||||
@ -182,6 +182,8 @@
|
||||
<ClCompile Include="..\src\cressi_leonardo_parser.c" />
|
||||
<ClCompile Include="..\src\custom.c" />
|
||||
<ClCompile Include="..\src\datetime.c" />
|
||||
<ClCompile Include="..\src\deepblu_cosmiq.c" />
|
||||
<ClCompile Include="..\src\deepblu_cosmiq_parser.c" />
|
||||
<ClCompile Include="..\src\deepsix_excursion.c" />
|
||||
<ClCompile Include="..\src\deepsix_excursion_parser.c" />
|
||||
<ClCompile Include="..\src\descriptor.c" />
|
||||
@ -308,6 +310,7 @@
|
||||
<ClInclude Include="..\src\cressi_edy.h" />
|
||||
<ClInclude Include="..\src\cressi_goa.h" />
|
||||
<ClInclude Include="..\src\cressi_leonardo.h" />
|
||||
<ClInclude Include="..\src\deepblu_cosmiq.h" />
|
||||
<ClInclude Include="..\src\deepsix_excursion.h" />
|
||||
<ClInclude Include="..\src\descriptor-private.h" />
|
||||
<ClInclude Include="..\src\device-private.h" />
|
||||
|
||||
@ -77,6 +77,7 @@ libdivecomputer_la_SOURCES = \
|
||||
sporasub_sp2.h sporasub_sp2.c sporasub_sp2_parser.c \
|
||||
deepsix_excursion.h deepsix_excursion.c deepsix_excursion_parser.c \
|
||||
seac_screen.h seac_screen.c seac_screen_parser.c \
|
||||
deepblu_cosmiq.h deepblu_cosmiq.c deepblu_cosmiq_parser.c \
|
||||
socket.h socket.c \
|
||||
irda.c \
|
||||
usb.c \
|
||||
|
||||
11
src/array.c
11
src/array.c
@ -373,3 +373,14 @@ bcd2dec (unsigned char value)
|
||||
{
|
||||
return ((value >> 4) & 0x0f) * 10 + (value & 0x0f);
|
||||
}
|
||||
|
||||
unsigned char
|
||||
dec2bcd (unsigned char value)
|
||||
{
|
||||
if (value >= 100)
|
||||
return 0;
|
||||
|
||||
unsigned char hi = value / 10;
|
||||
unsigned char lo = value % 10;
|
||||
return (hi << 4) | lo;
|
||||
}
|
||||
|
||||
@ -120,6 +120,9 @@ array_uint16_le_set (unsigned char data[], const unsigned short input);
|
||||
unsigned char
|
||||
bcd2dec (unsigned char value);
|
||||
|
||||
unsigned char
|
||||
dec2bcd (unsigned char value);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* __cplusplus */
|
||||
|
||||
548
src/deepblu_cosmiq.c
Normal file
548
src/deepblu_cosmiq.c
Normal file
@ -0,0 +1,548 @@
|
||||
/*
|
||||
* libdivecomputer
|
||||
*
|
||||
* Copyright (C) 2019 Linus Torvalds
|
||||
* Copyright (C) 2022 Jef Driesen
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
|
||||
#include "deepblu_cosmiq.h"
|
||||
#include "context-private.h"
|
||||
#include "device-private.h"
|
||||
#include "platform.h"
|
||||
#include "checksum.h"
|
||||
#include "array.h"
|
||||
|
||||
// Maximum data in a packet. It's actually much
|
||||
// less than this, since BLE packets are small and
|
||||
// with the 7 bytes of headers and final newline
|
||||
// and the HEX encoding, the actual maximum is
|
||||
// just something like 6 bytes.
|
||||
//
|
||||
// But in theory the data could be done over
|
||||
// multiple packets. That doesn't seem to be
|
||||
// the case in anything I've seen so far.
|
||||
//
|
||||
// Pick something small and easy to use for
|
||||
// stack buffers.
|
||||
#define MAX_DATA 20
|
||||
|
||||
#define SZ_HEADER 36
|
||||
|
||||
#define FP_OFFSET 6
|
||||
#define FP_SIZE 6
|
||||
|
||||
#define CMD_SET_DATETIME 0x20
|
||||
|
||||
#define CMD_DIVE_COUNT 0x40
|
||||
#define CMD_DIVE_HEADER 0x41
|
||||
#define CMD_DIVE_HEADER_DATA 0x42
|
||||
#define CMD_DIVE_PROFILE 0x43
|
||||
#define CMD_DIVE_PROFILE_DATA 0x44
|
||||
|
||||
#define CMD_SYSTEM_FW 0x58
|
||||
#define CMD_SYSTEM_MAC 0x5A
|
||||
|
||||
#define NSTEPS 1000
|
||||
#define STEP(i,n) (NSTEPS * (i) / (n))
|
||||
|
||||
typedef struct deepblu_cosmiq_device_t {
|
||||
dc_device_t base;
|
||||
dc_iostream_t *iostream;
|
||||
unsigned char fingerprint[FP_SIZE];
|
||||
} deepblu_cosmiq_device_t;
|
||||
|
||||
static dc_status_t deepblu_cosmiq_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size);
|
||||
static dc_status_t deepblu_cosmiq_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, void *userdata);
|
||||
static dc_status_t deepblu_cosmiq_device_timesync(dc_device_t *abstract, const dc_datetime_t *datetime);
|
||||
|
||||
static const dc_device_vtable_t deepblu_cosmiq_device_vtable = {
|
||||
sizeof(deepblu_cosmiq_device_t),
|
||||
DC_FAMILY_DEEPBLU_COSMIQ,
|
||||
deepblu_cosmiq_device_set_fingerprint, /* set_fingerprint */
|
||||
NULL, /* read */
|
||||
NULL, /* write */
|
||||
NULL, /* dump */
|
||||
deepblu_cosmiq_device_foreach, /* foreach */
|
||||
deepblu_cosmiq_device_timesync, /* timesync */
|
||||
NULL, /* close */
|
||||
};
|
||||
|
||||
//
|
||||
// Send a cmd packet.
|
||||
//
|
||||
// The format of the cmd on the "wire" is:
|
||||
// - byte '#'
|
||||
// - HEX char of cmd
|
||||
// - HEX char two's complement modular sum of packet data (including cmd/size)
|
||||
// - HEX char size of data as encoded in HEX
|
||||
// - n * HEX char data
|
||||
// - byte '\n'
|
||||
// so you end up having 8 bytes of header/trailer overhead, and two bytes
|
||||
// for every byte of data sent due to the HEX encoding.
|
||||
//
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_send (deepblu_cosmiq_device_t *device, const unsigned char cmd, const unsigned char data[], size_t size)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
dc_device_t *abstract = (dc_device_t *) device;
|
||||
|
||||
if (size > MAX_DATA)
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
|
||||
if (device_is_cancelled (abstract))
|
||||
return DC_STATUS_CANCELLED;
|
||||
|
||||
// Build the packet.
|
||||
unsigned char csum = ~checksum_add_uint8 (data, size, cmd + 2 * size) + 1;
|
||||
unsigned char raw[3 + MAX_DATA] = {
|
||||
cmd,
|
||||
csum,
|
||||
2 * size
|
||||
};
|
||||
if (size) {
|
||||
memcpy (raw + 3, data, size);
|
||||
}
|
||||
unsigned char packet[1 + 2 * (3 + MAX_DATA) + 1] = {0};
|
||||
packet[0] = '#';
|
||||
array_convert_bin2hex (raw, 3 + size, packet + 1, 2 * (3 + size));
|
||||
packet[1 + 2 * (3 + size)] = '\n';
|
||||
|
||||
HEXDUMP (device->base.context, DC_LOGLEVEL_DEBUG, "cmd", raw, 3 + size);
|
||||
|
||||
// Send the command.
|
||||
status = dc_iostream_write (device->iostream, packet, 2 + 2 * (3 + size), NULL);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (device->base.context, "Failed to send the command.");
|
||||
return status;
|
||||
}
|
||||
|
||||
return status;
|
||||
}
|
||||
|
||||
//
|
||||
// Receive one 'line' of data
|
||||
//
|
||||
// The deepblu BLE protocol is ASCII line based and packetized.
|
||||
// Normally one packet is one line, but it looks like the Nordic
|
||||
// Semi BLE chip will sometimes send packets early (some internal
|
||||
// serial buffer timeout?) with incompete data.
|
||||
//
|
||||
// So read packets until you get newline.
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_recv_line (deepblu_cosmiq_device_t *device, unsigned char data[], size_t size, size_t *actual)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
size_t nbytes = 0;
|
||||
|
||||
while (1) {
|
||||
unsigned char buffer[20] = {0};
|
||||
size_t transferred = 0;
|
||||
|
||||
status = dc_iostream_read (device->iostream, buffer, sizeof(buffer), &transferred);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (device->base.context, "Failed to receive the reply packet.");
|
||||
return status;
|
||||
}
|
||||
|
||||
if (transferred < 1) {
|
||||
ERROR (device->base.context, "Empty reply packet received.");
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
// Append the payload data to the output buffer. If the output
|
||||
// buffer is too small, the error is not reported immediately
|
||||
// but delayed until all packets have been received.
|
||||
if (nbytes < size) {
|
||||
size_t n = transferred;
|
||||
if (nbytes + n > size) {
|
||||
n = size - nbytes;
|
||||
}
|
||||
memcpy(data + nbytes, buffer, n);
|
||||
}
|
||||
nbytes += transferred;
|
||||
|
||||
// Last packet?
|
||||
if (buffer[transferred - 1] == '\n')
|
||||
break;
|
||||
}
|
||||
|
||||
// Verify the expected number of bytes.
|
||||
if (nbytes > size) {
|
||||
ERROR (device->base.context, "Unexpected number of bytes received (" DC_PRINTF_SIZE " " DC_PRINTF_SIZE ").", nbytes, size);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
if (actual)
|
||||
*actual = nbytes;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
//
|
||||
// Receive a reply packet
|
||||
//
|
||||
// The reply packet has the same format as the cmd packet we
|
||||
// send, except the first byte is '$' instead of '#'.
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_recv (deepblu_cosmiq_device_t *device, const unsigned char cmd, unsigned char data[], size_t size, size_t *actual)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
unsigned char packet[1 + 2 * (3 + MAX_DATA) + 1] = {0};
|
||||
|
||||
size_t transferred = 0;
|
||||
status = deepblu_cosmiq_recv_line (device, packet, sizeof(packet), &transferred);
|
||||
if (status != DC_STATUS_SUCCESS)
|
||||
return status;
|
||||
|
||||
if (transferred < 8 || (transferred % 2) != 0) {
|
||||
ERROR (device->base.context, "Unexpected packet length (" DC_PRINTF_SIZE ").", transferred);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
if (packet[0] != '$' || packet[transferred - 1] != '\n') {
|
||||
ERROR (device->base.context, "Unexpected packet start/end byte.");
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
size_t length = transferred - 2;
|
||||
|
||||
unsigned char raw[3 + MAX_DATA] = {0};
|
||||
if (array_convert_hex2bin (packet + 1, length, raw, length / 2) != 0) {
|
||||
ERROR (device->base.context, "Unexpected packet data.");
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
length /= 2;
|
||||
|
||||
HEXDUMP (device->base.context, DC_LOGLEVEL_DEBUG, "rcv", raw, length);
|
||||
|
||||
unsigned char rsp = raw[0];
|
||||
if (rsp != cmd) {
|
||||
ERROR (device->base.context, "Unexpected packet command byte (%02x)", rsp);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
unsigned int n = raw[2];
|
||||
if ((n % 2) != 0 || n != transferred - 8) {
|
||||
ERROR (device->base.context, "Unexpected packet length (%u)", n);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
unsigned char csum = checksum_add_uint8 (raw, length, 0);
|
||||
if (csum != 0) {
|
||||
ERROR (device->base.context, "Unexpected packet checksum (%02x).", csum);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
length -= 3;
|
||||
|
||||
if (length > size) {
|
||||
ERROR (device->base.context, "Unexpected number of bytes received (" DC_PRINTF_SIZE " " DC_PRINTF_SIZE ").", length, size);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
memcpy (data, raw + 3, length);
|
||||
|
||||
if (actual)
|
||||
*actual = length;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_transfer (deepblu_cosmiq_device_t *device, const unsigned char cmd,
|
||||
const unsigned char input[], size_t isize,
|
||||
unsigned char output[], size_t osize)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
|
||||
status = deepblu_cosmiq_send (device, cmd, input, isize);
|
||||
if (status != DC_STATUS_SUCCESS)
|
||||
return status;
|
||||
|
||||
size_t transferred = 0;
|
||||
status = deepblu_cosmiq_recv (device, cmd, output, osize, &transferred);
|
||||
if (status != DC_STATUS_SUCCESS)
|
||||
return status;
|
||||
|
||||
if (transferred != osize) {
|
||||
ERROR (device->base.context, "Unexpected number of bytes received (" DC_PRINTF_SIZE " " DC_PRINTF_SIZE ").", osize, transferred);
|
||||
return DC_STATUS_PROTOCOL;
|
||||
}
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_recv_bulk (deepblu_cosmiq_device_t *device, dc_event_progress_t *progress, const unsigned char cmd, unsigned char data[], size_t size)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
dc_device_t *abstract = (dc_device_t *) device;
|
||||
|
||||
const unsigned int initial = progress ? progress->current : 0;
|
||||
|
||||
size_t nbytes = 0;
|
||||
while (nbytes < size) {
|
||||
size_t transferred = 0;
|
||||
status = deepblu_cosmiq_recv (device, cmd, data + nbytes, size - nbytes, &transferred);
|
||||
if (status != DC_STATUS_SUCCESS)
|
||||
return status;
|
||||
|
||||
nbytes += transferred;
|
||||
|
||||
// Update and emit a progress event.
|
||||
if (progress) {
|
||||
progress->current = initial + STEP(nbytes, size);
|
||||
device_event_emit (abstract, DC_EVENT_PROGRESS, progress);
|
||||
}
|
||||
}
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
dc_status_t
|
||||
deepblu_cosmiq_device_open (dc_device_t **out, dc_context_t *context, dc_iostream_t *iostream)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
deepblu_cosmiq_device_t *device = NULL;
|
||||
|
||||
if (out == NULL)
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
|
||||
// Allocate memory.
|
||||
device = (deepblu_cosmiq_device_t *) dc_device_allocate (context, &deepblu_cosmiq_device_vtable);
|
||||
if (device == NULL) {
|
||||
ERROR (context, "Failed to allocate memory.");
|
||||
return DC_STATUS_NOMEMORY;
|
||||
}
|
||||
|
||||
// Set the default values.
|
||||
device->iostream = iostream;
|
||||
memset (device->fingerprint, 0, sizeof(device->fingerprint));
|
||||
|
||||
// Set the timeout for receiving data (1000ms).
|
||||
status = dc_iostream_set_timeout (device->iostream, 1000);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (context, "Failed to set the timeout.");
|
||||
goto error_free;
|
||||
}
|
||||
|
||||
dc_iostream_purge (device->iostream, DC_DIRECTION_ALL);
|
||||
|
||||
*out = (dc_device_t *) device;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
|
||||
error_free:
|
||||
dc_device_deallocate ((dc_device_t *) device);
|
||||
return status;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size)
|
||||
{
|
||||
deepblu_cosmiq_device_t *device = (deepblu_cosmiq_device_t *)abstract;
|
||||
|
||||
if (size && size != sizeof (device->fingerprint))
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
|
||||
if (size)
|
||||
memcpy (device->fingerprint, data, sizeof (device->fingerprint));
|
||||
else
|
||||
memset (device->fingerprint, 0, sizeof (device->fingerprint));
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, void *userdata)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
deepblu_cosmiq_device_t *device = (deepblu_cosmiq_device_t *) abstract;
|
||||
const unsigned char zero = 0;
|
||||
|
||||
// Enable progress notifications.
|
||||
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
|
||||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||||
|
||||
// Read the firmware version.
|
||||
unsigned char fw[1] = {0};
|
||||
status = deepblu_cosmiq_transfer (device, CMD_SYSTEM_FW, &zero, 1, fw, sizeof(fw));
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the firmware version.");
|
||||
goto error_exit;
|
||||
}
|
||||
|
||||
// Read the MAC address.
|
||||
unsigned char mac[6] = {0};
|
||||
status = deepblu_cosmiq_transfer (device, CMD_SYSTEM_MAC, &zero, 1, mac, sizeof(mac));
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the MAC address.");
|
||||
goto error_exit;
|
||||
}
|
||||
|
||||
// Emit a device info event.
|
||||
dc_event_devinfo_t devinfo;
|
||||
devinfo.model = 0;
|
||||
devinfo.firmware = fw[0] & 0x3F;
|
||||
devinfo.serial = array_uint32_le (mac);
|
||||
device_event_emit (abstract, DC_EVENT_DEVINFO, &devinfo);
|
||||
|
||||
unsigned char ndives = 0;
|
||||
status = deepblu_cosmiq_transfer (device, CMD_DIVE_COUNT, &zero, 1, &ndives, 1);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the number of dives.");
|
||||
goto error_exit;
|
||||
}
|
||||
|
||||
// Update and emit a progress event.
|
||||
progress.current = (ndives == 0 ? 1 : 0) * NSTEPS;
|
||||
progress.maximum = (ndives + 1) * NSTEPS;
|
||||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||||
|
||||
if (ndives == 0) {
|
||||
status = DC_STATUS_SUCCESS;
|
||||
goto error_exit;
|
||||
}
|
||||
|
||||
unsigned char *headers = malloc (ndives * SZ_HEADER);
|
||||
if (headers == NULL) {
|
||||
ERROR (abstract->context, "Failed to allocate memory.");
|
||||
status = DC_STATUS_NOMEMORY;
|
||||
goto error_exit;
|
||||
}
|
||||
|
||||
unsigned int count = 0;
|
||||
for (unsigned int i = 0; i < ndives; ++i) {
|
||||
unsigned char number = i + 1;
|
||||
|
||||
unsigned char len = 0;
|
||||
status = deepblu_cosmiq_transfer (device, CMD_DIVE_HEADER, &number, 1, &len, 1);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the dive header.");
|
||||
goto error_free_headers;
|
||||
}
|
||||
|
||||
if (len != SZ_HEADER) {
|
||||
status = DC_STATUS_PROTOCOL;
|
||||
goto error_free_headers;
|
||||
}
|
||||
|
||||
status = deepblu_cosmiq_recv_bulk (device, NULL, CMD_DIVE_HEADER_DATA, headers + i * SZ_HEADER, SZ_HEADER);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the dive header.");
|
||||
goto error_free_headers;
|
||||
}
|
||||
|
||||
// Update and emit a progress event.
|
||||
progress.current = STEP(i + 1, ndives);
|
||||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||||
|
||||
if (memcmp (headers + i * SZ_HEADER + FP_OFFSET, device->fingerprint, sizeof (device->fingerprint)) == 0)
|
||||
break;
|
||||
|
||||
count++;
|
||||
}
|
||||
|
||||
// Update and emit a progress event.
|
||||
progress.current = 1 * NSTEPS;
|
||||
progress.maximum = (count + 1) * NSTEPS;
|
||||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||||
|
||||
// Allocate memory for the dive.
|
||||
dc_buffer_t *dive = dc_buffer_new (4096);
|
||||
if (dive == NULL) {
|
||||
ERROR (abstract->context, "Failed to allocate memory.");
|
||||
status = DC_STATUS_NOMEMORY;
|
||||
goto error_free_headers;
|
||||
}
|
||||
|
||||
for (unsigned int i = 0; i < count; ++i) {
|
||||
unsigned char number = i + 1;
|
||||
|
||||
unsigned char rsp_len[2] = {0};
|
||||
status = deepblu_cosmiq_transfer (device, CMD_DIVE_PROFILE, &number, 1, rsp_len, sizeof(rsp_len));
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the dive profile.");
|
||||
goto error_free_dive;
|
||||
}
|
||||
|
||||
unsigned int len = array_uint16_be (rsp_len);
|
||||
|
||||
// Erase the buffer.
|
||||
dc_buffer_clear (dive);
|
||||
|
||||
// Allocate memory for the dive.
|
||||
if (!dc_buffer_resize (dive, len + SZ_HEADER)) {
|
||||
ERROR (abstract->context, "Failed to allocate memory.");
|
||||
status = DC_STATUS_NOMEMORY;
|
||||
goto error_free_dive;
|
||||
}
|
||||
|
||||
// Cache the pointer.
|
||||
unsigned char *data = dc_buffer_get_data (dive);
|
||||
|
||||
memcpy (data, headers + i * SZ_HEADER, SZ_HEADER);
|
||||
|
||||
status = deepblu_cosmiq_recv_bulk (device, &progress, CMD_DIVE_PROFILE_DATA, data + SZ_HEADER, len);
|
||||
if (status != DC_STATUS_SUCCESS) {
|
||||
ERROR (abstract->context, "Failed to read the dive profile.");
|
||||
goto error_free_dive;
|
||||
}
|
||||
|
||||
if (callback && !callback (data, len + SZ_HEADER, data + FP_OFFSET, FP_SIZE, userdata))
|
||||
break;
|
||||
}
|
||||
|
||||
error_free_dive:
|
||||
dc_buffer_free (dive);
|
||||
error_free_headers:
|
||||
free (headers);
|
||||
error_exit:
|
||||
return status;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_device_timesync (dc_device_t *abstract, const dc_datetime_t *datetime)
|
||||
{
|
||||
dc_status_t status = DC_STATUS_SUCCESS;
|
||||
deepblu_cosmiq_device_t *device = (deepblu_cosmiq_device_t *) abstract;
|
||||
|
||||
if (datetime->year < 2000) {
|
||||
ERROR (abstract->context, "Invalid date/time value specified.");
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
}
|
||||
|
||||
const unsigned char cmd[6] = {
|
||||
dec2bcd(datetime->year - 2000),
|
||||
dec2bcd(datetime->month),
|
||||
dec2bcd(datetime->day),
|
||||
dec2bcd(datetime->hour),
|
||||
dec2bcd(datetime->minute),
|
||||
dec2bcd(datetime->second)};
|
||||
unsigned char rsp[1] = {0};
|
||||
status = deepblu_cosmiq_transfer (device, CMD_SET_DATETIME,
|
||||
cmd, sizeof(cmd), rsp, sizeof(rsp));
|
||||
if (status != DC_STATUS_SUCCESS)
|
||||
return status;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
44
src/deepblu_cosmiq.h
Normal file
44
src/deepblu_cosmiq.h
Normal file
@ -0,0 +1,44 @@
|
||||
/*
|
||||
* libdivecomputer
|
||||
*
|
||||
* Copyright (C) 2018 Linus Torvalds
|
||||
* Copyright (C) 2022 Jef Driesen
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#ifndef DEEPBLU_COSMIQ_H
|
||||
#define DEEPBLU_COSMIQ_H
|
||||
|
||||
#include <libdivecomputer/context.h>
|
||||
#include <libdivecomputer/iostream.h>
|
||||
#include <libdivecomputer/device.h>
|
||||
#include <libdivecomputer/parser.h>
|
||||
|
||||
#ifdef __cplusplus
|
||||
extern "C" {
|
||||
#endif /* __cplusplus */
|
||||
|
||||
dc_status_t
|
||||
deepblu_cosmiq_device_open (dc_device_t **device, dc_context_t *context, dc_iostream_t *iostream);
|
||||
|
||||
dc_status_t
|
||||
deepblu_cosmiq_parser_create (dc_parser_t **parser, dc_context_t *context);
|
||||
|
||||
#ifdef __cplusplus
|
||||
}
|
||||
#endif /* __cplusplus */
|
||||
#endif /* DEEPBLU_COSMIQ_H */
|
||||
218
src/deepblu_cosmiq_parser.c
Normal file
218
src/deepblu_cosmiq_parser.c
Normal file
@ -0,0 +1,218 @@
|
||||
/*
|
||||
* libdivecomputer
|
||||
*
|
||||
* Copyright (C) 2019 Linus Torvalds
|
||||
* Copyright (C) 2022 Jef Driesen
|
||||
*
|
||||
* This library is free software; you can redistribute it and/or
|
||||
* modify it under the terms of the GNU Lesser General Public
|
||||
* License as published by the Free Software Foundation; either
|
||||
* version 2.1 of the License, or (at your option) any later version.
|
||||
*
|
||||
* This library is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
* Lesser General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU Lesser General Public
|
||||
* License along with this library; if not, write to the Free Software
|
||||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||||
* MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
#include <stdlib.h>
|
||||
|
||||
#include <libdivecomputer/units.h>
|
||||
|
||||
#include "deepblu_cosmiq.h"
|
||||
#include "context-private.h"
|
||||
#include "parser-private.h"
|
||||
#include "array.h"
|
||||
|
||||
#define SCUBA 2
|
||||
#define GAUGE 3
|
||||
#define FREEDIVE 4
|
||||
|
||||
#define SZ_HEADER 36
|
||||
#define SZ_SAMPLE 4
|
||||
|
||||
typedef struct deepblu_cosmiq_parser_t {
|
||||
dc_parser_t base;
|
||||
double hydrostatic;
|
||||
} deepblu_cosmiq_parser_t;
|
||||
|
||||
static dc_status_t deepblu_cosmiq_parser_set_data (dc_parser_t *abstract, const unsigned char *data, unsigned int size);
|
||||
static dc_status_t deepblu_cosmiq_parser_set_density (dc_parser_t *abstract, double density);
|
||||
static dc_status_t deepblu_cosmiq_parser_get_datetime (dc_parser_t *abstract, dc_datetime_t *datetime);
|
||||
static dc_status_t deepblu_cosmiq_parser_get_field (dc_parser_t *abstract, dc_field_type_t type, unsigned int flags, void *value);
|
||||
static dc_status_t deepblu_cosmiq_parser_samples_foreach (dc_parser_t *abstract, dc_sample_callback_t callback, void *userdata);
|
||||
|
||||
static const dc_parser_vtable_t deepblu_cosmiq_parser_vtable = {
|
||||
sizeof(deepblu_cosmiq_parser_t),
|
||||
DC_FAMILY_DEEPBLU_COSMIQ,
|
||||
deepblu_cosmiq_parser_set_data, /* set_data */
|
||||
NULL, /* set_clock */
|
||||
NULL, /* set_atmospheric */
|
||||
deepblu_cosmiq_parser_set_density, /* set_density */
|
||||
deepblu_cosmiq_parser_get_datetime, /* datetime */
|
||||
deepblu_cosmiq_parser_get_field, /* fields */
|
||||
deepblu_cosmiq_parser_samples_foreach, /* samples_foreach */
|
||||
NULL /* destroy */
|
||||
};
|
||||
|
||||
dc_status_t
|
||||
deepblu_cosmiq_parser_create (dc_parser_t **out, dc_context_t *context)
|
||||
{
|
||||
deepblu_cosmiq_parser_t *parser = NULL;
|
||||
|
||||
if (out == NULL)
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
|
||||
// Allocate memory.
|
||||
parser = (deepblu_cosmiq_parser_t *) dc_parser_allocate (context, &deepblu_cosmiq_parser_vtable);
|
||||
if (parser == NULL) {
|
||||
ERROR (context, "Failed to allocate memory.");
|
||||
return DC_STATUS_NOMEMORY;
|
||||
}
|
||||
|
||||
// Set the default values.
|
||||
parser->hydrostatic = DEF_DENSITY_SALT * GRAVITY;
|
||||
|
||||
*out = (dc_parser_t *) parser;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_parser_set_data (dc_parser_t *abstract, const unsigned char *data, unsigned int size)
|
||||
{
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_parser_set_density (dc_parser_t *abstract, double density)
|
||||
{
|
||||
deepblu_cosmiq_parser_t *parser = (deepblu_cosmiq_parser_t *) abstract;
|
||||
|
||||
parser->hydrostatic = density * GRAVITY;
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_parser_get_datetime (dc_parser_t *abstract, dc_datetime_t *datetime)
|
||||
{
|
||||
const unsigned char *data = abstract->data;
|
||||
unsigned int size = abstract->size;
|
||||
|
||||
if (size < SZ_HEADER)
|
||||
return DC_STATUS_DATAFORMAT;
|
||||
|
||||
if (datetime) {
|
||||
datetime->year = array_uint16_le(data + 6);
|
||||
datetime->day = data[8];
|
||||
datetime->month = data[9];
|
||||
datetime->minute = data[10];
|
||||
datetime->hour = data[11];
|
||||
datetime->second = 0;
|
||||
datetime->timezone = DC_TIMEZONE_NONE;
|
||||
}
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_parser_get_field (dc_parser_t *abstract, dc_field_type_t type, unsigned int flags, void *value)
|
||||
{
|
||||
deepblu_cosmiq_parser_t *parser = (deepblu_cosmiq_parser_t *) abstract;
|
||||
const unsigned char *data = abstract->data;
|
||||
unsigned int size = abstract->size;
|
||||
|
||||
dc_gasmix_t *gasmix = (dc_gasmix_t *) value;
|
||||
|
||||
if (size < SZ_HEADER)
|
||||
return DC_STATUS_DATAFORMAT;
|
||||
|
||||
unsigned int mode = data[2];
|
||||
unsigned int atmospheric = array_uint16_le (data + 4) & 0x1FFF;
|
||||
|
||||
if (value) {
|
||||
switch (type) {
|
||||
case DC_FIELD_DIVETIME:
|
||||
if (mode == SCUBA || mode == GAUGE)
|
||||
*((unsigned int *) value) = array_uint16_le(data + 12) * 60;
|
||||
else
|
||||
*((unsigned int *) value) = array_uint16_le(data + 12);
|
||||
break;
|
||||
case DC_FIELD_MAXDEPTH:
|
||||
*((double *) value) = (signed int) (array_uint16_le (data + 22) - atmospheric) * (BAR / 1000.0) / parser->hydrostatic;
|
||||
break;
|
||||
case DC_FIELD_GASMIX_COUNT:
|
||||
*((unsigned int *) value) = mode == SCUBA;
|
||||
break;
|
||||
case DC_FIELD_GASMIX:
|
||||
gasmix->oxygen = data[3] / 100.0;
|
||||
gasmix->helium = 0.0;
|
||||
gasmix->nitrogen = 1.0 - gasmix->oxygen - gasmix->helium;
|
||||
break;
|
||||
case DC_FIELD_DIVEMODE:
|
||||
switch (mode) {
|
||||
case SCUBA:
|
||||
*((dc_divemode_t *) value) = DC_DIVEMODE_OC;
|
||||
break;
|
||||
case GAUGE:
|
||||
*((dc_divemode_t *) value) = DC_DIVEMODE_GAUGE;
|
||||
break;
|
||||
case FREEDIVE:
|
||||
*((dc_divemode_t *) value) = DC_DIVEMODE_FREEDIVE;
|
||||
break;
|
||||
default:
|
||||
ERROR (abstract->context, "Unknown activity type '%02x'", mode);
|
||||
return DC_STATUS_DATAFORMAT;
|
||||
}
|
||||
break;
|
||||
case DC_FIELD_ATMOSPHERIC:
|
||||
*((double *) value) = atmospheric / 1000.0;
|
||||
break;
|
||||
default:
|
||||
return DC_STATUS_UNSUPPORTED;
|
||||
}
|
||||
}
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
|
||||
static dc_status_t
|
||||
deepblu_cosmiq_parser_samples_foreach (dc_parser_t *abstract, dc_sample_callback_t callback, void *userdata)
|
||||
{
|
||||
deepblu_cosmiq_parser_t *parser = (deepblu_cosmiq_parser_t *) abstract;
|
||||
const unsigned char *data = abstract->data;
|
||||
unsigned int size = abstract->size;
|
||||
|
||||
if (size < SZ_HEADER)
|
||||
return DC_STATUS_DATAFORMAT;
|
||||
|
||||
unsigned int interval = data[26];
|
||||
unsigned int atmospheric = array_uint16_le (data + 4) & 0x1FFF;
|
||||
|
||||
unsigned int time = 0;
|
||||
unsigned int offset = SZ_HEADER;
|
||||
while (offset + SZ_SAMPLE <= size) {
|
||||
dc_sample_value_t sample = {0};
|
||||
unsigned int temperature = array_uint16_le(data + offset + 0);
|
||||
unsigned int depth = array_uint16_le(data + offset + 2);
|
||||
offset += SZ_SAMPLE;
|
||||
|
||||
time += interval;
|
||||
sample.time = time;
|
||||
if (callback) callback (DC_SAMPLE_TIME, sample, userdata);
|
||||
|
||||
sample.depth = (signed int) (depth - atmospheric) * (BAR / 1000.0) / parser->hydrostatic;
|
||||
if (callback) callback (DC_SAMPLE_DEPTH, sample, userdata);
|
||||
|
||||
sample.temperature = temperature / 10.0;
|
||||
if (callback) callback (DC_SAMPLE_TEMPERATURE, sample, userdata);
|
||||
}
|
||||
|
||||
return DC_STATUS_SUCCESS;
|
||||
}
|
||||
@ -63,6 +63,7 @@ static int dc_filter_oceanic (dc_transport_t transport, const void *userdata, vo
|
||||
static int dc_filter_mclean (dc_transport_t transport, const void *userdata, void *params);
|
||||
static int dc_filter_atomic (dc_transport_t transport, const void *userdata, void *params);
|
||||
static int dc_filter_deepsix (dc_transport_t transport, const void *userdata, void *params);
|
||||
static int dc_filter_deepblu (dc_transport_t transport, const void *userdata, void *params);
|
||||
|
||||
static dc_status_t dc_descriptor_iterator_next (dc_iterator_t *iterator, void *item);
|
||||
|
||||
@ -445,6 +446,8 @@ static const dc_descriptor_t g_descriptors[] = {
|
||||
/* Seac Screen */
|
||||
{"Seac", "Screen", DC_FAMILY_SEAC_SCREEN, 0, DC_TRANSPORT_SERIAL, NULL},
|
||||
{"Seac", "Action", DC_FAMILY_SEAC_SCREEN, 0, DC_TRANSPORT_SERIAL, NULL},
|
||||
/* Deepblu Cosmiq */
|
||||
{"Deepblu", "Cosmiq+", DC_FAMILY_DEEPBLU_COSMIQ, 0, DC_TRANSPORT_BLE, dc_filter_deepblu},
|
||||
};
|
||||
|
||||
static int
|
||||
@ -766,6 +769,18 @@ static int dc_filter_deepsix (dc_transport_t transport, const void *userdata, vo
|
||||
return 1;
|
||||
}
|
||||
|
||||
static int dc_filter_deepblu (dc_transport_t transport, const void *userdata, void *params)
|
||||
{
|
||||
static const char * const bluetooth[] = {
|
||||
"COSMIQ",
|
||||
};
|
||||
|
||||
if (transport == DC_TRANSPORT_BLE) {
|
||||
return DC_FILTER_INTERNAL (userdata, bluetooth, 0, dc_match_name);
|
||||
}
|
||||
|
||||
return 1;
|
||||
}
|
||||
|
||||
dc_status_t
|
||||
dc_descriptor_iterator (dc_iterator_t **out)
|
||||
|
||||
@ -62,6 +62,7 @@
|
||||
#include "sporasub_sp2.h"
|
||||
#include "deepsix_excursion.h"
|
||||
#include "seac_screen.h"
|
||||
#include "deepblu_cosmiq.h"
|
||||
|
||||
#include "device-private.h"
|
||||
#include "context-private.h"
|
||||
@ -231,6 +232,9 @@ dc_device_open (dc_device_t **out, dc_context_t *context, dc_descriptor_t *descr
|
||||
case DC_FAMILY_SEAC_SCREEN:
|
||||
rc = seac_screen_device_open (&device, context, iostream);
|
||||
break;
|
||||
case DC_FAMILY_DEEPBLU_COSMIQ:
|
||||
rc = deepblu_cosmiq_device_open (&device, context, iostream);
|
||||
break;
|
||||
default:
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
}
|
||||
|
||||
@ -61,6 +61,7 @@
|
||||
#include "sporasub_sp2.h"
|
||||
#include "deepsix_excursion.h"
|
||||
#include "seac_screen.h"
|
||||
#include "deepblu_cosmiq.h"
|
||||
|
||||
#include "context-private.h"
|
||||
#include "parser-private.h"
|
||||
@ -191,6 +192,9 @@ dc_parser_new_internal (dc_parser_t **out, dc_context_t *context, dc_family_t fa
|
||||
case DC_FAMILY_SEAC_SCREEN:
|
||||
rc = seac_screen_parser_create (&parser, context);
|
||||
break;
|
||||
case DC_FAMILY_DEEPBLU_COSMIQ:
|
||||
rc = deepblu_cosmiq_parser_create (&parser, context);
|
||||
break;
|
||||
default:
|
||||
return DC_STATUS_INVALIDARGS;
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user