nigreon/btgps
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Initial commit

Browse source
This commit is contained in:
Nigreon 2024-07-21 01:32:01 +02:00
commit 19d38903cf
29 changed files with 4984 additions and 0 deletions
Download patch file Download diff file Expand all files Collapse all files

218
src/battery.c Normal file
View file

@ -0,0 +1,218 @@
/*
* Copyright 2024 Marcus Alexander Tjomsaas
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include "battery.h"
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/drivers/adc.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(battery);
#define GPIO_BATTERY_CHARGE_SPEED 13
#define GPIO_BATTERY_CHARGING_ENABLE 17
#define GPIO_BATTERY_READ_ENABLE 14
static K_MUTEX_DEFINE(battery_mut);
// Change this to a higher number for better averages
// Note that increasing this holds up the thread / ADC for longer.
#define ADC_TOTAL_SAMPLES 10
//--------------------------------------------------------------
// ADC setup
#define ADC_RESOLUTION 12
#define ADC_CHANNEL 7
#define ADC_PORT SAADC_CH_PSELP_PSELP_AnalogInput7 // AIN7
#define ADC_REFERENCE ADC_REF_INTERNAL // 0.6V
#define ADC_GAIN ADC_GAIN_1_6 // ADC REFERENCE * 6 = 3.6V
#define ADC_SAMPLE_INTERVAL_US 500 // Time between each sample
static struct adc_channel_cfg channel_7_cfg = {
.gain = ADC_GAIN,
.reference = ADC_REFERENCE,
.acquisition_time = ADC_ACQ_TIME_DEFAULT,
.channel_id = ADC_CHANNEL,
#ifdef CONFIG_ADC_NRFX_SAADC
.input_positive = ADC_PORT
#endif
};
static struct adc_sequence_options options = {
.extra_samplings = ADC_TOTAL_SAMPLES - 1,
.interval_us = ADC_SAMPLE_INTERVAL_US,
};
static int16_t sample_buffer[ADC_TOTAL_SAMPLES];
static struct adc_sequence sequence = {
.options = &options,
.channels = BIT(ADC_CHANNEL),
.buffer = sample_buffer,
.buffer_size = sizeof(sample_buffer),
.resolution = ADC_RESOLUTION,
};
//--------------------------------------------------------------
// MCU peripherals for reading battery voltage
static const struct device *gpio_battery_dev = DEVICE_DT_GET(DT_NODELABEL(gpio0));
static const struct device *adc_battery_dev = DEVICE_DT_GET(DT_NODELABEL(adc));
typedef struct
{
uint16_t voltage;
uint8_t percentage;
} BatteryState;
#define BATTERY_STATES_COUNT 12
// Assuming LiPo battery.
// For better accuracy, use your battery's datasheet.
static BatteryState battery_states[BATTERY_STATES_COUNT] = {
{4200, 100},
{4160, 99},
{4090, 91},
{4030, 78},
{3890, 63},
{3830, 53},
{3680, 36},
{3660, 35},
{3480, 14},
{3420, 11},
{3150, 1}, // 3240
{0000, 0} // Below safe level
};
//------------------------------------------------------------------------------------------
// Public functions
int battery_get_millivolt(uint16_t *battery_millivolt)
{
int ret = 0;
// Voltage divider circuit (Should tune R1 in software if possible)
const uint16_t R1 = 1037; // Originally 1M ohm, calibrated after measuring actual voltage values. Can happen due to resistor tolerances, temperature ect..
const uint16_t R2 = 510; // 510K ohm
// ADC measure
uint16_t adc_vref = adc_ref_internal(adc_battery_dev);
int adc_mv = 0;
ret |= gpio_pin_configure(gpio_battery_dev, GPIO_BATTERY_READ_ENABLE, GPIO_OUTPUT | GPIO_ACTIVE_LOW);
ret |= gpio_pin_set(gpio_battery_dev, GPIO_BATTERY_READ_ENABLE, 1);
k_msleep(100);
k_mutex_lock(&battery_mut, K_FOREVER);
ret |= adc_read(adc_battery_dev, &sequence);
if (ret)
{
LOG_WRN("ADC read failed (error %d)", ret);
}
// Get average sample value.
for (uint8_t sample = 0; sample < ADC_TOTAL_SAMPLES; sample++)
{
adc_mv += sample_buffer[sample]; // ADC value, not millivolt yet.
}
adc_mv /= ADC_TOTAL_SAMPLES;
// Convert ADC value to millivolts
ret |= adc_raw_to_millivolts(adc_vref, ADC_GAIN, ADC_RESOLUTION, &adc_mv);
// Calculate battery voltage.
*battery_millivolt = adc_mv * ((R1 + R2) / R2);
k_mutex_unlock(&battery_mut);
ret |= gpio_pin_configure(gpio_battery_dev, GPIO_BATTERY_READ_ENABLE, GPIO_DISCONNECTED);
LOG_INF("%d mV", *battery_millivolt);
return ret;
}
int battery_get_percentage(uint8_t *battery_percentage, uint16_t battery_millivolt)
{
// Ensure voltage is within bounds
if (battery_millivolt > battery_states[0].voltage)
*battery_percentage = 100;
if (battery_millivolt < battery_states[BATTERY_STATES_COUNT - 1].voltage)
*battery_percentage = 0;
for (uint16_t i = 0; i < BATTERY_STATES_COUNT - 1; i++)
{
// Find the two points battery_millivolt is between
if (battery_states[i].voltage >= battery_millivolt && battery_millivolt >= battery_states[i + 1].voltage)
{
// Linear interpolation
*battery_percentage = battery_states[i].percentage +
((float)(battery_millivolt - battery_states[i].voltage) *
((float)(battery_states[i + 1].percentage - battery_states[i].percentage) /
(float)(battery_states[i + 1].voltage - battery_states[i].voltage)));
LOG_INF("%d %%", *battery_percentage);
return 0;
}
}
return -ESPIPE;
}
int battery_init()
{
int ret = 0;
// ADC
if (!device_is_ready(adc_battery_dev))
{
LOG_ERR("ADC device not found!");
return -EIO;
}
ret |= adc_channel_setup(adc_battery_dev, &channel_7_cfg);
if (ret)
{
LOG_ERR("ADC setup failed (error %d)", ret);
return ret;
}
// GPIO
if (!device_is_ready(gpio_battery_dev))
{
LOG_ERR("GPIO device not found!");
return -EIO;
}
ret |= gpio_pin_configure(gpio_battery_dev, GPIO_BATTERY_CHARGING_ENABLE, GPIO_DISCONNECTED);
ret |= gpio_pin_configure(gpio_battery_dev, GPIO_BATTERY_READ_ENABLE, GPIO_DISCONNECTED);
ret |= gpio_pin_configure(gpio_battery_dev, GPIO_BATTERY_CHARGE_SPEED, GPIO_DISCONNECTED);
if (ret)
{
LOG_ERR("GPIO configure failed!");
return ret;
}
LOG_INF("Initialized");
// Get ready for battery charging and sampling
return ret;
}

50
src/battery.h Normal file
View file

@ -0,0 +1,50 @@
/*
* Copyright 2024 Marcus Alexander Tjomsaas
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*/
#include <stdint.h>
#include <stdbool.h>
#ifndef __BATTERY_H__
#define __BATTERY_H__
/**
* @brief Calculates the battery voltage using the ADC.
*
* @param[in] battery_millivolt Pointer to where battery voltage is stored.
*
* @retval 0 if successful. Negative errno number on error.
*/
int battery_get_millivolt(uint16_t *battery_millivolt);
/**
* @brief Calculates the battery percentage using the battery voltage.
*
* @param[in] battery_percentage Pointer to where battery percentage is stored.
*
* @param[in] battery_millivolt Voltage used to calculate the percentage of how much energy is left in a 3.7V LiPo battery.
*
* @retval 0 if successful. Negative errno number on error.
*/
int battery_get_percentage(uint8_t *battery_percentage, uint16_t battery_millivolt);
/**
* @brief Initialize the battery charging circuit.
*
* @retval 0 if successful. Negative errno number on error.
*/
int battery_init(void);
#endif

76
src/bme280.c Normal file
View file

@ -0,0 +1,76 @@
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include "bme280.h"
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(bme280);
const struct device *bme280;
#define CALIB_TEMP_VAL1 0
#define CALIB_PRES_VAL1 1
//#define CALIB_HUMI_VAL1 0
#define CALIB_TEMP_VAL2 0
#define CALIB_PRES_VAL2 500000
//#define CALIB_HUMI_VAL2 0
void init_bme280()
{
bme280 = DEVICE_DT_GET_ANY(bosch_bme280);
if (bme280 == NULL) {
/* No such node, or the node does not have status "okay". */
LOG_ERR("Error: no device found.\n");
return;
}
if (!device_is_ready(bme280)) {
LOG_ERR("Error: Device \"%s\" is not ready; ", bme280->name);
return;
}
LOG_WRN("Found device \"%s\"", bme280->name);
}
void probe_bme280(bool ptemperature, bool phumidity, bool ppressure)
{
if(ptemperature == true && ppressure == false)
{
sensor_sample_fetch_chan(bme280, SENSOR_CHAN_AMBIENT_TEMP);
//} else if(phumidity == true && ppressure == false) {
// sensor_sample_fetch_chan(bme280, SENSOR_CHAN_HUMIDITY);
} else {
sensor_sample_fetch_chan(bme280, SENSOR_CHAN_ALL);
}
}
struct sensor_value get_temperature()
{
struct sensor_value sval;
sensor_channel_get(bme280, SENSOR_CHAN_AMBIENT_TEMP, &sval);
sval.val1 = sval.val1 + CALIB_TEMP_VAL1;
sval.val2 = sval.val2 + CALIB_TEMP_VAL2;
return sval;
}
struct sensor_value get_pressure()
{
struct sensor_value sval;
sensor_channel_get(bme280, SENSOR_CHAN_PRESS, &sval);
sval.val1 = sval.val1 + CALIB_PRES_VAL1;
sval.val2 = sval.val2 + CALIB_PRES_VAL2;
return sval;
}
/* struct sensor_value get_humidity()
{
struct sensor_value sval;
sensor_channel_get(bme280, SENSOR_CHAN_HUMIDITY, &sval);
sval.val1 = sval.val1 + CALIB_HUMI_VAL1;
sval.val2 = sval.val2 + CALIB_HUMI_VAL2;
return sval;
} */

8
src/bme280.h Normal file
View file

@ -0,0 +1,8 @@
#include <zephyr/types.h>
#include <zephyr/drivers/sensor.h>
void init_bme280();
void probe_bme280(bool ptemperature, bool phumidity, bool ppressure);
struct sensor_value get_temperature();
struct sensor_value get_pressure();
//struct sensor_value get_humidity();

27
src/calendar.c Normal file
View file

@ -0,0 +1,27 @@
#include "calendar.h"
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(calendar);
//uint32_t timeoffset = 0xff000078;
uint32_t timeoffset = 0x00000000;
void setTs(uint32_t ts)
{
LOG_INF("setTs: %d", ts);
timeoffset = ts-(k_uptime_get()/1000);
}
uint32_t getTs()
{
uint32_t ts = (k_uptime_get()/1000) + timeoffset;
// LOG_INF("getTs: %d", ts);
return ts;
}
bool TsOK()
{
return timeoffset != 0;
}

6
src/calendar.h Normal file
View file

@ -0,0 +1,6 @@
#include <zephyr/kernel.h>
void setTs(uint32_t ts);
uint32_t getTs();
bool TsOK();

440
src/gps.c Normal file
View file

@ -0,0 +1,440 @@
/*
* Copyright (c) 2022 Libre Solar Technologies GmbH
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/drivers/uart.h>
#include <string.h>
#include <time.h>
#include <zephyr/sys/timeutil.h>
//#include <SEGGER_RTT.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(gps);
//LOG_MODULE_REGISTER(gps, LOG_LEVEL_DBG);
#include "lwgps.h"
#include "gps.h"
#include "calendar.h"
#define MY_STACK_SIZE 2048
#define MY_PRIORITY 5
K_FIFO_DEFINE(fifo_gps_sat_data);
K_FIFO_DEFINE(fifo_gps_data);
K_SEM_DEFINE(gps_data_tosend, 0, 1);
K_SEM_DEFINE(gps_init_ok, 0, 1);
/*#define RTT_BUFFER_DOWN_SIZE 512
#define RTT_BUFFER_UP_SIZE 512
struct rtt_config_t {
void *up_buffer;
size_t up_size;
void *down_buffer;
size_t down_size;
uint8_t channel;
};*/
/* change this to any other UART peripheral if desired */
#define UART_DEVICE_NODE DT_NODELABEL(uart0)
#define MSG_SIZE 128
/* queue to store up to 10 messages (aligned to 4-byte boundary) */
K_MSGQ_DEFINE(uart_msgq, MSG_SIZE, 10, 4);
static K_SEM_DEFINE(segger_init_ok, 0, 1);
static const struct device *const uart_dev = DEVICE_DT_GET(UART_DEVICE_NODE);
static char nmea_sentence[MSG_SIZE];
static int nmea_position = 0;
static bool gps_state = false;
const unsigned char ubxNMEA[] = { 0x06, 0x17, 0x14, 0x00, 0x00, 0x41, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char ubxGNSS[] = { 0x06, 0x3E, 0x3C, 0x00, 0x00, 0x20, 0x20, 0x07, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x03, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x05, 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 };
//const unsigned char ubxPoweroff[] = { 0x02, 0x41, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00 };
const unsigned char ubxEnablePulse[] = { 0x06, 0x31, 0x20, 0x00, 0x00, 0x01, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x40, 0x42, 0x0f, 0x00, 0x40, 0x42, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa0, 0x86, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x77, 0x00, 0x00, 0x00};
const unsigned char ubxDisablePulse[] = { 0x06, 0x31, 0x20, 0x00, 0x00, 0x01, 0x00, 0x00, 0x32, 0x00, 0x00, 0x00, 0x40, 0x42, 0x0f, 0x00, 0x40, 0x42, 0x0f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x77, 0x00, 0x00, 0x00};
const unsigned char GNSSStart[] = { 0x06,0x04,0x04,0x00,0x00,0x00,0x09,0x00 };
const unsigned char GNSSStop[] = { 0x06,0x04,0x04,0x00,0x00,0x00,0x08,0x00 };
const unsigned char enablePUBX0[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
const unsigned char disablePUBX0[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char enablePUBX4[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x04, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
const unsigned char disablePUBX4[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char rate2Hz2Mes[] = { 0x06, 0x08, 0x06, 0x00, 0xf4, 0x02, 0x02, 0x00, 0x00, 0x00 };
const unsigned char rate1Hz1Mes[] = { 0x06, 0x08, 0x06, 0x00, 0xe8, 0x03, 0x01, 0x00, 0x00, 0x00 };
const unsigned char rate1Hz5Mes[] = { 0x06, 0x08, 0x06, 0x00, 0xe8, 0x03, 0x05, 0x00, 0x00, 0x00 };
const char disableRMC[] = "PUBX,40,RMC,0,0,0,0,0,0";
const char disableGLL[] = "PUBX,40,GLL,0,0,0,0,0,0";
const char disableGSV[] = "PUBX,40,GSV,0,0,0,0,0,0";
const char disableGSA[] = "PUBX,40,GSA,0,0,0,0,0,0";
const char disableGGA[] = "PUBX,40,GGA,0,0,0,0,0,0";
const char disableVTG[] = "PUBX,40,VTG,0,0,0,0,0,0";
const char disableZDA[] = "PUBX,40,ZDA,0,0,0,0,0,0";
const char disableGNS[] = "PUBX,40,GNS,0,0,0,0,0,0";
//const char disableGBS[] PROGMEM = "PUBX,40,GBS,0,0,0,0,0,0";
const char disableGST[] = "PUBX,40,GST,0,0,0,0,0,0";
const char enableRMC[] = "PUBX,40,RMC,0,1,0,0,0,0";
const char enableGLL[] = "PUBX,40,GLL,0,1,0,0,0,0";
const char enableGSV[] = "PUBX,40,GSV,0,1,0,0,0,0";
const char enableGSA[] = "PUBX,40,GSA,0,1,0,0,0,0";
const char enableGGA[] = "PUBX,40,GGA,0,1,0,0,0,0";
const char enableVTG[] = "PUBX,40,VTG,0,1,0,0,0,0";
const char enableZDA[] = "PUBX,40,ZDA,0,1,0,0,0,0";
const char enableGNS[] = "PUBX,40,GNS,0,1,0,0,0,0";
//const char enableGBS[] PROGMEM = "PUBX,40,GBS,0,1,0,0,0,0";
const char enableGST[] = "PUBX,40,GST,0,1,0,0,0,0\r\n";
/* void print_segger(char *buf)
{
int msg_len = strlen(buf);
for (int i = 0; i < msg_len; i++) {
//uart_poll_out(uart_dev, buf[i]);
SEGGER_RTT_Write(1,&buf[i],1);
}
}
void print_segger_byte(char buf)
{
SEGGER_RTT_Write(1,&buf,1);
}*/
void serial_cb(const struct device *dev, void *user_data)
{
uint8_t c;
if (!uart_irq_update(uart_dev)) {
return;
}
if (!uart_irq_rx_ready(uart_dev)) {
return;
}
/* read until FIFO empty */
while (uart_fifo_read(uart_dev, &c, 1) == 1) {
if(c == '$')
{
nmea_position = 0;
nmea_sentence[nmea_position] = c;
nmea_position++;
} else if(nmea_position > 0)
{
//if((nmea_position == 1 && c != 'G') || (nmea_position == 2 && (c != 'L' && c != 'N' && c != 'P')) || (nmea_position == 6 && c!= ','))
//{
// nmea_position = 0;
//}
if(nmea_position < (sizeof(nmea_sentence) -1))
{
nmea_sentence[nmea_position] = c;
nmea_position++;
if(c == '\n')
{
nmea_sentence[nmea_position] = '\0';
k_msgq_put(&uart_msgq, &nmea_sentence, K_NO_WAIT);
nmea_position = 0;
}
} else {
printk("overflow\n");
nmea_position = 0;
}
}
//print_segger_byte(c);
}
}
static char formatHex( uint8_t val )
{
val &= 0x0F;
return (val >= 10) ? ((val - 10) + 'A') : (val + '0');
}
void print_uart_byte(char buf)
{
uart_poll_out(uart_dev, buf);
}
void print_uart(char *buf)
{
int msg_len = strlen(buf);
for (int i = 0; i < msg_len; i++) {
uart_poll_out(uart_dev, buf[i]);
}
}
void print_uart_const(const char *buf)
{
int msg_len = strlen(buf);
print_uart_byte('$');
uint8_t crc = 0;
for (int i = 0; i < msg_len; i++) {
print_uart_byte(buf[i]);
crc ^= buf[i];
}
print_uart_byte('*');
print_uart_byte(formatHex(crc>>4));
print_uart_byte(formatHex(crc));
print_uart_byte('\r');
print_uart_byte('\n');
}
void sendUBX( const unsigned char *progmemBytes, size_t len)
{
/*if(wakeup == true)
{
gpsPort.write( 0xFF );
delay( 500 );
}*/
print_uart_byte( 0xB5 ); // SYNC1
print_uart_byte( 0x62 ); // SYNC2
uint8_t a = 0, b = 0;
while (len-- > 0) {
uint8_t c = *(progmemBytes++);
a += c;
b += a;
print_uart_byte( c );
}
print_uart_byte( a ); // CHECKSUM A
print_uart_byte( b ); // CHECKSUM B
} // sendUBX
void init_gps()
{
sendUBX(GNSSStart, sizeof(GNSSStart));
k_msleep(100);
sendUBX(ubxNMEA, sizeof(ubxNMEA));
k_msleep(100);
sendUBX(ubxGNSS, sizeof(ubxGNSS));
k_msleep(100);
sendUBX(rate1Hz5Mes, sizeof(rate1Hz5Mes));
k_msleep(100);
sendUBX(ubxDisablePulse, sizeof(ubxDisablePulse));
k_msleep(100);
print_uart_const(disableRMC);
k_msleep(100);
print_uart_const(disableGSV);
k_msleep(100);
print_uart_const(disableVTG);
k_msleep(100);
print_uart_const(disableGLL);
k_msleep(100);
print_uart_const(enableGGA);
k_msleep(100);
print_uart_const(enableGSA);
k_msleep(100);
sendUBX(enablePUBX0, sizeof(enablePUBX0));
k_msleep(100);
sendUBX(enablePUBX4, sizeof(enablePUBX4));
}
/*void segger_read_ep(void *, void *, void *)
{
k_sem_take(&segger_init_ok, K_FOREVER);
while (1) {
char tx_buf;
unsigned numbytes = 0;
do {
numbytes = SEGGER_RTT_Read(1, &tx_buf, 1);
if(numbytes > 0)
{
//print_uart("Echo: ");
//for(int i = numbytes; i < MSG_SIZE; i++)
//{
// tx_buf[i] = '\0';
//}
printk("Segger received %d bytes: %d\n", numbytes, tx_buf);
print_uart_byte(tx_buf);
//print_uart("\r\n");
}
} while(numbytes > 0);
k_msleep(100);
k_yield();
}
}
K_THREAD_DEFINE(segger_read_tid, MY_STACK_SIZE,
segger_read_ep, NULL, NULL, NULL,
MY_PRIORITY, 0, 0);
*/
void stop_gps() {
sendUBX(GNSSStop, sizeof(GNSSStop));
}
void gpsPower(bool state) {
gps_state = state;
if(state == true) {
LOG_WRN("Start GPS");
init_gps();
} else {
LOG_WRN("Stop GPS");
stop_gps();
}
}
void gpsBoost(bool state) {
if(state) {
sendUBX(rate1Hz1Mes, sizeof(rate1Hz1Mes));
k_msleep(100);
} else {
sendUBX(rate1Hz5Mes, sizeof(rate1Hz5Mes));
k_msleep(100);
}
}
void gps_ep(void *, void *, void *)
{
char tx_buf[MSG_SIZE];
lwgps_t hgps;
/* struct rtt_config_t rtt_config;
rtt_config.up_size = RTT_BUFFER_UP_SIZE;
rtt_config.down_size = RTT_BUFFER_DOWN_SIZE;
rtt_config.up_buffer = k_malloc(RTT_BUFFER_UP_SIZE);
rtt_config.down_buffer = k_malloc(RTT_BUFFER_DOWN_SIZE);
rtt_config.channel = 1;
SEGGER_RTT_ConfigUpBuffer(rtt_config.channel, "RTT GPS", rtt_config.up_buffer, rtt_config.up_size, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
SEGGER_RTT_ConfigDownBuffer(rtt_config.channel, "RTT GPS", rtt_config.down_buffer, rtt_config.down_size, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
k_sem_give(&segger_init_ok);*/
if (!device_is_ready(uart_dev)) {
printk("UART device not found!");
return;
}
gpsPower(true);
k_msleep(250);
gpsPower(false);
lwgps_init(&hgps);
//k_fifo_init(&fifo_gps_sat_data);
//k_sem_init(&gps_data_tosend, 0, 2);
/* configure interrupt and callback to receive data */
int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL);
if (ret < 0) {
if (ret == -ENOTSUP) {
printk("Interrupt-driven UART API support not enabled\n");
} else if (ret == -ENOSYS) {
printk("UART device does not support interrupt-driven API\n");
} else {
printk("Error setting UART callback: %d\n", ret);
}
return;
}
uart_irq_rx_enable(uart_dev);
LOG_INF("Hello GPS");
k_sem_give(&gps_init_ok);
uint8_t gnss_count = 0;
uint32_t altitude = 0;
uint8_t gps = 0;
uint8_t glonass = 0;
uint8_t galileo = 0;
/* indefinitely wait for input from the user */
while (k_msgq_get(&uart_msgq, &tx_buf, K_FOREVER) == 0) {
//print_uart("Echo: ");
//char talkerid[3];
//minmea_talker_id(&talkerid, tx_buf);
//LOG_INF("NMEA msg");
if(gps_state == true)
{
LOG_DBG("NMEA Sentence: %s", tx_buf);
lwgps_process(&hgps, tx_buf, strlen(tx_buf));
//LOG_INF("NMEA Sentence %s", hgps.p.term_str);
if(hgps.p.stat == STAT_GSA) {
uint8_t sat_count = 0;
for(int i = 0; i < sizeof(hgps.satellites_ids); i++)
{
if(hgps.satellites_ids[i] != 0) { sat_count++; }
}
LOG_INF("systemId: %d %d", hgps.systemId, sat_count);
if(hgps.systemId == 1) {
gps = sat_count;
} else if(hgps.systemId == 2) {
glonass = sat_count;
} else if(hgps.systemId == 3) {
galileo = sat_count;
}
gnss_count++;
if(gnss_count >= 3 && (gps != 0 || glonass != 0 || galileo != 0)) {
struct gps_sat_data_t *sat_data = k_malloc(sizeof(*sat_data));
sat_data->gps = gps;
sat_data->glonass = glonass;
sat_data->galileo = galileo;
//LOG_INF("Send Sat Data");
k_fifo_put(&fifo_gps_sat_data, sat_data);
k_sem_give(&gps_data_tosend);
gnss_count = 0;
}
} else if(hgps.p.stat == STAT_GGA) {
altitude = (int32_t) (hgps.altitude*100);
} else if(hgps.p.stat == STAT_UBX_POSITION) {
LOG_INF("Position %d %d", hgps.latitude, hgps.longitude);
LOG_INF("Altitude: %d", altitude);
LOG_INF("DOP %d %d", hgps.dop_h, hgps.dop_v);
LOG_INF("Accuracy H:%d V:%d", hgps.acc_h, hgps.acc_v);
LOG_INF("Speed:%d Course:%d", hgps.speed, hgps.course);
if(hgps.dop_h != 999 && hgps.dop_v != 999) {
struct gps_data_t *data = k_malloc(sizeof(*data));
data->latitude = hgps.latitude;
data->longitude = hgps.longitude;
data->altitude = altitude;
data->dop_h = hgps.dop_h;
data->dop_v = hgps.dop_v;
data->acc_h = hgps.acc_h;
data->acc_v = hgps.acc_v;
data->course = hgps.course;
data->speed = hgps.speed;
k_fifo_put(&fifo_gps_data, data);
k_sem_give(&gps_data_tosend);
}
} else if(hgps.p.stat == STAT_UBX_TIME) {
if(hgps.year >= 24)
{
struct tm current_tm;
current_tm.tm_hour = hgps.hours;
current_tm.tm_min = hgps.minutes;
current_tm.tm_sec = hgps.seconds;
current_tm.tm_mday= hgps.date;
current_tm.tm_mon = hgps.month-1;
current_tm.tm_year = hgps.year + 2000 - 1900;
setTs((uint32_t) timeutil_timegm64(&current_tm));
sendUBX(disablePUBX4, sizeof(disablePUBX4));
}
}
} else if(strcmp(tx_buf, "$GNTXT,01,01,02,Stopping GNSS*52\r\n") != 0) {
gpsPower(false);
}
}
}
K_THREAD_DEFINE(gps_tid, MY_STACK_SIZE,
gps_ep, NULL, NULL, NULL,
MY_PRIORITY, 0, 0);

35
src/gps.h Normal file
View file

@ -0,0 +1,35 @@
#ifndef APPLICATION_GPS_H_
#define APPLICATION_GPS_H_
#include <zephyr/types.h>
struct gps_sat_data_t {
void *fifo_reserved;
uint8_t gps;
uint8_t glonass;
uint8_t galileo;
};
struct gps_data_t {
void *fifo_reserved;
int32_t latitude; /*!< Latitude in units of degrees */
int32_t longitude; /*!< Longitude in units of degrees */
int32_t altitude; /*!< Longitude in units of degrees */
uint16_t dop_h; /*!< Horizontal dilution of precision */
uint16_t dop_v; /*!< Vertical dilution of precision */
uint32_t acc_h;
uint32_t acc_v;
uint16_t speed; /*!< Ground speed in knots */
uint16_t course; /*!< Ground coarse */
};
//extern K_FIFO_DEFINE(fifo_gps_sat_data);
extern struct k_fifo fifo_gps_sat_data;
extern struct k_fifo fifo_gps_data;
extern struct k_sem gps_data_tosend;
extern struct k_sem gps_init_ok;
void gpsPower(bool state);
void gpsBoost(bool state);
#endif /* APPLICATION_GPS_H_ */

651
src/lwgps.c Normal file
View file

@ -0,0 +1,651 @@
/**
* \file lwgps.c
* \brief GPS main file
*/
/*
* Copyright (c) 2023 Tilen MAJERLE
*
* 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.
*
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
*
* Author: Tilen MAJERLE <tilen@majerle.eu>
* Version: v2.1.0
*/
#include <zephyr/kernel.h>
#include <stdbool.h>
#include <stdlib.h>
#include <string.h>
#include "lwgps.h"
//#if LWESP_CFG_DISTANCE_BEARING
#include <math.h>
//#endif
#define FLT(x) ((lwgps_float_t)(x))
#define D2R(x) FLT(FLT(x) * FLT(0.01745329251994)) /*!< Degrees to radians */
#define R2D(x) FLT(FLT(x) * FLT(57.29577951308232)) /*!< Radians to degrees */
#define EARTH_RADIUS FLT(6371.0) /*!< Earth radius in units of kilometers */
#if LWGPS_CFG_CRC
#define CRC_ADD(_gh, ch) (_gh)->p.crc_calc ^= (uint8_t)(ch)
#else
#define CRC_ADD(_gh, ch)
#endif /* LWGPS_CFG_CRC */
#define TERM_ADD(_gh, ch) \
do { \
if ((_gh)->p.term_pos < (sizeof((_gh)->p.term_str) - 1)) { \
(_gh)->p.term_str[(_gh)->p.term_pos] = (ch); \
(_gh)->p.term_str[++(_gh)->p.term_pos] = 0; \
} \
} while (0)
#define TERM_NEXT(_gh) \
do { \
(_gh)->p.term_str[((_gh)->p.term_pos = 0)] = 0; \
++(_gh)->p.term_num; \
} while (0)
#define CIN(x) ((x) >= '0' && (x) <= '9')
#define CIHN(x) (((x) >= '0' && (x) <= '9') || ((x) >= 'a' && (x) <= 'f') || ((x) >= 'A' && (x) <= 'F'))
#define CTN(x) ((x) - '0')
#define CHTN(x) \
(((x) >= '0' && (x) <= '9') \
? ((x) - '0') \
: (((x) >= 'a' && (x) <= 'z') ? ((x) - 'a' + 10) : (((x) >= 'A' && (x) <= 'Z') ? ((x) - 'A' + 10) : 0)))
/**
* \brief Parse number as integer
* \param[in] gh: GPS handle
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
* \return Parsed integer
*/
static int32_t
prv_parse_number(lwgps_t* gh, const char* t) {
int32_t res = 0;
uint8_t minus;
if (t == NULL) {
t = gh->p.term_str;
}
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
minus = (*t == '-' ? (++t, 1) : 0);
for (; t != NULL && CIN(*t); ++t) {
res = 10 * res + CTN(*t);
}
return minus ? -res : res;
}
/**
* \brief Parse number as double and convert it to \ref lwgps_float_t
* \param[in] gh: GPS handle
* \param[in] t: Text to parse. Set to `NULL` to parse current GPS term
* \return Parsed double in \ref lwgps_float_t format
*/
static lwgps_float_t
prv_parse_float_number(lwgps_t* gh, const char* t) {
lwgps_float_t value = (lwgps_float_t)0, power = (lwgps_float_t)1;
int sign = 1;
if (t == NULL) {
t = gh->p.term_str;
}
for (; t != NULL && *t == ' '; ++t) {} /* Strip leading spaces */
if (*t == '-') { /* Check sign */
sign = -1;
++t;
}
while (CIN(*t)) { /* Convert main part */
value = value * (lwgps_float_t)10 + CTN(*t);
++t;
}
if (*t == '.') { /* Skip the dot character */
++t;
}
while (CIN(*t)) { /* Get the power */
value = value * (lwgps_float_t)10 + CTN(*t);
power *= (lwgps_float_t)10.0;
++t;
}
return sign * value / power;
}
/**
* \brief Parse latitude/longitude NMEA format to double
*
* NMEA output for latitude is ddmm.sss and longitude is dddmm.sss
* \param[in] gh: GPS handle
* \return Latitude/Longitude value in degrees
*/
static lwgps_float_t
prv_parse_lat_long(lwgps_t* gh) {
lwgps_float_t ll, deg, min;
ll = prv_parse_float_number(gh, NULL); /* Parse value as double */
deg = FLT((int)((int)ll / 100)); /* Get absolute degrees value, interested in integer part only */
min = ll - (deg * FLT(100)); /* Get remaining part from full number, minutes */
ll = deg + (min / FLT(60.0)); /* Calculate latitude/longitude */
return ll;
}
/* static int32_t
prv_parse_lat_long_int32(lwgps_t* gh) {
const char *t = gh->p.term_str;
int32_t ret = 0;
bool dec_part = true;
for(int i = strlen(t)-1; i >= 0; i--)
{
if(t[i] == '.') {
dec_part = false;
} else {
if(dec_part == true) {
ret = ret + CTN(t[i]) * ((uint32_t) pow(10.0, (strlen(t)-i-1)));
} else {
ret = ret + CTN(t[i]) * ((uint32_t) pow(10.0, (strlen(t)-i-2)));
}
}
}
return ret;
}*/
/**
* \brief Parse received term
* \param[in] gh: GPS handle
* \return `1` on success, `0` otherwise
*/
static uint8_t
prv_parse_term(lwgps_t* gh) {
if (gh->p.term_num == 0) { /* Check string type */
if (0) {
#if LWGPS_CFG_STATEMENT_GPGGA
} else if (!strncmp(gh->p.term_str, "$GPGGA", 6) || !strncmp(gh->p.term_str, "$GNGGA", 6)) {
gh->p.stat = STAT_GGA;
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
#if LWGPS_CFG_STATEMENT_GPGSA
} else if (!strncmp(gh->p.term_str, "$GPGSA", 6) || !strncmp(gh->p.term_str, "$GNGSA", 6)) {
gh->p.stat = STAT_GSA;
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
#if LWGPS_CFG_STATEMENT_GPGSV
} else if (!strncmp(gh->p.term_str, "$GPGSV", 6) || !strncmp(gh->p.term_str, "$GNGSV", 6)) {
gh->p.stat = STAT_GSV;
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
#if LWGPS_CFG_STATEMENT_GPRMC
} else if (!strncmp(gh->p.term_str, "$GPRMC", 6) || !strncmp(gh->p.term_str, "$GNRMC", 6)) {
gh->p.stat = STAT_RMC;
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
#if LWGPS_CFG_STATEMENT_PUBX
} else if (!strncmp(gh->p.term_str, "$PUBX", 5)) {
gh->p.stat = STAT_UBX;
#endif /* LWGPS_CFG_STATEMENT_PUBX */
} else {
gh->p.stat = STAT_UNKNOWN; /* Invalid statement for library */
}
return 1;
}
/* Start parsing terms */
if (gh->p.stat == STAT_UNKNOWN) {
#if LWGPS_CFG_STATEMENT_GPGGA
} else if (gh->p.stat == STAT_GGA) { /* Process GPGGA statement */
switch (gh->p.term_num) {
case 1: /* Process UTC time */
gh->p.data.gga.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
gh->p.data.gga.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
gh->p.data.gga.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
break;
case 2: /* Latitude */
gh->p.data.gga.latitude = prv_parse_lat_long(gh); /* Parse latitude */
break;
case 3: /* Latitude north/south information */
if (gh->p.term_str[0] == 'S' || gh->p.term_str[0] == 's') {
gh->p.data.gga.latitude = -gh->p.data.gga.latitude;
}
break;
case 4: /* Longitude */
gh->p.data.gga.longitude = prv_parse_lat_long(gh); /* Parse longitude */
break;
case 5: /* Longitude east/west information */
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
gh->p.data.gga.longitude = -gh->p.data.gga.longitude;
}
break;
case 6: /* Fix status */ gh->p.data.gga.fix = (uint8_t)prv_parse_number(gh, NULL); break;
case 7: /* Satellites in use */ gh->p.data.gga.sats_in_use = (uint8_t)prv_parse_number(gh, NULL); break;
case 9: /* Altitude */ gh->p.data.gga.altitude = prv_parse_float_number(gh, NULL); break;
case 11: /* Altitude above ellipsoid */ gh->p.data.gga.geo_sep = prv_parse_float_number(gh, NULL); break;
default: break;
}
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
#if LWGPS_CFG_STATEMENT_GPGSA
} else if (gh->p.stat == STAT_GSA) { /* Process GPGSA statement */
switch (gh->p.term_num) {
case 2: /* Process fix mode */ gh->p.data.gsa.fix_mode = (uint8_t)prv_parse_number(gh, NULL); break;
case 15: /* Process PDOP */ gh->p.data.gsa.dop_p = prv_parse_float_number(gh, NULL); break;
case 16: /* Process HDOP */ gh->p.data.gsa.dop_h = prv_parse_float_number(gh, NULL); break;
case 17: /* Process VDOP */ gh->p.data.gsa.dop_v = prv_parse_float_number(gh, NULL); break;
case 18: /* Process systemId */ gh->p.data.gsa.systemId = (uint8_t)prv_parse_number(gh, NULL); break;
default:
/* Parse satellite IDs */
if (gh->p.term_num >= 3 && gh->p.term_num <= 14) {
gh->p.data.gsa.satellites_ids[gh->p.term_num - 3] = (uint8_t)prv_parse_number(gh, NULL);
}
break;
}
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
#if LWGPS_CFG_STATEMENT_GPGSV
} else if (gh->p.stat == STAT_GSV) { /* Process GPGSV statement */
switch (gh->p.term_num) {
case 2: /* Current GPGSV statement number */
gh->p.data.gsv.stat_num = (uint8_t)prv_parse_number(gh, NULL);
break;
case 3: /* Process satellites in view */
gh->p.data.gsv.sats_in_view = (uint8_t)prv_parse_number(gh, NULL);
break;
default:
#if LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
if (gh->p.term_num >= 4 && gh->p.term_num <= 19) { /* Check current term number */
uint8_t index, term_num = gh->p.term_num - 4; /* Normalize term number from 4-19 to 0-15 */
uint16_t value;
index = ((gh->p.data.gsv.stat_num - 1) << 0x02) + (term_num >> 2); /* Get array index */
if (index < sizeof(gh->sats_in_view_desc) / sizeof(gh->sats_in_view_desc[0])) {
value = (uint16_t)prv_parse_number(gh, NULL); /* Parse number as integer */
switch (term_num & 0x03) {
case 0: gh->sats_in_view_desc[index].num = value; break;
case 1: gh->sats_in_view_desc[index].elevation = value; break;
case 2: gh->sats_in_view_desc[index].azimuth = value; break;
case 3: gh->sats_in_view_desc[index].snr = value; break;
default: break;
}
}
}
#endif /* LWGPS_CFG_STATEMENT_GPGSV_SAT_DET */
break;
}
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
#if LWGPS_CFG_STATEMENT_GPRMC
} else if (gh->p.stat == STAT_RMC) { /* Process GPRMC statement */
switch (gh->p.term_num) {
case 2: /* Process valid status */ gh->p.data.rmc.is_valid = (gh->p.term_str[0] == 'A'); break;
case 7: /* Process ground speed in knots */ gh->p.data.rmc.speed = prv_parse_float_number(gh, NULL); break;
case 8: /* Process true ground coarse */ gh->p.data.rmc.course = prv_parse_float_number(gh, NULL); break;
case 9: /* Process date */
gh->p.data.rmc.date = (uint8_t)(10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]));
gh->p.data.rmc.month = (uint8_t)(10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]));
gh->p.data.rmc.year = (uint8_t)(10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]));
break;
case 10: /* Process magnetic variation */
gh->p.data.rmc.variation = prv_parse_float_number(gh, NULL);
break;
case 11: /* Process magnetic variation east/west */
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
gh->p.data.rmc.variation = -gh->p.data.rmc.variation;
}
break;
default: break;
}
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
#if LWGPS_CFG_STATEMENT_PUBX
} else if (gh->p.stat == STAT_UBX) { /* Disambiguate generic PUBX statement */
if (gh->p.term_str[0] == '0' && gh->p.term_str[1] == '4') {
gh->p.stat = STAT_UBX_TIME;
}
if (gh->p.term_str[0] == '0' && gh->p.term_str[1] == '0') {
gh->p.stat = STAT_UBX_POSITION;
}
#if LWGPS_CFG_STATEMENT_PUBX_TIME
} else if (gh->p.stat == STAT_UBX_TIME) { /* Process PUBX (uBlox) TIME statement */
switch (gh->p.term_num) {
case 2: /* Process UTC time; ignore fractions of seconds */
gh->p.data.time.hours = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
gh->p.data.time.minutes = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
gh->p.data.time.seconds = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
break;
case 3: /* Process UTC date */
gh->p.data.time.date = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
gh->p.data.time.month = 10 * CTN(gh->p.term_str[2]) + CTN(gh->p.term_str[3]);
gh->p.data.time.year = 10 * CTN(gh->p.term_str[4]) + CTN(gh->p.term_str[5]);
break;
case 4: /* Process UTC TimeOfWeek */ gh->p.data.time.utc_tow = prv_parse_float_number(gh, NULL); break;
case 5: /* Process UTC WeekNumber */ gh->p.data.time.utc_wk = prv_parse_number(gh, NULL); break;
case 6: /* Process UTC leap seconds */
/*
* Accomodate a 2- or 3-digit leap second count
* a trailing 'D' means this is the firmware's default value.
*/
if (gh->p.term_str[2] == 'D' || gh->p.term_str[2] == '\0') {
gh->p.data.time.leap_sec = 10 * CTN(gh->p.term_str[0]) + CTN(gh->p.term_str[1]);
} else {
gh->p.data.time.leap_sec =
100 * CTN(gh->p.term_str[0]) + 10 * CTN(gh->p.term_str[1]) + CTN(gh->p.term_str[2]);
}
break;
case 7: /* Process clock bias */ gh->p.data.time.clk_bias = prv_parse_number(gh, NULL); break;
case 8: /* Process clock drift */ gh->p.data.time.clk_drift = prv_parse_float_number(gh, NULL); break;
case 9: /* Process time pulse granularity */ gh->p.data.time.tp_gran = prv_parse_number(gh, NULL); break;
default: break;
}
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
#if LWGPS_CFG_STATEMENT_PUBX_POSITION
} else if (gh->p.stat == STAT_UBX_POSITION) { /* Process PUBX (uBlox) POSITION statement */
switch (gh->p.term_num) {
case 3: /* Latitude */
gh->p.data.position.latitude = (int32_t) (prv_parse_lat_long(gh)*10000000); /* Parse latitude */
break;
case 4: /* Latitude north/south information */
if (gh->p.term_str[0] == 'S' || gh->p.term_str[0] == 's') {
gh->p.data.position.latitude = -gh->p.data.position.latitude;
}
break;
case 5: /* Longitude */
gh->p.data.position.longitude = (int32_t) (prv_parse_lat_long(gh)*10000000); /* Parse longitude */
break;
case 6: /* Longitude east/west information */
if (gh->p.term_str[0] == 'W' || gh->p.term_str[0] == 'w') {
gh->p.data.position.longitude = -gh->p.data.position.longitude;
}
break;
case 9: /* Acc H */ gh->p.data.position.acc_h = (uint32_t) (prv_parse_float_number(gh, NULL)*100); break;
case 10: /* Acc V */ gh->p.data.position.acc_v = (uint32_t) (prv_parse_float_number(gh, NULL)*100); break;
case 11: /* Speed */ gh->p.data.position.speed = (uint16_t) (prv_parse_float_number(gh, NULL)*100000/3600); break;
case 12: /* Course */ gh->p.data.position.course = (uint16_t) (prv_parse_float_number(gh, NULL)*100); break;
case 15: /* Process HDOP */ gh->p.data.position.dop_h = (uint16_t) (prv_parse_float_number(gh, NULL)*10); break;
case 16: /* Process VDOP */ gh->p.data.position.dop_v = (uint16_t) (prv_parse_float_number(gh, NULL)*10); break;
default: break;
}
#endif /* LWGPS_CFG_STATEMENT_PUBX_POSITION */
#endif /* LWGPS_CFG_STATEMENT_PUBX */
}
return 1;
}
#if LWGPS_CFG_CRC
/**
* \brief Compare calculated CRC with received CRC
* \param[in] gh: GPS handle
* \return `1` on success, `0` otherwise
*/
static uint8_t
prv_check_crc(lwgps_t* gh) {
uint8_t crc;
crc = (uint8_t)((CHTN(gh->p.term_str[0]) & 0x0F) << 0x04)
| (CHTN(gh->p.term_str[1]) & 0x0F); /* Convert received CRC from string (hex) to number */
return gh->p.crc_calc == crc; /* They must match! */
}
#else
#define prv_check_crc(_gh) (1)
#endif /* LWGPS_CFG_CRC */
/**
* \brief Copy temporary memory to user memory
* \param[in] gh: GPS handle
* \return `1` on success, `0` otherwise
*/
static uint8_t
prv_copy_from_tmp_memory(lwgps_t* gh) {
if (0) {
#if LWGPS_CFG_STATEMENT_GPGGA
} else if (gh->p.stat == STAT_GGA) {
#if !LWGPS_CFG_STATEMENT_PUBX_POSITION
gh->latitude = gh->p.data.gga.latitude;
gh->longitude = gh->p.data.gga.longitude;
#endif /* !LWGPS_CFG_STATEMENT_PUBX_POSITION */
gh->altitude = gh->p.data.gga.altitude;
gh->geo_sep = gh->p.data.gga.geo_sep;
gh->sats_in_use = gh->p.data.gga.sats_in_use;
gh->fix = gh->p.data.gga.fix;
gh->hours = gh->p.data.gga.hours;
gh->minutes = gh->p.data.gga.minutes;
gh->seconds = gh->p.data.gga.seconds;
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
#if LWGPS_CFG_STATEMENT_GPGSA
} else if (gh->p.stat == STAT_GSA) {
gh->latitude = gh->p.data.gga.latitude;
#if !LWGPS_CFG_STATEMENT_PUBX_POSITION
gh->dop_h = gh->p.data.gsa.dop_h;
gh->dop_v = gh->p.data.gsa.dop_v;
#endif /* !LWGPS_CFG_STATEMENT_PUBX_POSITION */
gh->dop_v = gh->p.data.gsa.dop_v;
gh->systemId = gh->p.data.gsa.systemId;
gh->fix_mode = gh->p.data.gsa.fix_mode;
memcpy(gh->satellites_ids, gh->p.data.gsa.satellites_ids, sizeof(gh->satellites_ids));
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
#if LWGPS_CFG_STATEMENT_GPGSV
} else if (gh->p.stat == STAT_GSV) {
gh->sats_in_view = gh->p.data.gsv.sats_in_view;
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
#if LWGPS_CFG_STATEMENT_GPRMC
} else if (gh->p.stat == STAT_RMC) {
gh->course = gh->p.data.rmc.course;
gh->is_valid = gh->p.data.rmc.is_valid;
gh->speed = gh->p.data.rmc.speed;
gh->variation = gh->p.data.rmc.variation;
gh->date = gh->p.data.rmc.date;
gh->month = gh->p.data.rmc.month;
gh->year = gh->p.data.rmc.year;
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
#if LWGPS_CFG_STATEMENT_PUBX_TIME
} else if (gh->p.stat == STAT_UBX_TIME) {
gh->hours = gh->p.data.time.hours;
gh->minutes = gh->p.data.time.minutes;
gh->seconds = gh->p.data.time.seconds;
gh->date = gh->p.data.time.date;
gh->month = gh->p.data.time.month;
gh->year = gh->p.data.time.year;
gh->utc_tow = gh->p.data.time.utc_tow;
gh->utc_wk = gh->p.data.time.utc_wk;
gh->leap_sec = gh->p.data.time.leap_sec;
gh->clk_bias = gh->p.data.time.clk_bias;
gh->clk_drift = gh->p.data.time.clk_drift;
gh->tp_gran = gh->p.data.time.tp_gran;
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
#if LWGPS_CFG_STATEMENT_PUBX_POSITION
} else if (gh->p.stat == STAT_UBX_POSITION) {
gh->latitude = gh->p.data.position.latitude;
gh->longitude = gh->p.data.position.longitude;
gh->dop_h = gh->p.data.position.dop_h;
gh->dop_v = gh->p.data.position.dop_v;
gh->acc_h = gh->p.data.position.acc_h;
gh->acc_v = gh->p.data.position.acc_v;
gh->speed = gh->p.data.position.speed;
gh->course = gh->p.data.position.course;
#endif /* LWGPS_CFG_STATEMENT_PUBX_POSITION */
}
return 1;
}
/**
* \brief Init GPS handle
* \param[in] gh: GPS handle structure
* \return `1` on success, `0` otherwise
*/
uint8_t
lwgps_init(lwgps_t* gh) {
memset(gh, 0x00, sizeof(*gh)); /* Reset structure */
return 1;
}
/**
* \brief Process NMEA data from GPS receiver
* \param[in] gh: GPS handle structure
* \param[in] data: Received data
* \param[in] len: Number of bytes to process
* \param[in] evt_fn: Event function to notify application layer.
* This parameter is available only if \ref LWGPS_CFG_STATUS is enabled
* \return `1` on success, `0` otherwise
*/
uint8_t
#if LWGPS_CFG_STATUS || __DOXYGEN__
lwgps_process(lwgps_t* gh, const void* data, size_t len, lwgps_process_fn evt_fn) {
#else /* LWGPS_CFG_STATUS */
lwgps_process(lwgps_t* gh, const void* data, size_t len) {
#endif /* !LWGPS_CFG_STATUS */
const uint8_t* d = data;
for (; len > 0; ++d, --len) { /* Process all bytes */
if (*d == '$') { /* Check for beginning of NMEA line */
memset(&gh->p, 0x00, sizeof(gh->p)); /* Reset private memory */
TERM_ADD(gh, *d); /* Add character to term */
} else if (*d == ',') { /* Term separator character */
prv_parse_term(gh); /* Parse term we have currently in memory */
CRC_ADD(gh, *d); /* Add character to CRC computation */
TERM_NEXT(gh); /* Start with next term */
} else if (*d == '*') { /* Start indicates end of data for CRC computation */
prv_parse_term(gh); /* Parse term we have currently in memory */
gh->p.star = 1; /* STAR detected */
TERM_NEXT(gh); /* Start with next term */
} else if (*d == '\r') {
if (prv_check_crc(gh)) { /* Check for CRC result */
/* CRC is OK, in theory we can copy data from statements to user data */
prv_copy_from_tmp_memory(gh); /* Copy memory from temporary to user memory */
#if LWGPS_CFG_STATUS
if (evt_fn != NULL) {
evt_fn(gh->p.stat);
}
} else if (evt_fn != NULL) {
evt_fn(STAT_CHECKSUM_FAIL);
#endif /* LWGPS_CFG_STATUS */
}
} else {
if (!gh->p.star) { /* Add to CRC only if star not yet detected */
CRC_ADD(gh, *d); /* Add to CRC */
}
TERM_ADD(gh, *d); /* Add character to term */
}
}
return 1;
}
#if LWESP_CFG_DISTANCE_BEARING || __DOXYGEN__
/**
* \brief Calculate distance and bearing between `2` latitude and longitude coordinates
* \param[in] las: Latitude start coordinate, in units of degrees
* \param[in] los: Longitude start coordinate, in units of degrees
* \param[in] lae: Latitude end coordinate, in units of degrees
* \param[in] loe: Longitude end coordinate, in units of degrees
* \param[out] d: Pointer to output distance in units of meters
* \param[out] b: Pointer to output bearing between start and end coordinate in relation to north in units of degrees
* \return `1` on success, `0` otherwise
*/
uint8_t
lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae, lwgps_float_t loe, lwgps_float_t* d,
lwgps_float_t* b) {
lwgps_float_t df, dfi, a;
if (d == NULL && b == NULL) {
return 0;
}
/* Convert degrees to radians */
df = D2R(lae - las);
dfi = D2R(loe - los);
las = D2R(las);
los = D2R(los);
lae = D2R(lae);
loe = D2R(loe);
/*
* Calculate distance
*
* Calculated distance is absolute value in meters between 2 points on earth.
*/
if (d != NULL) {
/*
* a = sin(df / 2)^2 + cos(las) * cos(lae) * sin(dfi / 2)^2
* *d = RADIUS * 2 * atan(sqrt(a) / sqrt(1 - a)) * 1000 (for meters)
*/
#if LWGPS_CFG_DOUBLE
a = FLT(sin(df * 0.5) * sin(df * 0.5) + sin(dfi * 0.5) * sin(dfi * 0.5) * cos(las) * cos(lae));
*d = FLT(EARTH_RADIUS * 2.0 * atan2(sqrt(a), sqrt(1.0 - a)) * 1000.0);
#else /* LWGPS_CFG_DOUBLE */
a = FLT(sinf(df * 0.5f) * sinf(df * 0.5f) + sinf(dfi * 0.5f) * sinf(dfi * 0.5f) * cosf(las) * cosf(lae));
*d = FLT(EARTH_RADIUS * 2.0f * atan2f(sqrtf(a), sqrtf(1.0f - a)) * 1000.0f);
#endif /* !LWGPS_CFG_DOUBLE */
}
/*
* Calculate bearing
*
* Bearing is calculated from point 1 to point 2.
* Result will tell us in which direction (according to north) we should move,
* to reach point 2.
*
* Example:
* Bearing is 0 => move to north
* Bearing is 90 => move to east
* Bearing is 180 => move to south
* Bearing is 270 => move to west
*/
if (b != NULL) {
#if LWGPS_CFG_DOUBLE
df = FLT(sin(loe - los) * cos(lae));
dfi = FLT(cos(las) * sin(lae) - sin(las) * cos(lae) * cos(loe - los));
*b = R2D(atan2(df, dfi)); /* Calculate bearing and convert to degrees */
#else /* LWGPS_CFG_DOUBLE */
df = FLT(sinf(loe - los) * cosf(lae));
dfi = FLT(cosf(las) * sinf(lae) - sinf(las) * cosf(lae) * cosf(loe - los));
*b = R2D(atan2f(df, dfi)); /* Calculate bearing and convert to degrees */
#endif /* !LWGPS_CFG_DOUBLE */
if (*b < 0) { /* Check for negative angle */
*b += FLT(360); /* Make bearing always positive */
}
}
return 1;
}
#endif /* LWESP_CFG_DISTANCE_BEARING || __DOXYGEN__ */
/**
* \brief Convert NMEA GPS speed (in knots = nautical mile per hour) to different speed format
* \param[in] sik: Speed in knots, received from GPS NMEA statement
* \param[in] ts: Target speed to convert to from knots
* \return Speed calculated from knots
*/
lwgps_float_t
lwgps_to_speed(lwgps_float_t sik, lwgps_speed_t ts) {
switch (ts) {
case LWGPS_SPEED_KPS: return FLT(sik * FLT(0.000514));
case LWGPS_SPEED_KPH: return FLT(sik * FLT(1.852));
case LWGPS_SPEED_MPS: return FLT(sik * FLT(0.5144));
case LWGPS_SPEED_MPM: return FLT(sik * FLT(30.87));
case LWGPS_SPEED_MIPS: return FLT(sik * FLT(0.0003197));
case LWGPS_SPEED_MPH: return FLT(sik * FLT(1.151));
case LWGPS_SPEED_FPS: return FLT(sik * FLT(1.688));
case LWGPS_SPEED_FPM: return FLT(sik * FLT(101.3));
case LWGPS_SPEED_MPK: return FLT(sik * FLT(32.4));
case LWGPS_SPEED_SPK: return FLT(sik * FLT(1944.0));
case LWGPS_SPEED_SP100M: return FLT(sik * FLT(194.4));
case LWGPS_SPEED_MIPM: return FLT(sik * FLT(52.14));
case LWGPS_SPEED_SPM: return FLT(sik * FLT(3128.0));
case LWGPS_SPEED_SP100Y: return FLT(sik * FLT(177.7));
case LWGPS_SPEED_SMPH: return FLT(sik * FLT(1.0));
default: return 0;
}
}

339
src/lwgps.h Normal file
View file

@ -0,0 +1,339 @@
/**
* \file lwgps.h
* \brief GPS main file
*/
/*
* Copyright (c) 2023 Tilen MAJERLE
*
* 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.
*
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
*
* Author: Tilen MAJERLE <tilen@majerle.eu>
* Version: v2.1.0
*/
#ifndef LWGPS_HDR_H
#define LWGPS_HDR_H
#include <limits.h>
#include <stddef.h>
#include <stdint.h>
#include "lwgps_opt.h"
#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */
/**
* \defgroup LWGPS Lightweight GPS NMEA parser
* \brief Lightweight GPS NMEA parser
* \{
*/
/**
* \brief GPS float definition, can be either `float` or `double`
* \note Check for \ref LWGPS_CFG_DOUBLE configuration
*/
#if LWGPS_CFG_DOUBLE || __DOXYGEN__
typedef double lwgps_float_t;
#else
typedef float lwgps_float_t;
#endif
/**
* \brief Satellite descriptor
*/
typedef struct {
uint8_t num; /*!< Satellite number */
uint8_t elevation; /*!< Elevation value */
uint16_t azimuth; /*!< Azimuth in degrees */
uint8_t snr; /*!< Signal-to-noise ratio */
} lwgps_sat_t;
/**
* \brief ENUM of possible GPS statements parsed
*/
typedef enum {
STAT_UNKNOWN = 0, /*!< Unknown NMEA statement */
STAT_GGA = 1, /*!< GPGGA statement */
STAT_GSA = 2, /*!< GPGSA statement */
STAT_GSV = 3, /*!< GPGSV statement */
STAT_RMC = 4, /*!< GPRMC statement */
STAT_UBX = 5, /*!< UBX statement (uBlox specific) */
STAT_UBX_TIME = 6, /*!< UBX TIME statement (uBlox specific) */
STAT_UBX_POSITION = 7, /*!< UBX TIME statement (uBlox specific) */
STAT_CHECKSUM_FAIL = UINT8_MAX /*!< Special case, used when checksum fails */
} lwgps_statement_t;
/**
* \brief GPS main structure
*/
typedef struct {
#if LWGPS_CFG_STATEMENT_GPGGA || __DOXYGEN__
/* Information related to GPGGA statement */
#if !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__
lwgps_float_t latitude; /*!< Latitude in units of degrees */
lwgps_float_t longitude; /*!< Longitude in units of degrees */
#endif /* !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__ */
lwgps_float_t altitude; /*!< Altitude in units of meters */
lwgps_float_t geo_sep; /*!< Geoid separation in units of meters */
uint8_t sats_in_use; /*!< Number of satellites in use */
uint8_t fix; /*!< Fix status. `0` = invalid, `1` = GPS fix, `2` = DGPS fix, `3` = PPS fix */
uint8_t hours; /*!< Hours in UTC */
uint8_t minutes; /*!< Minutes in UTC */
uint8_t seconds; /*!< Seconds in UTC */
#endif /* LWGPS_CFG_STATEMENT_GPGGA || __DOXYGEN__ */
#if LWGPS_CFG_STATEMENT_GPGSA || __DOXYGEN__
/* Information related to GPGSA statement */
#if !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__
lwgps_float_t dop_h; /*!< Dolution of precision, horizontal */
lwgps_float_t dop_v; /*!< Dolution of precision, vertical */
lwgps_float_t dop_p; /*!< Dolution of precision, position */
#endif /* !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__ */
uint8_t systemId;
uint8_t fix_mode; /*!< Fix mode. `1` = NO fix, `2` = 2D fix, `3` = 3D fix */
uint8_t satellites_ids[12]; /*!< List of satellite IDs in use. Valid range is `0` to `sats_in_use` */
#endif /* LWGPS_CFG_STATEMENT_GPGSA || __DOXYGEN__ */
#if LWGPS_CFG_STATEMENT_GPGSV || __DOXYGEN__
/* Information related to GPGSV statement */
uint8_t sats_in_view; /*!< Number of satellites in view */
#if LWGPS_CFG_STATEMENT_GPGSV_SAT_DET || __DOXYGEN__
lwgps_sat_t sats_in_view_desc[12];
#endif /* LWGPS_CFG_STATEMENT_GPGSV_SAT_DET || __DOXYGEN__ */
#endif /* LWGPS_CFG_STATEMENT_GPGSV || __DOXYGEN__ */
#if LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__
/* Information related to GPRMC statement */
uint8_t is_valid; /*!< GPS valid status */
#if !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__
lwgps_float_t speed; /*!< Ground speed in knots */
lwgps_float_t course; /*!< Ground coarse */
#endif /* !LWGPS_CFG_STATEMENT_PUBX_POSITION && !__DOXYGEN__ */
lwgps_float_t variation; /*!< Magnetic variation */
uint8_t date; /*!< Fix date */
uint8_t month; /*!< Fix month */
uint8_t year; /*!< Fix year */
#endif /* LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__ */
#if !LWGPS_CFG_STATEMENT_GPGGA && !__DOXYGEN__
/* rely on time fields from GPGGA if possible */
uint8_t hours;
uint8_t minutes;
uint8_t seconds;
#endif /* !LWGPS_CFG_STATEMENT_GPGGA && !__DOXYGEN__ */
#if !LWGPS_CFG_STATEMENT_GPRMC && !__DOXYGEN__
/* rely on date fields from GPRMC if possible */
uint8_t date;
uint8_t month;
uint8_t year;
#endif /* !LWGPS_CFG_STATEMENT_GPRMC && !__DOXYGEN__ */
#if LWGPS_CFG_STATEMENT_PUBX_TIME || __DOXYGEN__
/* fields only available in PUBX_TIME */
lwgps_float_t utc_tow; /*!< UTC TimeOfWeek, eg 113851.00 */
uint16_t utc_wk; /*!< UTC week number, continues beyond 1023 */
uint8_t leap_sec; /*!< UTC leap seconds; UTC + leap_sec = TAI */
uint32_t clk_bias; /*!< Receiver clock bias, eg 1930035 */
lwgps_float_t clk_drift; /*!< Receiver clock drift, eg -2660.664 */
uint32_t tp_gran; /*!< Time pulse granularity, eg 43 */
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME || __DOXYGEN__ */
#if LWGPS_CFG_STATEMENT_PUBX_POSITION || __DOXYGEN__
int32_t latitude; /*!< Latitude in units of degrees */
int32_t longitude; /*!< Longitude in units of degrees */
uint16_t dop_h; /*!< Horizontal dilution of precision */
uint16_t dop_v; /*!< Vertical dilution of precision */
uint32_t acc_h;
uint32_t acc_v;
uint16_t speed; /*!< Ground speed in knots */
uint16_t course; /*!< Ground coarse */
#endif /* LWGPS_CFG_STATEMENT_PUBX_POSITION || __DOXYGEN__ */
#if !__DOXYGEN__
struct {
lwgps_statement_t stat; /*!< Statement index */
char term_str[13]; /*!< Current term in string format */
uint8_t term_pos; /*!< Current index position in term */
uint8_t term_num; /*!< Current term number */
uint8_t star; /*!< Star detected flag */
#if LWGPS_CFG_CRC
uint8_t crc_calc; /*!< Calculated CRC string */
#endif /* LWGPS_CFG_CRC */
union {
uint8_t dummy; /*!< Dummy byte */
#if LWGPS_CFG_STATEMENT_GPGGA
struct {
lwgps_float_t latitude; /*!< GPS latitude position in degrees */
lwgps_float_t longitude; /*!< GPS longitude position in degrees */
lwgps_float_t altitude; /*!< GPS altitude in meters */
lwgps_float_t geo_sep; /*!< Geoid separation in units of meters */
uint8_t sats_in_use; /*!< Number of satellites currently in use */
uint8_t fix; /*!< Type of current fix, `0` = Invalid, `1` = GPS fix, `2` = Differential GPS fix */
uint8_t hours; /*!< Current UTC hours */
uint8_t minutes; /*!< Current UTC minutes */
uint8_t seconds; /*!< Current UTC seconds */
} gga; /*!< GPGGA message */
#endif /* LWGPS_CFG_STATEMENT_GPGGA */
#if LWGPS_CFG_STATEMENT_GPGSA
struct {
lwgps_float_t dop_h; /*!< Horizontal dilution of precision */
lwgps_float_t dop_v; /*!< Vertical dilution of precision */
lwgps_float_t dop_p; /*!< Position dilution of precision */
uint8_t systemId;
uint8_t fix_mode; /*!< Fix mode, `1` = No fix, `2` = 2D fix, `3` = 3D fix */
uint8_t satellites_ids[12]; /*!< IDs of satellites currently in use */
} gsa; /*!< GPGSA message */
#endif /* LWGPS_CFG_STATEMENT_GPGSA */
#if LWGPS_CFG_STATEMENT_GPGSV
struct {
uint8_t sats_in_view; /*!< Number of stallites in view */
uint8_t stat_num; /*!< Satellite line number during parsing GPGSV data */
} gsv; /*!< GPGSV message */
#endif /* LWGPS_CFG_STATEMENT_GPGSV */
#if LWGPS_CFG_STATEMENT_GPRMC
struct {
uint8_t is_valid; /*!< Status whether GPS status is valid or not */
uint8_t date; /*!< Current UTC date */
uint8_t month; /*!< Current UTC month */
uint8_t year; /*!< Current UTC year */
lwgps_float_t speed; /*!< Current spead over the ground in knots */
lwgps_float_t course; /*!< Current course over ground */
lwgps_float_t variation; /*!< Current magnetic variation in degrees */
} rmc; /*!< GPRMC message */
#endif /* LWGPS_CFG_STATEMENT_GPRMC */
#if LWGPS_CFG_STATEMENT_PUBX_POSITION
struct {
int32_t latitude; /*!< Latitude in units of degrees */
int32_t longitude; /*!< Longitude in units of degrees */
uint16_t dop_h; /*!< Horizontal dilution of precision */
uint16_t dop_v; /*!< Vertical dilution of precision */
uint32_t acc_h;
uint32_t acc_v;
uint16_t speed; /*!< Ground speed in knots */
uint16_t course; /*!< Ground coarse */
} position; /*!< PUBX TIME message */
#endif /* LWGPS_CFG_STATEMENT_PUBX_POSITION */
#if LWGPS_CFG_STATEMENT_PUBX_TIME
struct {
uint8_t hours; /*!< Current UTC hours */
uint8_t minutes; /*!< Current UTC minutes */
uint8_t seconds; /*!< Current UTC seconds */
uint8_t date; /*!< Current UTC date */
uint8_t month; /*!< Current UTC month */
uint8_t year; /*!< Current UTC year */
lwgps_float_t utc_tow; /*!< UTC TimeOfWeek, eg 113851.00 */
uint16_t utc_wk; /*!< UTC week number, continues beyond 1023 */
uint8_t leap_sec; /*!< UTC leap seconds; UTC + leap_sec = TAI */
uint32_t clk_bias; /*!< Receiver clock bias, eg 1930035 */
lwgps_float_t clk_drift; /*!< Receiver clock drift, eg -2660.664 */
uint32_t tp_gran; /*!< Time pulse granularity, eg 43 */
} time; /*!< PUBX TIME message */
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME */
} data; /*!< Union with data for each information */
} p; /*!< Structure with private data */
#endif /* !__DOXYGEN__ */
} lwgps_t;
/**
* \brief List of optional speed transformation from GPS values (in knots)
*/
typedef enum {
/* Metric values */
LWGPS_SPEED_KPS, /*!< Kilometers per second */
LWGPS_SPEED_KPH, /*!< Kilometers per hour */
LWGPS_SPEED_MPS, /*!< Meters per second */
LWGPS_SPEED_MPM, /*!< Meters per minute */
/* Imperial values */
LWGPS_SPEED_MIPS, /*!< Miles per second */
LWGPS_SPEED_MPH, /*!< Miles per hour */
LWGPS_SPEED_FPS, /*!< Foots per second */
LWGPS_SPEED_FPM, /*!< Foots per minute */
/* Optimized for runners/joggers */
LWGPS_SPEED_MPK, /*!< Minutes per kilometer */
LWGPS_SPEED_SPK, /*!< Seconds per kilometer */
LWGPS_SPEED_SP100M, /*!< Seconds per 100 meters */
LWGPS_SPEED_MIPM, /*!< Minutes per mile */
LWGPS_SPEED_SPM, /*!< Seconds per mile */
LWGPS_SPEED_SP100Y, /*!< Seconds per 100 yards */
/* Nautical values */
LWGPS_SPEED_SMPH, /*!< Sea miles per hour */
} lwgps_speed_t;
#define lwgps_speed_kps LWGPS_SPEED_KPS /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_kph LWGPS_SPEED_KPH /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mps LWGPS_SPEED_MPS /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mpm LWGPS_SPEED_MPM /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mips LWGPS_SPEED_MIPS /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mph LWGPS_SPEED_MPH /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_fps LWGPS_SPEED_FPS /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_fpm LWGPS_SPEED_FPM /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mpk LWGPS_SPEED_MPK /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_spk LWGPS_SPEED_SPK /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_sp100m LWGPS_SPEED_SP100M /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_mipm LWGPS_SPEED_MIPM /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_spm LWGPS_SPEED_SPM /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_sp100y LWGPS_SPEED_SP100Y /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
#define lwgps_speed_smph LWGPS_SPEED_SMPH /*!< Backward compatibility. \deprecated Use \ref lwgps_speed_t instead */
/**
* \brief Signature for caller-suplied callback function from gps_process
* \param[in] res: statement type of recently parsed statement
*/
typedef void (*lwgps_process_fn)(lwgps_statement_t res);
/**
* \brief Check if current GPS data contain valid signal
* \note \ref LWGPS_CFG_STATEMENT_GPRMC must be enabled and `GPRMC` statement must be sent from GPS receiver
* \param[in] _gh: GPS handle
* \return `1` on success, `0` otherwise
*/
#if LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__
#define lwgps_is_valid(_gh) ((_gh)->is_valid)
#else
#define lwgps_is_valid(_gh) (0)
#endif /* LWGPS_CFG_STATEMENT_GPRMC || __DOXYGEN__ */
uint8_t lwgps_init(lwgps_t* gh);
#if LWGPS_CFG_STATUS || __DOXYGEN__
uint8_t lwgps_process(lwgps_t* gh, const void* data, size_t len, lwgps_process_fn evt_fn);
#else /* LWGPS_CFG_STATUS */
uint8_t lwgps_process(lwgps_t* gh, const void* data, size_t len);
#endif /* !LWGPS_CFG_STATUS */
uint8_t lwgps_distance_bearing(lwgps_float_t las, lwgps_float_t los, lwgps_float_t lae, lwgps_float_t loe,
lwgps_float_t* d, lwgps_float_t* b);
lwgps_float_t lwgps_to_speed(lwgps_float_t sik, lwgps_speed_t ts);
/**
* \}
*/
#ifdef __cplusplus
}
#endif /* __cplusplus */
#endif /* LWGPS_HDR_H */

199
src/lwgps_opt.h Normal file
View file

@ -0,0 +1,199 @@
/**
* \file lwgps_opt.h
* \brief LwGPS options
*/
/*
* Copyright (c) 2023 Tilen MAJERLE
*
* 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.
*
* This file is part of LwGPS - Lightweight GPS NMEA parser library.
*
* Author: Tilen MAJERLE <tilen@majerle.eu>
* Version: $2.1.0$
*/
#ifndef LWGPS_OPT_HDR_H
#define LWGPS_OPT_HDR_H
/* Uncomment to ignore user options (or set macro in compiler flags) */
/* #define LWGPS_IGNORE_USER_OPTS */
/* Include application options */
//#ifndef LWGPS_IGNORE_USER_OPTS
//#include "lwgps_opts.h"
//#endif /* LWGPS_IGNORE_USER_OPTS */
#ifdef __cplusplus
extern "C" {
#endif /* __cplusplus */
/**
* \defgroup LWGPS_OPT Configuration
* \brief Default configuration setup
* \{
*/
/**
* \brief Enables `1` or disables `0` `double precision` for floating point
* values such as latitude, longitude, altitude.
*
* `double` is used as variable type when enabled, `float` when disabled.
*/
#ifndef LWGPS_CFG_DOUBLE
#define LWGPS_CFG_DOUBLE 0
#endif
/**
* \brief Enables `1` or disables `0` status reporting callback
* by \ref lwgps_process
*
* \note This is an extension, so not enabled by default.
*/
#ifndef LWGPS_CFG_STATUS
#define LWGPS_CFG_STATUS 0
#endif
/**
* \brief Enables `1` or disables `0` `GGA` statement parsing.
*
* \note This statement must be enabled to parse:
* - Latitude, Longitude, Altitude
* - Number of satellites in use, fix (no fix, GPS, DGPS), UTC time
*/
#ifndef LWGPS_CFG_STATEMENT_GPGGA
#define LWGPS_CFG_STATEMENT_GPGGA 1
#endif
/**
* \brief Enables `1` or disables `0` `GSA` statement parsing.
*
* \note This statement must be enabled to parse:
* - Position/Vertical/Horizontal dilution of precision
* - Fix mode (no fix, 2D, 3D fix)
* - IDs of satellites in use
*/
#ifndef LWGPS_CFG_STATEMENT_GPGSA
#define LWGPS_CFG_STATEMENT_GPGSA 1
#endif
/**
* \brief Enables `1` or disables `0` `RMC` statement parsing.
*
* \note This statement must be enabled to parse:
* - Validity of GPS signal
* - Ground speed in knots and coarse in degrees
* - Magnetic variation
* - UTC date
*/
#ifndef LWGPS_CFG_STATEMENT_GPRMC
#define LWGPS_CFG_STATEMENT_GPRMC 0
#endif
/**
* \brief Enables `1` or disables `0` `GSV` statement parsing.
*
* \note This statement must be enabled to parse:
* - Number of satellites in view
* - Optional details of each satellite in view. See \ref LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
*/
#ifndef LWGPS_CFG_STATEMENT_GPGSV
#define LWGPS_CFG_STATEMENT_GPGSV 0
#endif
/**
* \brief Enables `1` or disables `0` detailed parsing of each
* satellite in view for `GSV` statement.
*
* \note When this feature is disabled, only number of "satellites in view" is parsed
*/
#ifndef LWGPS_CFG_STATEMENT_GPGSV_SAT_DET
#define LWGPS_CFG_STATEMENT_GPGSV_SAT_DET 0
#endif
/**
* \brief Enables `1` or disables `0` parsing and generation
* of PUBX (uBlox) messages
*
* PUBX are a nonstandard ublox-specific extensions,
* so disabled by default.
*/
#ifndef LWGPS_CFG_STATEMENT_PUBX
#define LWGPS_CFG_STATEMENT_PUBX 1
#endif
/**
* \brief Enables `1` or disables `0` parsing and generation
* of PUBX (uBlox) TIME messages.
*
* \note TIME messages can be used to obtain:
* - UTC time of week
* - UTC week number
* - Leap seconds (allows conversion to eg. TAI)
*
* This is a nonstandard ublox-specific extension,
* so disabled by default.
*
* This configure option requires LWGPS_CFG_STATEMENT_PUBX
*/
#ifndef LWGPS_CFG_STATEMENT_PUBX_TIME
#define LWGPS_CFG_STATEMENT_PUBX_TIME 1
#endif
#ifndef LWGPS_CFG_STATEMENT_PUBX_POSITION
#define LWGPS_CFG_STATEMENT_PUBX_POSITION 1
#endif
/**
* \brief Enables `1` or disables `0` CRC calculation and check
*
* \note When not enabled, CRC check is ignored
*/
#ifndef LWGPS_CFG_CRC
#define LWGPS_CFG_CRC 1
#endif
/**
* \brief Enables `1` or disables `0` distance and bearing calculation
*
* \note When not enabled, corresponding function is disabled
*/
#ifndef LWESP_CFG_DISTANCE_BEARING
#define LWESP_CFG_DISTANCE_BEARING 0
#endif
/* Guard against accidental parser breakage */
#if LWGPS_CFG_STATEMENT_PUBX_TIME && !LWGPS_CFG_STATEMENT_PUBX
#error LWGPS_CFG_STATEMENT_PUBX must be enabled when enabling LWGPS_CFG_STATEMENT_PUBX_TIME
#endif /* LWGPS_CFG_STATEMENT_PUBX_TIME && !LWGPS_CFG_STATEMENT_PUBX */
#if LWGPS_CFG_STATEMENT_PUBX_POSITION && !LWGPS_CFG_STATEMENT_PUBX
#error LWGPS_CFG_STATEMENT_PUBX must be enabled when enabling LWGPS_CFG_STATEMENT_PUBX_POSITION
#endif /* LWGPS_CFG_STATEMENT_PUBX_POSITION && !LWGPS_CFG_STATEMENT_PUBX */
/**
* \}
*/
#ifdef __cplusplus
}
#endif /* __cplusplus */
#endif /* LWGPS_OPT_HDR_H */

764
src/main.c Normal file
View file

@ -0,0 +1,764 @@
/*
* Copyright (c) 2022 Libre Solar Technologies GmbH
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <zephyr/bluetooth/services/bas.h>
#include <math.h>
#include <zephyr/device.h>
#include <zephyr/devicetree.h>
#include <zephyr/drivers/gpio.h>
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(main);
#include <string.h>
#include "gps.h"
#include "bme280.h"
#include "ruuvi.h"
#include "battery.h"
#define STACKSIZE 2048
#define PRIORITY 7
#define LED0_NODE DT_ALIAS(led0)
static const struct gpio_dt_spec led0 = GPIO_DT_SPEC_GET(LED0_NODE, gpios);
#define LED1_NODE DT_ALIAS(led1)
static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(LED1_NODE, gpios);
#define LED2_NODE DT_ALIAS(led2)
static const struct gpio_dt_spec led2 = GPIO_DT_SPEC_GET(LED2_NODE, gpios);
static K_SEM_DEFINE(ble_init_ok, 0, 1);
bool btconnected = false;
bool btfirstadv = true;
#define BT_UUID_NUS_VAL BT_UUID_128_ENCODE(0x6E400001, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_RX_VAL BT_UUID_128_ENCODE(0x6E400002, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_TX_VAL BT_UUID_128_ENCODE(0x6E400003, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_PRESSURE_VAL BT_UUID_128_ENCODE(0x6E400004, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_TEMPERATURE_VAL BT_UUID_128_ENCODE(0x6E400005, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_LNS_LS_VAL 0x2A67
#define BT_UUID_LNS_PQ_VAL 0x2A69
#define BT_UUID_CUS_VAL 0x2000
#define BT_UUID_CUS_WRITE_VAL 0x2001
#define BT_UUID_CUS_SAT_VAL 0x2002
#define BT_UUID_CUS_CALIBRATE_VAL 0x2003
static uint8_t write_cus_buf[2];
uint8_t gpsmode = 1;
static ssize_t read_u32(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset);
static ssize_t read_s16(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset);
static ssize_t on_write_rx(struct bt_conn *conn, const struct bt_gatt_attr *attr, const void *buf, uint16_t len, uint16_t offset, uint8_t flags);
static ssize_t on_read_rx(struct bt_conn *conn, const struct bt_gatt_attr *attr, void *buf, uint16_t len, uint16_t offset);
static void nus_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value);
static void nus_ccc_cfg_changed2(const struct bt_gatt_attr *attr, uint16_t value);
static uint8_t nus_rx[26];
static uint8_t nus_tx_started;
static K_FIFO_DEFINE(fifo_btsendhisto);
struct btsendhisto_t {
void *fifo_reserved;
uint8_t type;
uint32_t from;
};
enum captor_type { BATTERY, TEMPERATURE, PRESSURE };
struct s_captor_params {
uint8_t wait_adv;
uint8_t wait_record;
};
struct s_captors {
uint8_t captor;
struct s_captor_params params;
};
struct s_captors captors_params[] = {
{ TEMPERATURE, {1, 3}},
{ PRESSURE, {6, 12}},
{ BATTERY, {12, 12}},
};
struct s_captors captors_state[] = {
{ TEMPERATURE, {0, 0}},
{ PRESSURE, {0, 0}},
{ BATTERY, {0, 0}},
};
int16_t temperature;
uint32_t pressure;
bool findifadv(uint8_t captor)
{
for(int i = 0; i < sizeof(captors_params) / sizeof(captors_params[0]); i++)
{
if(captors_params[i].captor == captor)
{
captors_state[i].params.wait_adv++;
if(captors_state[i].params.wait_adv == captors_params[i].params.wait_adv)
{
captors_state[i].params.wait_adv = 0;
return true;
} else {
return false;
}
}
}
return false;
}
bool findifrecord(uint8_t captor)
{
for(int i = 0; i < sizeof(captors_params) / sizeof(captors_params[0]); i++)
{
if(captors_params[i].captor == captor)
{
captors_state[i].params.wait_record++;
if(captors_state[i].params.wait_record == captors_params[i].params.wait_record)
{
captors_state[i].params.wait_record = 0;
return true;
} else {
return false;
}
}
}
return false;
}
void reset_adv_state()
{
for(int i = 0; i < sizeof(captors_state) / sizeof(captors_state[0]); i++)
{
captors_state[i].params.wait_adv = 0;
}
}
void temperature_notify(struct sensor_value *sval);
void pressure_notify(struct sensor_value *sval);
//void humidity_notify(struct sensor_value *sval);
void ruuvi_advertise();
void update_captors(bool force)
{
LOG_INF("Update Sensors");
bool temperature_toadv = false;
bool temperature_torecord = false;
bool pressure_toadv = false;
bool pressure_torecord = false;
bool battery_toadv = false;
bool battery_torecord = false;
bool ppressure = false;
bool ptemperature = false;
struct sensor_value sval;
if(btconnected == false)
{
temperature_toadv = findifadv(TEMPERATURE);
pressure_toadv = findifadv(PRESSURE);
battery_toadv = findifadv(BATTERY);
}
if(force == false)
{
temperature_torecord = findifrecord(TEMPERATURE);
pressure_torecord = findifrecord(PRESSURE);
battery_torecord = findifrecord(BATTERY);
}
if(temperature_torecord == true || temperature_toadv == true || force == true)
{
ptemperature = true;
}
if(pressure_torecord == true || pressure_toadv == true || force == true)
{
ppressure = true;
}
if(ptemperature == true || ppressure == true)
{
probe_bme280(ptemperature, false, ppressure);
}
if(temperature_toadv == true || force == true)
{
LOG_INF("ADV Temperature");
sval = get_temperature();
if(sval.val1 != 0 || sval.val2 != 0)
{
//temperature_notify(&sval);
ruuvi_updateTemperature(&sval);
ruuvi_advertise();
}
}
if(temperature_torecord == true && force == false)
{
LOG_INF("Record Temperature");
sval = get_temperature();
if(sval.val1 != 0 || sval.val2 != 0)
{
addRecord(TEMPERATURE, (sval.val1*100)+(int16_t)round(sval.val2/10000));
}
}
if(pressure_toadv == true || force == true)
{
LOG_INF("ADV Pressure");
sval = get_pressure();
if(sval.val1 != 0 || sval.val2 != 0)
{
//pressure_notify(&sval);
ruuvi_updatePressure(&sval);
ruuvi_advertise();
}
}
if(pressure_torecord == true && force == false)
{
LOG_INF("Record Pressure");
sval = get_pressure();
if(sval.val1 != 0 || sval.val2 != 0)
{
addRecord(PRESSURE, (sval.val1*100)+(int16_t)round(sval.val2/10000));
}
}
if(battery_toadv == true || force == true)
{
LOG_INF("ADV Battery");
uint16_t battery_millivolt = 0;
uint8_t battery_percentage = 0;
battery_get_millivolt(&battery_millivolt);
battery_get_percentage(&battery_percentage, battery_millivolt);
ruuvi_updateBattery(battery_millivolt);
ruuvi_advertise();
bt_bas_set_battery_level(battery_percentage);
}
}
static void work_captors_force_handler(struct k_work *work)
{
update_captors(true);
}
K_WORK_DEFINE(work_captors_force, work_captors_force_handler);
static void work_captors_handler(struct k_work *work)
{
update_captors(false);
}
K_WORK_DEFINE(work_captors, work_captors_handler);
void timer_captors_handler(struct k_timer *stm)
{
k_work_submit(&work_captors);
}
K_TIMER_DEFINE(timer_captors, timer_captors_handler, NULL);
static ssize_t on_write_cus(struct bt_conn *conn, const struct bt_gatt_attr *attr,
const void *buf, uint16_t len, uint16_t offset, uint8_t flags);
#define BT_UUID_LNS_LS BT_UUID_DECLARE_16(BT_UUID_LNS_LS_VAL)
#define BT_UUID_LNS_PQ BT_UUID_DECLARE_16(BT_UUID_LNS_PQ_VAL)
#define BT_UUID_CUS BT_UUID_DECLARE_16(BT_UUID_CUS_VAL)
#define BT_UUID_CUS_WRITE BT_UUID_DECLARE_16(BT_UUID_CUS_WRITE_VAL)
#define BT_UUID_CUS_SAT BT_UUID_DECLARE_16(BT_UUID_CUS_SAT_VAL)
#define BT_UUID_CUS_CALIBRATE BT_UUID_DECLARE_16(BT_UUID_CUS_CALIBRATE_VAL)
BT_GATT_SERVICE_DEFINE(lns_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_LNS),
BT_GATT_CHARACTERISTIC(BT_UUID_LNS_LS,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_LNS_PQ,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
BT_GATT_SERVICE_DEFINE(cus_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_CUS),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_WRITE,
BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP,
BT_GATT_PERM_WRITE,
NULL, on_write_cus, &write_cus_buf),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_SAT,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
BT_GATT_SERVICE_DEFINE(ess_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_ESS),
BT_GATT_CHARACTERISTIC(BT_UUID_TEMPERATURE,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ,
read_s16, NULL, &temperature),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_PRESSURE,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ,
read_u32, NULL, &pressure),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
#define BT_UUID_NUS BT_UUID_DECLARE_128(BT_UUID_NUS_VAL)
#define BT_UUID_NUS_RX BT_UUID_DECLARE_128(BT_UUID_NUS_RX_VAL)
#define BT_UUID_NUS_TX BT_UUID_DECLARE_128(BT_UUID_NUS_TX_VAL)
#define BT_UUID_NUS_PRESSURE BT_UUID_DECLARE_128(BT_UUID_NUS_PRESSURE_VAL)
#define BT_UUID_NUS_TEMPERATURE BT_UUID_DECLARE_128(BT_UUID_NUS_TEMPERATURE_VAL)
BT_GATT_SERVICE_DEFINE(nus_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_NUS),
/* RX */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_RX,
BT_GATT_CHRC_WRITE | BT_GATT_CHRC_WRITE_WITHOUT_RESP,
BT_GATT_PERM_READ | BT_GATT_PERM_WRITE,
on_read_rx, on_write_rx, &nus_rx),
/* TX */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_TX,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
/* Pressure */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_PRESSURE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed2, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
/* Temperature */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_TEMPERATURE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed2, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE)
);
static const struct bt_data sd[] = {
BT_DATA_BYTES(BT_DATA_UUID16_ALL,
BT_UUID_16_ENCODE(BT_UUID_BAS_VAL),
BT_UUID_16_ENCODE(BT_UUID_DIS_VAL),
BT_UUID_16_ENCODE(BT_UUID_ESS_VAL),
BT_UUID_16_ENCODE(BT_UUID_LNS_VAL),
BT_UUID_16_ENCODE(BT_UUID_CUS_VAL)),
BT_DATA_BYTES(BT_DATA_UUID128_ALL,
BT_UUID_NUS_VAL),
};
static void nus_ccc_cfg_changed2(const struct bt_gatt_attr *attr, uint16_t value)
{
LOG_WRN("%s: handle: %d value: %d", __func__, attr->handle, value);
//nus_tx_started = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
}
static ssize_t read_u32(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset)
{
const uint32_t *u32 = attr->user_data;
uint32_t value = sys_cpu_to_le32(*u32);
return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
sizeof(value));
}
static ssize_t read_s16(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset)
{
const int16_t *s16 = attr->user_data;
int16_t value = sys_cpu_to_le16(*s16);
return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
sizeof(value));
}
static void work_notify_bme280_handler(struct k_work *work)
{
struct sensor_value sval;
probe_bme280(true, false, true);
LOG_INF("Notify Pressure");
sval = get_pressure();
if(sval.val1 != 0 || sval.val2 != 0)
{
pressure_notify(&sval);
}
LOG_INF("Notify Temperature");
sval = get_temperature();
if(sval.val1 != 0 || sval.val2 != 0)
{
temperature_notify(&sval);
}
}
K_WORK_DEFINE(work_notify_bme280, work_notify_bme280_handler);
void timer_notify_bme280_handler(struct k_timer *stm)
{
k_work_submit(&work_notify_bme280);
}
K_TIMER_DEFINE(timer_notify_bme280, timer_notify_bme280_handler, NULL);
static void connected(struct bt_conn *conn, uint8_t err)
{
if (err) {
printk("Connection failed (err 0x%02x)\n", err);
} else {
printk("Connected\n");
btconnected = true;
k_timer_start(&timer_notify_bme280, K_NO_WAIT, K_SECONDS(10));
gpsPower(true);
}
}
static void disconnected(struct bt_conn *conn, uint8_t reason)
{
printk("Disconnected (reason 0x%02x)\n", reason);
btconnected = false;
k_timer_stop(&timer_notify_bme280);
reset_adv_state();
update_captors(true);
gpsPower(false);
}
BT_CONN_CB_DEFINE(conn_callbacks) = {
.connected = connected,
.disconnected = disconnected,
};
static void auth_passkey_display(struct bt_conn *conn, unsigned int passkey)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
LOG_INF("Passkey for %s: %06u", addr, passkey);
}
static void auth_cancel(struct bt_conn *conn)
{
char addr[BT_ADDR_LE_STR_LEN];
bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
LOG_INF("Pairing cancelled: %s", addr);
}
static struct bt_conn_auth_cb auth_cb_display = {
.passkey_display = auth_passkey_display,
.cancel = auth_cancel,
.pairing_confirm = NULL,
// .pairing_complete = pairing_complete,
// .pairing_failed = pairing_failed
};
static ssize_t on_write_rx(struct bt_conn *conn, const struct bt_gatt_attr *attr,
const void *buf, uint16_t len, uint16_t offset, uint8_t flags)
{
uint8_t * value = attr->user_data;
if (offset + len > sizeof(nus_rx)) {
LOG_ERR("%s: INVALID OFFSET: offset(%d) + len(%d) > sizeof(nus_rx) %d ",
__func__, offset, len, sizeof(nus_rx));
return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
}
memcpy(value + offset, buf, len);
if(len == 11)
{
struct btsendhisto_t bthisto;
uint32_t ts;
ts = value[10] + (value[9] << 8) + (value[8] << 16) + (value[7] << 24);
LOG_INF("Send histo from %d header %d %d %d", ts, value[0], value[1], value[2]);
//btsendhisto(ts, value[0]);
bthisto.from = ts;
bthisto.type = value[0];
size_t size = sizeof(struct btsendhisto_t);
char *mem_ptr = k_malloc(size);
memcpy(mem_ptr, &bthisto, size);
//bt_gatt_notify_uuid(NULL, BT_UUID_NUS_TX, nus_svc.attrs, &bttosend, sizeof(bttosend));
k_fifo_put(&fifo_btsendhisto, mem_ptr);
LOG_INF("End send histo");
}
return len;
}
static ssize_t on_read_rx(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset)
{
LOG_INF("%s", __func__);
return bt_gatt_attr_read(conn, attr, buf, len, offset, &nus_rx, sizeof(nus_rx));
}
static void nus_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
LOG_WRN("%s: value: %d", __func__, value);
nus_tx_started = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
}
void btsendhisto_thread()
{
for (;;) {
struct btsendhisto_t *request = k_fifo_get(&fifo_btsendhisto,
K_FOREVER);
uint8_t bttosend[11];
bttosend[0] = request->type;
bttosend[2] = 0x10;
for(int i=0;i<SIZE_DB;i++)
{
struct s_record r = database.db[i];
if(r.timestamp > request->from && ((request->type == 0x30 && r.type == TEMPERATURE) || (request->type == 0x32 && r.type == PRESSURE) || request->type == 0x3a))
{
bttosend[6] = r.timestamp;
bttosend[5] = r.timestamp >> 8;
bttosend[4] = r.timestamp >> 16;
bttosend[3] = r.timestamp >> 24;
int32_t s32 = (int32_t) r.value;
if(r.type == TEMPERATURE)
{
bttosend[1] = 0x30;
} else if(r.type == PRESSURE)
{
bttosend[1] = 0x32;
s32 = s32*10;
}
bttosend[10] = s32;
bttosend[9] = s32 >> 8;
bttosend[8] = s32 >> 16;
bttosend[7] = s32 >> 24;
LOG_INF("SRec %d ts %u type %d value %d", i, r.timestamp, r.type, r.value);
bt_gatt_notify_uuid(NULL, BT_UUID_NUS_TX, nus_svc.attrs, &bttosend, sizeof(bttosend));
k_msleep(50);
}
}
bttosend[1] = request->type;
bttosend[3] = 0xff;
bttosend[4] = 0xff;
bttosend[5] = 0xff;
bttosend[6] = 0xff;
bttosend[7] = 0xff;
bttosend[8] = 0xff;
bttosend[9] = 0xff;
bttosend[10] = 0xff;
bt_gatt_notify_uuid(NULL, BT_UUID_NUS_TX, nus_svc.attrs, &bttosend, sizeof(bttosend));
k_free(request);
}
}
K_THREAD_DEFINE(btsendhisto_thread_id, STACKSIZE, btsendhisto_thread, NULL, NULL,
NULL, PRIORITY, 0, 0);
static ssize_t on_write_cus(struct bt_conn *conn, const struct bt_gatt_attr *attr,
const void *buf, uint16_t len, uint16_t offset, uint8_t flags)
{
int8_t * value = attr->user_data;
if (offset + len > sizeof(write_cus_buf)) {
LOG_ERR("%s: INVALID OFFSET: offset(%d) + len(%d) > sizeof(auth_buf) %d ",
__func__, offset, len, sizeof(write_cus_buf));
return BT_GATT_ERR(BT_ATT_ERR_INVALID_OFFSET);
}
memcpy(value + offset, buf, len);
if(len == 2)
{
if(value[1] & 0x01) {
LOG_INF("Mode 1");
gpsBoost(false);
gpsmode = 1;
} else if(value[1] & 0x02) {
LOG_INF("Mode 2");
gpsmode = 2;
gpsBoost(true);
} else if(value[1] & 0x04) {
LOG_INF("Mode 3");
gpsmode = 3;
gpsBoost(true);
}
}
return len;
}
void ruuvi_advertise()
{
//LOG_HEXDUMP_INF(ruuvi_getPayload(), 16, "Ruuvi Payload:");
struct bt_data ad[] = {
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
BT_DATA(BT_DATA_MANUFACTURER_DATA, ruuvi_getPayload(), 16),
BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1)
};
if(btfirstadv == true)
{
bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE, BT_GAP_ADV_FAST_INT_MIN_2, BT_GAP_ADV_FAST_INT_MAX_2, NULL), ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
btfirstadv = false;
} else {
bt_le_adv_update_data(ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd));
}
}
void temperature_notify(struct sensor_value *sval)
{
int16_t tnotifynus, tnotifyess;
tnotifynus = sys_cpu_to_be16((sval->val1*10)+(int16_t)round(sval->val2/100000));
bt_gatt_notify_uuid(NULL, BT_UUID_NUS_TEMPERATURE, nus_svc.attrs, &tnotifynus, sizeof(tnotifynus));
temperature = (sval->val1*100)+(int16_t)round(sval->val2/10000);
tnotifyess = sys_cpu_to_le16(temperature);
bt_gatt_notify_uuid(NULL, BT_UUID_TEMPERATURE, ess_svc.attrs, &tnotifyess, sizeof(tnotifyess));
}
void pressure_notify(struct sensor_value *sval)
{
uint32_t pnotifynus, pnotifyess;
pnotifynus = sys_cpu_to_be32((sval->val1*1000)+(int16_t)round(sval->val2/1000));
bt_gatt_notify_uuid(NULL, BT_UUID_NUS_PRESSURE, nus_svc.attrs, &pnotifynus, sizeof(pnotifynus));
pressure = (sval->val1*10000)+(int16_t)round(sval->val2/100);
pnotifyess = sys_cpu_to_le32(pressure);
bt_gatt_notify_uuid(NULL, BT_UUID_PRESSURE, ess_svc.attrs, &pnotifyess, sizeof(pnotifyess));
}
/* void humidity_notify(struct sensor_value *sval)
{
uint16_t hnotifyess;
humidity = (sval->val1*100)+(uint16_t)round(sval->val2/10000);
hnotifyess = sys_cpu_to_le16(humidity);
bt_gatt_notify_uuid(NULL, BT_UUID_HUMIDITY, ess_svc.attrs, &hnotifyess, sizeof(hnotifyess));
} */
void publish_bt_thread()
{
k_sem_take(&ble_init_ok, K_FOREVER);
for (;;) {
k_sem_take(&gps_data_tosend, K_FOREVER);
while(k_fifo_is_empty(&fifo_gps_data) == 0) {
struct gps_data_t *request = k_fifo_get(&fifo_gps_data, K_NO_WAIT);
uint8_t btgpsls[20];
memcpy(&btgpsls[0], &(request->speed), 2 );
memcpy(&btgpsls[2], &(request->latitude), 4 );
memcpy(&btgpsls[6], &(request->longitude), 4 );
memcpy(&btgpsls[10], &(request->acc_h), 4 );
memcpy(&btgpsls[14], &(request->altitude), 4 );
memcpy(&btgpsls[18], &(request->course), 2 );
LOG_INF("Notify GPS Data LS");
bt_gatt_notify_uuid(NULL, BT_UUID_LNS_LS, lns_svc.attrs, &btgpsls, sizeof(btgpsls));
if(gpsmode == 3) {
uint8_t btgpsverb[14];
uint8_t hdop=(uint8_t) round(request->dop_h/2);
uint8_t vdop=(uint8_t) round(request->dop_v/2);
memcpy(&btgpsverb[0], &(request->acc_h), 4 );
memcpy(&btgpsverb[4], &(request->acc_v), 4 );
memcpy(&btgpsverb[8], &hdop, 1 );
memcpy(&btgpsverb[9], &vdop, 1 );
memcpy(&btgpsverb[10], &(request->altitude), 4 );
LOG_INF("Notify GPS Data PQ");
bt_gatt_notify_uuid(NULL, BT_UUID_LNS_PQ, lns_svc.attrs, &btgpsverb, sizeof(btgpsverb));
}
k_free(request);
}
while(k_fifo_is_empty(&fifo_gps_sat_data) == 0) {
struct gps_sat_data_t *request = k_fifo_get(&fifo_gps_sat_data, K_NO_WAIT);
if(gpsmode == 3) {
uint8_t btgpssat[] = {request->gps, request->glonass, request->galileo};
LOG_INF("Notify GPS Sat");
bt_gatt_notify_uuid(NULL, BT_UUID_CUS_SAT, cus_svc.attrs, &btgpssat, sizeof(btgpssat));
}
k_free(request);
}
}
}
K_THREAD_DEFINE(publish_bt_id, STACKSIZE, publish_bt_thread, NULL, NULL, NULL, PRIORITY, 0, 0);
void led_init()
{
gpio_pin_configure_dt(&led0, GPIO_OUTPUT_INACTIVE);
gpio_pin_configure_dt(&led1, GPIO_OUTPUT_INACTIVE);
gpio_pin_configure_dt(&led2, GPIO_OUTPUT_INACTIVE);
}
void led_test()
{
gpio_pin_toggle_dt(&led0);
k_msleep(350);
gpio_pin_toggle_dt(&led0);
gpio_pin_toggle_dt(&led1);
k_msleep(350);
gpio_pin_toggle_dt(&led1);
gpio_pin_toggle_dt(&led2);
k_msleep(350);
gpio_pin_toggle_dt(&led2);
}
int main(void)
{
LOG_INF("Hello main");
led_init();
led_test();
k_sem_take(&gps_init_ok, K_FOREVER);
int err = bt_enable(NULL);
if (err) {
printk("Bluetooth init failed (err %d)\n", err);
return -1;
}
unsigned int passkey = 6768;
bt_passkey_set(passkey);
bt_conn_auth_cb_register(&auth_cb_display);
k_sem_give(&ble_init_ok);
battery_init();
init_bme280();
k_work_submit(&work_captors_force);
k_timer_start(&timer_captors, K_SECONDS(300), K_SECONDS(300));
//k_msleep(7500);
//gpsPower(false);
return 0;
}

546
src/main.c.save Normal file
View file

@ -0,0 +1,546 @@
/*
* Copyright (c) 2022 Libre Solar Technologies GmbH
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <zephyr/kernel.h>
#include <zephyr/device.h>
#include <zephyr/drivers/uart.h>
#include <zephyr/sys/byteorder.h>
#include <zephyr/bluetooth/bluetooth.h>
#include <zephyr/bluetooth/hci.h>
#include <zephyr/bluetooth/conn.h>
#include <zephyr/bluetooth/uuid.h>
#include <zephyr/bluetooth/gatt.h>
#include <string.h>
#include <SEGGER_RTT.h>
//#include "minmea.h"
#include "lwgps.h"
#define RTT_BUFFER_DOWN_SIZE 512
#define RTT_BUFFER_UP_SIZE 512
struct rtt_config_t {
void *up_buffer;
size_t up_size;
void *down_buffer;
size_t down_size;
uint8_t channel;
};
/* change this to any other UART peripheral if desired */
#define UART_DEVICE_NODE DT_CHOSEN(zephyr_shell_uart)
#define MSG_SIZE 128
/* queue to store up to 10 messages (aligned to 4-byte boundary) */
K_MSGQ_DEFINE(uart_msgq, MSG_SIZE, 10, 4);
static K_SEM_DEFINE(segger_init_ok, 0, 1);
static const struct device *const uart_dev = DEVICE_DT_GET(UART_DEVICE_NODE);
/* receive buffer used in UART ISR callback */
static char rx_buf[MSG_SIZE];
static int rx_buf_pos;
static char nmea_sentence[MSG_SIZE];
static int nmea_position = 0;
const unsigned char ubxNMEA[] = { 0x06, 0x17, 0x14, 0x00, 0x00, 0x41, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const unsigned char ubxGNSS[] = { 0x06, 0x3E, 0x3C, 0x00, 0x00, 0x20, 0x20, 0x07, 0x00, 0x08, 0x10, 0x00, 0x01, 0x00, 0x01, 0x01, 0x01, 0x01, 0x03, 0x00, 0x01, 0x00, 0x01, 0x01, 0x02, 0x04, 0x08, 0x00, 0x01, 0x00, 0x01, 0x01, 0x03, 0x08, 0x10, 0x00, 0x00, 0x00, 0x01, 0x01, 0x04, 0x00, 0x08, 0x00, 0x00, 0x00, 0x01, 0x03, 0x05, 0x00, 0x03, 0x00, 0x00, 0x00, 0x01, 0x05, 0x06, 0x08, 0x0E, 0x00, 0x01, 0x00, 0x01, 0x01 };
const unsigned char GNSSStart[] = { 0x06,0x04,0x04,0x00,0x00,0x00,0x09,0x00 };
const unsigned char GNSSStop[] = { 0x06,0x04,0x04,0x00,0x00,0x00,0x08,0x00 };
const unsigned char enablePUBX0[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00 };
const unsigned char disablePUBX0[] = { 0x06, 0x01, 0x08, 0x00, 0xf1, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
const char disableRMC[] = "PUBX,40,RMC,0,0,0,0,0,0";
const char disableGLL[] = "PUBX,40,GLL,0,0,0,0,0,0";
const char disableGSV[] = "PUBX,40,GSV,0,0,0,0,0,0";
const char disableGSA[] = "PUBX,40,GSA,0,0,0,0,0,0";
const char disableGGA[] = "PUBX,40,GGA,0,0,0,0,0,0";
const char disableVTG[] = "PUBX,40,VTG,0,0,0,0,0,0";
const char disableZDA[] = "PUBX,40,ZDA,0,0,0,0,0,0";
const char disableGNS[] = "PUBX,40,GNS,0,0,0,0,0,0";
//const char disableGBS[] PROGMEM = "PUBX,40,GBS,0,0,0,0,0,0";
const char disableGST[] = "PUBX,40,GST,0,0,0,0,0,0";
const char enableRMC[] = "PUBX,40,RMC,0,1,0,0,0,0";
const char enableGLL[] = "PUBX,40,GLL,0,1,0,0,0,0";
const char enableGSV[] = "PUBX,40,GSV,0,1,0,0,0,0";
const char enableGSA[] = "PUBX,40,GSA,0,1,0,0,0,0";
const char enableGGA[] = "PUBX,40,GGA,0,1,0,0,0,0";
const char enableVTG[] = "PUBX,40,VTG,0,1,0,0,0,0";
const char enableZDA[] = "PUBX,40,ZDA,0,1,0,0,0,0";
const char enableGNS[] = "PUBX,40,GNS,0,1,0,0,0,0";
//const char enableGBS[] PROGMEM = "PUBX,40,GBS,0,1,0,0,0,0";
const char enableGST[] = "PUBX,40,GST,0,1,0,0,0,0\r\n";
bool btconnected = false;
#define BT_UUID_NUS_VAL BT_UUID_128_ENCODE(0x6E400001, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_PRESSURE_VAL BT_UUID_128_ENCODE(0x6E400004, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_NUS_TEMPERATURE_VAL BT_UUID_128_ENCODE(0x6E400005, 0xB5A3, 0xF393, 0xE0A9, 0xE50E24DCCA9E)
#define BT_UUID_LNS_LS_VAL 0x2A67
#define BT_UUID_LNS_PQ_VAL 0x2A69
#define BT_UUID_CUS_VAL 0x2000
#define BT_UUID_CUS_WRITE_VAL 0x2001
#define BT_UUID_CUS_SAT_VAL 0x2002
#define BT_UUID_CUS_CALIBRATE_VAL 0x2003
#define BT_UUID_CUS_ADMIN_VAL 0x2004
static void nus_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value);
static ssize_t read_u32(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset);
static ssize_t read_s16(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset);
int16_t temperature;
uint32_t pressure;
static const struct bt_data ad[] = {
BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
BT_DATA_BYTES(BT_DATA_UUID16_ALL,
BT_UUID_16_ENCODE(BT_UUID_ESS_VAL),
BT_UUID_16_ENCODE(BT_UUID_LNS_VAL),
BT_UUID_16_ENCODE(BT_UUID_CUS_VAL)),
BT_DATA_BYTES(BT_DATA_UUID128_ALL,
BT_UUID_NUS_VAL),
};
BT_GATT_SERVICE_DEFINE(ess_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_ESS),
BT_GATT_CHARACTERISTIC(BT_UUID_TEMPERATURE,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ,
read_s16, NULL, &temperature),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_PRESSURE,
BT_GATT_CHRC_READ | BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_READ,
read_u32, NULL, &pressure),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
#define BT_UUID_NUS BT_UUID_DECLARE_128(BT_UUID_NUS_VAL)
#define BT_UUID_NUS_PRESSURE BT_UUID_DECLARE_128(BT_UUID_NUS_PRESSURE_VAL)
#define BT_UUID_NUS_TEMPERATURE BT_UUID_DECLARE_128(BT_UUID_NUS_TEMPERATURE_VAL)
BT_GATT_SERVICE_DEFINE(nus_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_NUS),
/* Pressure */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_PRESSURE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
/* Temperature */
BT_GATT_CHARACTERISTIC(BT_UUID_NUS_TEMPERATURE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
#define BT_UUID_LNS_LS BT_UUID_DECLARE_16(BT_UUID_LNS_LS_VAL)
#define BT_UUID_LNS_PQ BT_UUID_DECLARE_16(BT_UUID_LNS_PQ_VAL)
#define BT_UUID_CUS BT_UUID_DECLARE_16(BT_UUID_CUS_VAL)
#define BT_UUID_CUS_WRITE BT_UUID_DECLARE_16(BT_UUID_CUS_WRITE_VAL)
#define BT_UUID_CUS_SAT BT_UUID_DECLARE_16(BT_UUID_CUS_SAT_VAL)
#define BT_UUID_CUS_CALIBRATE BT_UUID_DECLARE_16(BT_UUID_CUS_CALIBRATE_VAL)
#define BT_UUID_CUS_ADMIN BT_UUID_DECLARE_16(BT_UUID_CUS_ADMIN_VAL)
BT_GATT_SERVICE_DEFINE(lns_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_LNS),
BT_GATT_CHARACTERISTIC(BT_UUID_LNS_LS,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_LNS_PQ,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
BT_GATT_SERVICE_DEFINE(cus_svc,
BT_GATT_PRIMARY_SERVICE(BT_UUID_CUS),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_WRITE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_SAT,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_CALIBRATE,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
BT_GATT_CHARACTERISTIC(BT_UUID_CUS_ADMIN,
BT_GATT_CHRC_NOTIFY,
BT_GATT_PERM_NONE,
NULL, NULL, NULL),
BT_GATT_CCC(nus_ccc_cfg_changed, BT_GATT_PERM_READ | BT_GATT_PERM_WRITE),
);
static void nus_ccc_cfg_changed(const struct bt_gatt_attr *attr, uint16_t value)
{
//LOG_WRN("%s: value: %d", __func__, value);
//nus_tx_started = (value == BT_GATT_CCC_NOTIFY) ? 1 : 0;
}
static ssize_t read_u32(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset)
{
const uint32_t *u32 = attr->user_data;
uint32_t value = sys_cpu_to_le32(*u32);
return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
sizeof(value));
}
static ssize_t read_s16(struct bt_conn *conn, const struct bt_gatt_attr *attr,
void *buf, uint16_t len, uint16_t offset)
{
const int16_t *s16 = attr->user_data;
int16_t value = sys_cpu_to_le16(*s16);
return bt_gatt_attr_read(conn, attr, buf, len, offset, &value,
sizeof(value));
}
/*
* Read characters from UART until line end is detected. Afterwards push the
* data to the message queue.
*/
void serial_cb_old(const struct device *dev, void *user_data)
{
uint8_t c;
if (!uart_irq_update(uart_dev)) {
return;
}
if (!uart_irq_rx_ready(uart_dev)) {
return;
}
/* read until FIFO empty */
while (uart_fifo_read(uart_dev, &c, 1) == 1) {
if ((c == '\n' || c == '\r') && rx_buf_pos > 0) {
/* terminate string */
rx_buf[rx_buf_pos] = '\0';
/* if queue is full, message is silently dropped */
k_msgq_put(&uart_msgq, &rx_buf, K_NO_WAIT);
/* reset the buffer (it was copied to the msgq) */
rx_buf_pos = 0;
} else if (rx_buf_pos < (sizeof(rx_buf) - 1)) {
rx_buf[rx_buf_pos++] = c;
}
/* else: characters beyond buffer size are dropped */
}
}
void print_segger(char *buf)
{
int msg_len = strlen(buf);
for (int i = 0; i < msg_len; i++) {
//uart_poll_out(uart_dev, buf[i]);
SEGGER_RTT_Write(1,&buf[i],1);
}
}
void print_segger_byte(char buf)
{
SEGGER_RTT_Write(1,&buf,1);
}
void serial_cb(const struct device *dev, void *user_data)
{
uint8_t c;
if (!uart_irq_update(uart_dev)) {
return;
}
if (!uart_irq_rx_ready(uart_dev)) {
return;
}
/* read until FIFO empty */
while (uart_fifo_read(uart_dev, &c, 1) == 1) {
if(c == '$')
{
nmea_position = 0;
nmea_sentence[nmea_position] = c;
nmea_position++;
} else if(nmea_position > 0)
{
//if((nmea_position == 1 && c != 'G') || (nmea_position == 2 && (c != 'L' && c != 'N' && c != 'P')) || (nmea_position == 6 && c!= ','))
//{
// nmea_position = 0;
//}
if(nmea_position < (sizeof(nmea_sentence) -1))
{
nmea_sentence[nmea_position] = c;
nmea_position++;
if(c == '\n')
{
nmea_sentence[nmea_position] = '\0';
k_msgq_put(&uart_msgq, &nmea_sentence, K_NO_WAIT);
nmea_position = 0;
}
} else {
printk("overflow\n");
nmea_position = 0;
}
}
print_segger_byte(c);
}
}
static char formatHex( uint8_t val )
{
val &= 0x0F;
return (val >= 10) ? ((val - 10) + 'A') : (val + '0');
}
void print_uart_byte(char buf)
{
uart_poll_out(uart_dev, buf);
//SEGGER_RTT_Write(1,&buf[i],1);
}
void print_uart(char *buf)
{
int msg_len = strlen(buf);
for (int i = 0; i < msg_len; i++) {
uart_poll_out(uart_dev, buf[i]);
//SEGGER_RTT_Write(1,&buf[i],1);
}
}
void print_uart_const(const char *buf)
{
int msg_len = strlen(buf);
print_uart_byte('$');
uint8_t crc = 0;
for (int i = 0; i < msg_len; i++) {
print_uart_byte(buf[i]);
//printk("%c", buf[i]);
//SEGGER_RTT_Write(1,&buf[i],1);
crc ^= buf[i];
}
print_uart_byte('*');
print_uart_byte(formatHex(crc>>4));
print_uart_byte(formatHex(crc));
print_uart_byte('\r');
print_uart_byte('\n');
}
void sendUBX( const unsigned char *progmemBytes, size_t len)
{
/*if(wakeup == true)
{
gpsPort.write( 0xFF );
delay( 500 );
}*/
print_uart_byte( 0xB5 ); // SYNC1
print_uart_byte( 0x62 ); // SYNC2
uint8_t a = 0, b = 0;
while (len-- > 0) {
uint8_t c = *(progmemBytes++);
a += c;
b += a;
print_uart_byte( c );
}
print_uart_byte( a ); // CHECKSUM A
print_uart_byte( b ); // CHECKSUM B
} // sendUBX
void init_gps()
{
//sendUBX(GNSSStart, sizeof(GNSSStart));
//k_msleep(100);
sendUBX(ubxNMEA, sizeof(ubxNMEA));
k_msleep(100);
sendUBX(ubxGNSS, sizeof(ubxGNSS));
k_msleep(100);
print_uart_const(disableRMC);
k_msleep(100);
print_uart_const(disableGSV);
k_msleep(100);
print_uart_const(disableVTG);
k_msleep(100);
print_uart_const(disableGLL);
k_msleep(100);
print_uart_const(enableGGA);
k_msleep(100);
print_uart_const(enableGSA);
k_msleep(100);
sendUBX(enablePUBX0, sizeof(enablePUBX0));
}
void segger_read_ep(void *, void *, void *)
{
/* indefinitely wait for input from the user */
k_sem_take(&segger_init_ok, K_FOREVER);
while (1) {
char tx_buf;
unsigned numbytes = SEGGER_RTT_Read(1, &tx_buf, 1);
if(numbytes > 0)
{
//print_uart("Echo: ");
/*for(int i = numbytes; i < MSG_SIZE; i++)
{
tx_buf[i] = '\0';
}*/
printk("Segger received %d bytes: %d\n", numbytes, tx_buf);
print_uart_byte(tx_buf);
//print_uart("\r\n");
}
}
}
#define MY_STACK_SIZE 500
#define MY_PRIORITY 5
K_THREAD_DEFINE(segger_read_tid, MY_STACK_SIZE,
segger_read_ep, NULL, NULL, NULL,
MY_PRIORITY, 0, 0);
static void connected(struct bt_conn *conn, uint8_t err)
{
if (err) {
printk("Connection failed (err 0x%02x)\n", err);
} else {
printk("Connected\n");
btconnected = true;
}
}
static void disconnected(struct bt_conn *conn, uint8_t reason)
{
printk("Disconnected (reason 0x%02x)\n", reason);
btconnected = false;
}
BT_CONN_CB_DEFINE(conn_callbacks) = {
.connected = connected,
.disconnected = disconnected,
};
void main(void)
{
char tx_buf[MSG_SIZE];
lwgps_t hgps;
struct rtt_config_t rtt_config;
rtt_config.up_size = RTT_BUFFER_UP_SIZE;
rtt_config.down_size = RTT_BUFFER_DOWN_SIZE;
rtt_config.up_buffer = k_malloc(RTT_BUFFER_UP_SIZE);
rtt_config.down_buffer = k_malloc(RTT_BUFFER_DOWN_SIZE);
rtt_config.channel = 1;
SEGGER_RTT_ConfigUpBuffer(rtt_config.channel, "RTT GPS", rtt_config.up_buffer, rtt_config.up_size, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
SEGGER_RTT_ConfigDownBuffer(rtt_config.channel, "RTT GPS", rtt_config.down_buffer, rtt_config.down_size, SEGGER_RTT_MODE_NO_BLOCK_SKIP);
k_sem_give(&segger_init_ok);
if (!device_is_ready(uart_dev)) {
printk("UART device not found!");
return;
}
init_gps();
lwgps_init(&hgps);
int err = bt_enable(NULL);
if (err) {
printk("Bluetooth init failed (err %d)\n", err);
return;
}
err = bt_le_adv_start(BT_LE_ADV_PARAM(BT_LE_ADV_OPT_CONNECTABLE | BT_LE_ADV_OPT_USE_NAME, BT_GAP_ADV_SLOW_INT_MIN, BT_GAP_ADV_SLOW_INT_MAX, NULL), ad, ARRAY_SIZE(ad), NULL, 0);
/* configure interrupt and callback to receive data */
int ret = uart_irq_callback_user_data_set(uart_dev, serial_cb, NULL);
if (ret < 0) {
if (ret == -ENOTSUP) {
printk("Interrupt-driven UART API support not enabled\n");
} else if (ret == -ENOSYS) {
printk("UART device does not support interrupt-driven API\n");
} else {
printk("Error setting UART callback: %d\n", ret);
}
return;
}
uart_irq_rx_enable(uart_dev);
//print_uart("Hello! I'm your echo bot.\r\n");
//print_uart("Tell me something and press enter:\r\n");
printk("Hello SEGGER\n");
/* indefinitely wait for input from the user */
while (k_msgq_get(&uart_msgq, &tx_buf, K_FOREVER) == 0) {
//print_uart("Echo: ");
//char talkerid[3];
//minmea_talker_id(&talkerid, tx_buf);
printk("NMEA Sentence: %s", tx_buf);
lwgps_process(&hgps, tx_buf, strlen(tx_buf));
if(hgps.p.stat == STAT_GSA) {
uint8_t sat_count = 0;
for(int i = 0; i < sizeof(hgps.satellites_ids); i++)
{
if(hgps.satellites_ids[i] != 0) { sat_count++; }
}
printk("systemId: %d %d\n", hgps.systemId, sat_count);
} else if(hgps.p.stat == STAT_GGA) { printk("Altitude: %d\n", (int32_t) (hgps.altitude*100));
} else if(hgps.p.stat == STAT_UBX_POSITION) {
printk("Position %d %d\n", hgps.latitude, hgps.longitude);
printk("DOP %d %d\n", hgps.dop_h, hgps.dop_v);
printk("Accuracy H:%d V:%d\n", hgps.acc_h, hgps.acc_v);
printk("Speed:%d Course:%d\n", hgps.speed, hgps.course);
}
//switch (minmea_sentence_id(tx_buf, false)) {
// case MINMEA_SENTENCE_GGA: {
//struct minmea_sentence_gga frame;
//if (minmea_parse_gga(&frame, tx_buf)) {
// printk("$xxGGA: fix quality: %d\n", frame.fix_quality);
//}
// } break;
//};
//print_uart("\r\n");
}
}

60
src/ruuvi.c Normal file
View file

@ -0,0 +1,60 @@
#include "ruuvi.h"
#include "calendar.h"
#include <zephyr/logging/log.h>
LOG_MODULE_REGISTER(ruuvi);
struct s_database database;
uint8_t mfg_ruuvi_data[] = { 0x99, 0x04, 0x03, 0x64, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0b, 0xb8 };
uint8_t* ruuvi_getPayload()
{
return mfg_ruuvi_data;
}
void ruuvi_updateTemperature(struct sensor_value *temperature)
{
int8_t truuvi1;
uint8_t truuvi2;
truuvi1 = temperature->val1;
truuvi2 = (uint8_t) (temperature->val2/10000);
mfg_ruuvi_data[4] = truuvi1;
mfg_ruuvi_data[5] = truuvi2;
}
void ruuvi_updatePressure(struct sensor_value *pressure)
{
uint32_t pruuvi;
pruuvi = (uint32_t)((pressure->val1*1000)+(pressure->val2/1000)-50000);
mfg_ruuvi_data[7] = pruuvi;
mfg_ruuvi_data[6] = pruuvi >> 8;
}
void ruuvi_updateBattery(uint16_t batt_mV)
{
mfg_ruuvi_data[15] = batt_mV;
mfg_ruuvi_data[14] = batt_mV >> 8;
}
void addRecord(uint8_t type, int16_t value)
{
if(TsOK() == true)
{
if(database.index == SIZE_DB) {
database.index = 0;
}
database.db[database.index].type = type;
database.db[database.index].value = value;
uint32_t ts = getTs();
database.db[database.index].timestamp = ts;
LOG_INF("Record index %d ts %u type %d value %d", database.index, ts, type, value);
database.index = database.index + 1;
}
}

28
src/ruuvi.h Normal file
View file

@ -0,0 +1,28 @@
#include <zephyr/types.h>
#include <zephyr/drivers/sensor.h>
#ifndef __RUUVI_H__
#define __RUUVI_H__
void addRecord(uint8_t type, int16_t value);
void ruuvi_updateTemperature(struct sensor_value *temperature);
void ruuvi_updatePressure(struct sensor_value *pressure);
void ruuvi_updateBattery(uint16_t batt_mV);
uint8_t* ruuvi_getPayload();
#define SIZE_DB 2160
struct s_record {
uint32_t timestamp;
uint8_t type;
int16_t value;
};
struct s_database {
uint16_t index;
struct s_record db[SIZE_DB];
};
extern struct s_database database;
#endif