kopia lustrzana https://github.com/PiInTheSky/lora-gateway
1084 wiersze
34 KiB
C
1084 wiersze
34 KiB
C
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
#include <string.h>
|
|
#include <time.h>
|
|
#include <inttypes.h>
|
|
|
|
#include "habpack.h"
|
|
#include "global.h"
|
|
#include "base64.h"
|
|
|
|
#include "cmp.h"
|
|
|
|
extern void LogMessage( const char *format, ... );
|
|
|
|
#define SECONDS_IN_A_DAY 86400
|
|
|
|
static int hb_buf_ptr = 0;
|
|
static int hb_ptr_max = 20;
|
|
|
|
static bool file_reader(cmp_ctx_t *ctx, void *data, size_t limit)
|
|
{
|
|
uint32_t i;
|
|
|
|
if (hb_buf_ptr + limit >= hb_ptr_max)
|
|
return false;
|
|
|
|
for(i = 0; i < limit; i++)
|
|
{
|
|
*(uint8_t *)data = ((uint8_t *)(ctx->buf))[hb_buf_ptr];
|
|
data++;
|
|
hb_buf_ptr++;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool file_skipper(struct cmp_ctx_s *ctx, size_t count)
|
|
{
|
|
if (hb_buf_ptr + count >= hb_ptr_max)
|
|
return false;
|
|
|
|
hb_buf_ptr += count;
|
|
|
|
return true;
|
|
}
|
|
|
|
static size_t file_writer(cmp_ctx_t *ctx, const void *data, size_t count)
|
|
{
|
|
uint32_t i;
|
|
|
|
if (hb_buf_ptr + count >= hb_ptr_max)
|
|
return 0;
|
|
|
|
for (i = 0; i < count; i++)
|
|
{
|
|
((char*)ctx->buf)[hb_buf_ptr] = *((uint8_t*)data);
|
|
data++;
|
|
hb_buf_ptr++;
|
|
}
|
|
|
|
return count;
|
|
}
|
|
|
|
static uint16_t CRC16(unsigned char *ptr)
|
|
{
|
|
uint16_t CRC;
|
|
int j;
|
|
|
|
CRC = 0xffff;
|
|
|
|
for ( ; *ptr; ptr++ )
|
|
{
|
|
CRC ^= ( ( ( unsigned int ) *ptr ) << 8 );
|
|
for ( j = 0; j < 8; j++ )
|
|
{
|
|
if ( CRC & 0x8000 )
|
|
CRC = ( CRC << 1 ) ^ 0x1021;
|
|
else
|
|
CRC <<= 1;
|
|
}
|
|
}
|
|
|
|
return CRC;
|
|
}
|
|
|
|
#define TRY_PARSE_STRING(processing) \
|
|
if(cmp_object_is_str(&item) \
|
|
&& cmp_object_to_str(&cmp, &item, str, str_max_length)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_FLOAT(processing) \
|
|
if(cmp_object_is_float(&item) \
|
|
&& cmp_object_as_float(&item, &tf32)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_DOUBLE(processing) \
|
|
if(cmp_object_is_double(&item) \
|
|
&& cmp_object_as_double(&item, &tf64)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_UNSIGNED_INTEGER(processing) \
|
|
if(cmp_object_is_uinteger(&item) \
|
|
&& cmp_object_as_uinteger(&item, &tu64)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_SIGNED_INTEGER(processing) \
|
|
if(cmp_object_is_sinteger(&item) \
|
|
&& cmp_object_as_sinteger(&item, &ts64)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_ARRAY(processing) \
|
|
if(cmp_object_is_array(&item) \
|
|
&& cmp_object_as_array(&item, &array_size)) \
|
|
{ \
|
|
for (j = 0; j < array_size; j++) \
|
|
{ \
|
|
/* Read array element */ \
|
|
if(!cmp_read_object(&cmp, &item)) \
|
|
return 0; \
|
|
\
|
|
processing \
|
|
} \
|
|
}
|
|
|
|
#define TRY_PARSE_ARRAY_AT_ONCE(processing) \
|
|
if(cmp_object_is_array(&item) \
|
|
&& cmp_object_as_array(&item, &array_size)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
#define TRY_PARSE_ARRAY_AT_ONCE_NESTED(processing) \
|
|
if(cmp_object_is_array(&item) \
|
|
&& cmp_object_as_array(&item, &array_size2)) \
|
|
{ \
|
|
processing \
|
|
}
|
|
|
|
void Habpack_telem_store_field(received_t *Received, uint64_t map_id, char *field_name, int64_t *ptr_integer, double *ptr_real, char *ptr_string)
|
|
{
|
|
habpack_telem_linklist_entry_t *walk_ptr, *last_ptr;
|
|
|
|
if(Received->Telemetry.habpack_extra == NULL)
|
|
{
|
|
/* First entry */
|
|
Received->Telemetry.habpack_extra = malloc(sizeof(habpack_telem_linklist_entry_t));
|
|
walk_ptr = Received->Telemetry.habpack_extra;
|
|
}
|
|
else
|
|
{
|
|
/* Not first entry, so walk */
|
|
last_ptr = Received->Telemetry.habpack_extra;
|
|
while(last_ptr->next != NULL)
|
|
{
|
|
last_ptr = last_ptr->next;
|
|
}
|
|
last_ptr->next = malloc(sizeof(habpack_telem_linklist_entry_t));
|
|
walk_ptr = last_ptr->next;
|
|
}
|
|
|
|
walk_ptr->type = map_id;
|
|
strncpy(walk_ptr->name, field_name, 31);
|
|
if(ptr_integer != NULL)
|
|
{
|
|
walk_ptr->value_type = HB_VALUE_INTEGER;
|
|
walk_ptr->value.integer = *ptr_integer;
|
|
}
|
|
else if(ptr_real != NULL)
|
|
{
|
|
walk_ptr->value_type = HB_VALUE_REAL;
|
|
walk_ptr->value.real = *ptr_real;
|
|
}
|
|
else if(ptr_string != NULL)
|
|
{
|
|
walk_ptr->value_type = HB_VALUE_STRING;
|
|
walk_ptr->value.string = strdup(ptr_string);
|
|
}
|
|
walk_ptr->next = NULL;
|
|
}
|
|
|
|
|
|
#define HABPACK_TELEM_STORE_AS_INT64(value) \
|
|
ts64 = (int64_t)value; \
|
|
Habpack_telem_store_field( \
|
|
Received, \
|
|
map_id, \
|
|
field_name, \
|
|
&ts64, \
|
|
NULL, \
|
|
NULL \
|
|
)
|
|
|
|
#define HABPACK_TELEM_STORE_AS_DOUBLE(value) \
|
|
tf64 = (double)value; \
|
|
Habpack_telem_store_field( \
|
|
Received, \
|
|
map_id, \
|
|
field_name, \
|
|
NULL, \
|
|
&tf64, \
|
|
NULL \
|
|
)
|
|
|
|
#define HABPACK_TELEM_STORE_AS_STRING(value) \
|
|
Habpack_telem_store_field( \
|
|
Received, \
|
|
map_id, \
|
|
field_name, \
|
|
NULL, \
|
|
NULL, \
|
|
value \
|
|
)
|
|
|
|
// sprintfs a real number without superfluous trailing zeroes.
|
|
// sprintf doesn't have that ability, hence this horrible little function
|
|
int snprintf_real(char *str, int max_len, double value)
|
|
{
|
|
int i, len;
|
|
|
|
len = snprintf(str, max_len, "%f,", value);
|
|
|
|
if (strchr(str, '.') != NULL)
|
|
{
|
|
// Just in case there's no dp in the resulting string. There should always be one (I think) but just in case ...
|
|
|
|
// Look backwards for first non-zero character, starting at the last digit in the string
|
|
for (i=len-2; i; i--)
|
|
{
|
|
if (str[i] != '0')
|
|
{
|
|
// Found a non zero
|
|
if (str[i] == '.')
|
|
{
|
|
// It's just zeroes after the dp, so we want to keep one zero, so step past that
|
|
i++;
|
|
}
|
|
|
|
// Add comma delimeter and null terminator, return the new length
|
|
str[++i] = ',';
|
|
str[++i] = '\0';
|
|
return i;
|
|
}
|
|
}
|
|
}
|
|
|
|
return len;
|
|
}
|
|
|
|
void Habpack_Telem_UKHAS_String(received_t *Received, char *ukhas_string, uint32_t max_length)
|
|
{
|
|
uint16_t crc;
|
|
uint32_t str_index;
|
|
habpack_telem_linklist_entry_t *walk_ptr;
|
|
|
|
/* $,CALLSIGN */
|
|
str_index = snprintf(
|
|
ukhas_string,
|
|
max_length,
|
|
"$$"
|
|
);
|
|
|
|
/* All other fields */
|
|
walk_ptr = Received->Telemetry.habpack_extra;
|
|
while(walk_ptr != NULL)
|
|
{
|
|
if(walk_ptr->value_type == HB_VALUE_INTEGER)
|
|
{
|
|
if(strcmp(walk_ptr->name, "time") == 0)
|
|
{
|
|
/* Convert time, urgh */
|
|
uint64_t temp;
|
|
temp = walk_ptr->value.integer % SECONDS_IN_A_DAY;
|
|
int hours = temp/(60*60);
|
|
temp -= (hours*60*60);
|
|
int mins = (temp/60);
|
|
int secs = temp-mins*60;
|
|
|
|
str_index += snprintf(
|
|
&ukhas_string[str_index],
|
|
(max_length - str_index),
|
|
"%02d:%02d:%02d,",
|
|
hours,mins,secs
|
|
);
|
|
}
|
|
else
|
|
{
|
|
str_index += snprintf(
|
|
&ukhas_string[str_index],
|
|
(max_length - str_index),
|
|
"%"PRId64",",
|
|
walk_ptr->value.integer
|
|
);
|
|
}
|
|
}
|
|
else if(walk_ptr->value_type == HB_VALUE_REAL)
|
|
{
|
|
str_index += snprintf_real(
|
|
&ukhas_string[str_index],
|
|
(max_length - str_index),
|
|
walk_ptr->value.real
|
|
);
|
|
}
|
|
else if(walk_ptr->value_type == HB_VALUE_STRING)
|
|
{
|
|
str_index += snprintf(
|
|
&ukhas_string[str_index],
|
|
(max_length - str_index),
|
|
"%s,",
|
|
walk_ptr->value.string
|
|
);
|
|
}
|
|
|
|
walk_ptr = walk_ptr->next;
|
|
}
|
|
|
|
/* Replace last comma with null */
|
|
ukhas_string[--str_index] = '\0';
|
|
|
|
crc = CRC16((unsigned char *)&ukhas_string[2]);
|
|
snprintf(&ukhas_string[str_index],
|
|
(max_length - str_index),
|
|
"*%04x\n",
|
|
crc
|
|
);
|
|
}
|
|
|
|
void Habpack_Telem_JSON(received_t *Received, char *json_string, uint32_t max_length)
|
|
{
|
|
uint32_t str_index;
|
|
habpack_telem_linklist_entry_t *walk_ptr;
|
|
|
|
str_index = snprintf(
|
|
json_string,
|
|
max_length,
|
|
"{\"type\":\"PAYLOAD_TELEMETRY\""
|
|
);
|
|
|
|
/* All other fields */
|
|
walk_ptr = Received->Telemetry.habpack_extra;
|
|
while(walk_ptr != NULL)
|
|
{
|
|
str_index += snprintf(
|
|
&json_string[str_index],
|
|
(max_length - str_index),
|
|
",\"%s\":",
|
|
walk_ptr->name
|
|
);
|
|
if(walk_ptr->value_type == HB_VALUE_INTEGER)
|
|
{
|
|
str_index += snprintf(
|
|
&json_string[str_index],
|
|
(max_length - str_index),
|
|
"%"PRId64,
|
|
walk_ptr->value.integer
|
|
);
|
|
}
|
|
else if(walk_ptr->value_type == HB_VALUE_REAL)
|
|
{
|
|
str_index += snprintf(
|
|
&json_string[str_index],
|
|
(max_length - str_index),
|
|
"%f",
|
|
walk_ptr->value.real
|
|
);
|
|
}
|
|
else if(walk_ptr->value_type == HB_VALUE_STRING)
|
|
{
|
|
str_index += snprintf(
|
|
&json_string[str_index],
|
|
(max_length - str_index),
|
|
"\"%s\"",
|
|
walk_ptr->value.string
|
|
);
|
|
}
|
|
|
|
walk_ptr = walk_ptr->next;
|
|
}
|
|
|
|
str_index += snprintf(
|
|
&json_string[str_index],
|
|
(max_length - str_index),
|
|
"}"
|
|
);
|
|
}
|
|
|
|
void Habpack_Telem_Destroy(received_t *Received)
|
|
{
|
|
habpack_telem_linklist_entry_t *walk_ptr, *last_ptr;
|
|
|
|
walk_ptr = Received->Telemetry.habpack_extra;
|
|
while(walk_ptr != NULL)
|
|
{
|
|
if(walk_ptr->value_type == HB_VALUE_STRING)
|
|
{
|
|
free(walk_ptr->value.string);
|
|
}
|
|
|
|
last_ptr = walk_ptr;
|
|
walk_ptr = walk_ptr->next;
|
|
|
|
free(last_ptr);
|
|
}
|
|
}
|
|
|
|
|
|
int Habpack_Process_Message(received_t *Received)
|
|
{
|
|
cmp_ctx_t cmp;
|
|
cmp_object_t item;
|
|
uint32_t map_size;
|
|
uint32_t array_size,array_size2;
|
|
uint32_t i,j,k;
|
|
|
|
uint64_t map_id;
|
|
uint64_t tu64;
|
|
int64_t ts64;
|
|
float tf32;
|
|
double tf64;
|
|
uint32_t str_max_length = 255;
|
|
char str[255];
|
|
char field_name[32];
|
|
|
|
cmp_init(&cmp, (void*)Received->Message, file_reader, file_skipper, file_writer);
|
|
|
|
hb_buf_ptr = 0;
|
|
hb_ptr_max = 256;
|
|
|
|
Received->isCallingBeacon = false;
|
|
|
|
/* Open MessagePack Map */
|
|
if (!cmp_read_map(&cmp, &map_size))
|
|
return 0;
|
|
|
|
/* Loop through each element of the Map */
|
|
for (i=0; i < map_size; i++)
|
|
{
|
|
/* Read Map ID (aka HABpack field ID) */
|
|
if (!(cmp_read_uinteger(&cmp, &map_id)))
|
|
return 0;
|
|
|
|
/* Read HABpack field object */
|
|
if(!cmp_read_object(&cmp, &item))
|
|
return 0;
|
|
|
|
/* Parse according to Map ID (aka HABpack field ID) */
|
|
switch(map_id)
|
|
{
|
|
case HABPACK_CALLSIGN:
|
|
strncpy(field_name, "callsign", 31);
|
|
|
|
TRY_PARSE_STRING(
|
|
strncpy(Received->Telemetry.Callsign, str, 31);
|
|
HABPACK_TELEM_STORE_AS_STRING(str);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
snprintf(Received->Telemetry.Callsign, 31, "%lld", tu64);
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_SENTENCE_ID:
|
|
strncpy(field_name, "sentence_id", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.SentenceId = tu64;
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_TIME:
|
|
strncpy(field_name, "time", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
if(tu64 > SECONDS_IN_A_DAY)
|
|
{
|
|
/* Larger than 1 day, so must be unix epoch time */
|
|
Received->Telemetry.Time = tu64;
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
|
|
/* Strip out date information. TODO: Find a good way of including this in the UKHAS ASCII */
|
|
tu64 -= (uint64_t)(tu64 / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY;
|
|
}
|
|
else
|
|
{
|
|
/* Smaller than 1 day, either we've gone over 88mph, or it's seconds since midnight today */
|
|
uint64_t unix_epoch_time;
|
|
unix_epoch_time = tu64 + (uint64_t)((uint64_t)time(NULL) / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY;
|
|
Received->Telemetry.Time = unix_epoch_time;
|
|
HABPACK_TELEM_STORE_AS_INT64(unix_epoch_time);
|
|
}
|
|
|
|
int hours = tu64/(60*60);
|
|
tu64 = tu64 - (hours*60*60);
|
|
int mins = (tu64/60);
|
|
int secs = tu64-mins*60;
|
|
|
|
snprintf(Received->Telemetry.TimeString, 9, "%02d:%02d:%02d",hours,mins,secs);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_POSITION:
|
|
TRY_PARSE_ARRAY_AT_ONCE(
|
|
/* Check size of the array */
|
|
if(array_size == 2 || array_size == 3)
|
|
{
|
|
/* Latitude - Signed Integer */
|
|
if (!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
Received->Telemetry.Latitude = (double)ts64 / 10e6;
|
|
strncpy(field_name, "latitude", 31);
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6);
|
|
|
|
/* Longitude - Signed Integer */
|
|
if (!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
Received->Telemetry.Longitude = (double)ts64 / 10e6;
|
|
strncpy(field_name, "longitude", 31);
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6);
|
|
|
|
/* Altitude (optional) - Signed Integer */
|
|
Received->Telemetry.Altitude = 0.0;
|
|
if(array_size == 3)
|
|
{
|
|
if(!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
Received->Telemetry.Altitude = ts64;
|
|
strncpy(field_name, "altitude", 31);
|
|
HABPACK_TELEM_STORE_AS_INT64(ts64);
|
|
}
|
|
}
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_GNSS_SATELLITES:
|
|
strncpy(field_name, "gnss_satellites", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_GNSS_LOCK:
|
|
strncpy(field_name, "gnss_lock_type", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_VOLTAGE:
|
|
strncpy(field_name, "voltage", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for mV
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for mV
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "voltage%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for mV
|
|
)
|
|
TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for mV
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_INTERNAL_TEMPERATURE:
|
|
strncpy(field_name, "internal_temperature", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "internal_temperature%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_EXTERNAL_TEMPERATURE:
|
|
strncpy(field_name, "external_temperature", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "external_temperature%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
TRY_PARSE_SIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 1000.0); // Scale for milli-degrees
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_PRESSURE:
|
|
strncpy(field_name, "pressure", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-bar
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "pressure%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-bar
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_RELATIVE_HUMIDITY:
|
|
strncpy(field_name, "relative_humidity", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tu64);
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "relative_humidity%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tu64);
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_ABSOLUTE_HUMIDITY:
|
|
strncpy(field_name, "absolute_humidity", 31);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-grams / m^3
|
|
)
|
|
else TRY_PARSE_ARRAY(
|
|
snprintf(field_name, 31, "absolute_humidity%d", j);
|
|
|
|
TRY_PARSE_FLOAT(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)tu64 / 1000.0); // Scale for milli-grams / m^3
|
|
)
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_FREQUENCY:
|
|
strncpy(field_name, "downlink_frequency", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->isCallingBeacon = true;
|
|
Received->Telemetry.DownlinkFrequency = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_MODE:
|
|
strncpy(field_name, "downlink_lora_mode", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_IMPLICIT:
|
|
strncpy(field_name, "downlink_lora_implicit", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = -1;
|
|
Received->Telemetry.DownlinkLoraImplicit = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_ERRORCODING:
|
|
strncpy(field_name, "downlink_lora_error_coding", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = -1;
|
|
Received->Telemetry.DownlinkLoraErrorCoding = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_BANDWIDTH:
|
|
strncpy(field_name, "downlink_lora_bandwidth", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = -1;
|
|
|
|
/* Convert integer ID to float bandwidth. Ref: https://ukhas.org.uk/communication:habpack */
|
|
switch(tu64)
|
|
{
|
|
case 0:
|
|
tf64 = 7.8;
|
|
break;
|
|
case 1:
|
|
tf64 = 10.4;
|
|
break;
|
|
case 2:
|
|
tf64 = 15.6;
|
|
break;
|
|
case 3:
|
|
tf64 = 20.8;
|
|
break;
|
|
case 4:
|
|
tf64 = 31.25;
|
|
break;
|
|
case 5:
|
|
tf64 = 41.7;
|
|
break;
|
|
case 6:
|
|
tf64 = 62.5;
|
|
break;
|
|
case 7:
|
|
tf64 = 125;
|
|
break;
|
|
case 8:
|
|
tf64 = 250;
|
|
break;
|
|
case 9:
|
|
tf64 = 500;
|
|
break;
|
|
default:
|
|
return 0;
|
|
}
|
|
Received->Telemetry.DownlinkLoraBandwidth = tf64;
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_SPREADING:
|
|
strncpy(field_name, "downlink_lora_spreading_factor", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = -1;
|
|
Received->Telemetry.DownlinkLoraSpreadingFactor = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_DOWNLINK_LORA_LDO:
|
|
strncpy(field_name, "downlink_lora_ldo_on", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
Received->Telemetry.DownlinkLoraMode = -1;
|
|
Received->Telemetry.DownlinkLoraLowDatarateOptimise = tu64;
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_UPLINK_COUNT:
|
|
strncpy(field_name, "uplink_count", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_PREDICTED_LANDING_TIME:
|
|
strncpy(field_name, "predicted_landing_time", 31);
|
|
|
|
TRY_PARSE_UNSIGNED_INTEGER(
|
|
if(tu64 > SECONDS_IN_A_DAY)
|
|
{
|
|
/* Larger than 1 day, so must be unix epoch time */
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
|
|
/* Strip out date information. TODO: Find a good way of including this in the UKHAS ASCII */
|
|
tu64 -= (uint64_t)(tu64 / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY;
|
|
}
|
|
else
|
|
{
|
|
/* Smaller than 1 day, either we've gone over 88mph, or it's seconds since midnight today */
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64 + (uint64_t)((uint64_t)time(NULL) / SECONDS_IN_A_DAY) * SECONDS_IN_A_DAY);
|
|
}
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
case HABPACK_PREDICTED_LANDING_POSITION:
|
|
TRY_PARSE_ARRAY_AT_ONCE(
|
|
/* Check size of the array */
|
|
if(array_size == 2 || array_size == 3)
|
|
{
|
|
/* Latitude - Signed Integer */
|
|
if (!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
strncpy(field_name, "predicted_landing_latitude", 31);
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6);
|
|
|
|
/* Longitude - Signed Integer */
|
|
if (!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
strncpy(field_name, "predicted_landing_longitude", 31);
|
|
HABPACK_TELEM_STORE_AS_DOUBLE((double)ts64 / 10e6);
|
|
|
|
/* Altitude (optional) - Signed Integer */
|
|
if(array_size == 3)
|
|
{
|
|
if(!(cmp_read_sinteger(&cmp, &ts64)))
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
strncpy(field_name, "predicted_landing_altitude", 31);
|
|
HABPACK_TELEM_STORE_AS_INT64(ts64);
|
|
}
|
|
}
|
|
else return 0;
|
|
)
|
|
else return 0;
|
|
|
|
break;
|
|
|
|
default: // Unknown
|
|
TRY_PARSE_STRING(
|
|
snprintf(field_name, 31, "%lld", map_id);
|
|
|
|
HABPACK_TELEM_STORE_AS_STRING(str);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld", map_id);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld", map_id);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(ts64);
|
|
)
|
|
else TRY_PARSE_FLOAT(
|
|
snprintf(field_name, 31, "%lld", map_id);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
snprintf(field_name, 31, "%lld", map_id);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_ARRAY_AT_ONCE(
|
|
for (j = 0; j < array_size; j++)
|
|
{
|
|
/* Read array element */
|
|
if(!cmp_read_object(&cmp, &item))
|
|
return 0;
|
|
|
|
TRY_PARSE_STRING(
|
|
snprintf(field_name, 31, "%lld.%d", map_id, j);
|
|
|
|
HABPACK_TELEM_STORE_AS_STRING(str);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld.%d", map_id, j);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld.%d", map_id, j);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(ts64);
|
|
)
|
|
else TRY_PARSE_FLOAT(
|
|
snprintf(field_name, 31, "%lld.%d", map_id, j);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
snprintf(field_name, 31, "%lld.%d", map_id, j);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else TRY_PARSE_ARRAY_AT_ONCE_NESTED(
|
|
for (k = 0; k < array_size2; k++)
|
|
{
|
|
/* Read array element */
|
|
if(!cmp_read_object(&cmp, &item))
|
|
return 0;
|
|
|
|
TRY_PARSE_STRING(
|
|
snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k);
|
|
|
|
HABPACK_TELEM_STORE_AS_STRING(str);
|
|
)
|
|
else TRY_PARSE_UNSIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(tu64);
|
|
)
|
|
else TRY_PARSE_SIGNED_INTEGER(
|
|
snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k);
|
|
|
|
HABPACK_TELEM_STORE_AS_INT64(ts64);
|
|
)
|
|
else TRY_PARSE_FLOAT(
|
|
snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf32);
|
|
)
|
|
else TRY_PARSE_DOUBLE(
|
|
snprintf(field_name, 31, "%lld.%d.%d", map_id, j, k);
|
|
|
|
HABPACK_TELEM_STORE_AS_DOUBLE(tf64);
|
|
)
|
|
else return 0;
|
|
}
|
|
)
|
|
else return 0;
|
|
}
|
|
)
|
|
break;
|
|
}
|
|
}
|
|
|
|
Habpack_Telem_UKHAS_String(Received, Received->UKHASstring, 255);
|
|
|
|
return 1;
|
|
} |