kopia lustrzana https://github.com/mikaelnousiainen/RS41ng
126 wiersze
3.7 KiB
C
126 wiersze
3.7 KiB
C
#include <string.h>
|
|
#include <assert.h>
|
|
|
|
#include "whisker.h"
|
|
|
|
#ifdef RS41
|
|
#define HARDWARE_ID 0x7d00
|
|
#endif
|
|
#ifdef DFM17
|
|
#define HARDWARE_ID 0x7d01
|
|
#endif
|
|
|
|
#define SOFTWARE_ID 0x00
|
|
|
|
static inline void cats_push_f16(cats_packet *p, float val)
|
|
{
|
|
union {
|
|
float f;
|
|
uint32_t u;
|
|
} fu = { .f = val };
|
|
|
|
assert(sizeof fu.f == sizeof fu.u);
|
|
|
|
uint32_t f32 = fu.u;
|
|
uint32_t sign = (f32 >> 16) & 0x8000;
|
|
uint32_t exponent = ((f32 >> 23) - 127 + 15) & 0x1F;
|
|
uint32_t mantissa = f32 & 0x007FFFFF;
|
|
uint32_t f16 = sign | (exponent << 10) | (mantissa >> 13);
|
|
|
|
p->data[p->len++] = f16;
|
|
p->data[p->len++] = f16 >> 8;
|
|
}
|
|
|
|
static inline void cats_push_f32(cats_packet *p, float val)
|
|
{
|
|
union {
|
|
float f;
|
|
uint32_t u;
|
|
} fu = { .f = val };
|
|
|
|
assert(sizeof fu.f == sizeof fu.u);
|
|
|
|
uint32_t f32 = fu.u;
|
|
p->data[p->len++] = f32;
|
|
p->data[p->len++] = f32 >> 8;
|
|
p->data[p->len++] = f32 >> 16;
|
|
p->data[p->len++] = f32 >> 24;
|
|
}
|
|
|
|
static inline void cats_push_u16(cats_packet *p, uint16_t val)
|
|
{
|
|
for(int i = 0; i < 2; i++) {
|
|
p->data[p->len++] = val >> (i * 8);
|
|
}
|
|
}
|
|
|
|
static inline void cats_push_u32(cats_packet *p, uint32_t val)
|
|
{
|
|
|
|
for(int i = 0; i < 4; i++) {
|
|
p->data[p->len++] = val >> (i * 8);
|
|
}
|
|
}
|
|
|
|
void cats_append_identification_whisker(cats_packet *packet, char *callsign, uint8_t ssid, uint16_t icon)
|
|
{
|
|
packet->data[packet->len++] = 0x00; // type = identidication
|
|
size_t callsign_len = strlen(callsign);
|
|
packet->data[packet->len++] = callsign_len + 3; // len
|
|
packet->data[packet->len++] = icon & 0xFF; // icon
|
|
packet->data[packet->len++] = (icon >> 8) & 0xFF; // icon
|
|
memcpy(packet->data + packet->len, callsign, callsign_len); //callsign
|
|
packet->len += callsign_len;
|
|
packet->data[packet->len++] = ssid; // ssid
|
|
}
|
|
|
|
void cats_append_gps_whisker(cats_packet *packet, gps_data gps)
|
|
{
|
|
packet->data[packet->len++] = 0x02; // type = gps
|
|
packet->data[packet->len++] = 14; // len
|
|
int32_t lat_converted = gps.latitude_degrees_1000000 * (1LL<<31) / 90LL / 10000000LL;
|
|
int32_t lon_converted = gps.longitude_degrees_1000000 * (1LL<<31) / 180LL / 10000000LL;
|
|
cats_push_u32(packet, lat_converted); // lat
|
|
cats_push_u32(packet, lon_converted); // lon
|
|
cats_push_f16(packet, gps.altitude_mm / 1000.0); // alt
|
|
packet->data[packet->len++] = 0; // max error = 0m
|
|
packet->data[packet->len++] = gps.heading_degrees_100000 / 100000.0 * 128.0 / 180.0; // heading
|
|
cats_push_f16(packet, gps.ground_speed_cm_per_second / 100.0); // horizontal speed
|
|
}
|
|
|
|
void cats_append_comment_whisker(cats_packet *packet, char *comment)
|
|
{
|
|
packet->data[packet->len++] = 0x03; // type = comment
|
|
size_t comment_len = strlen(comment);
|
|
assert(comment_len <= 255);
|
|
packet->data[packet->len++] = comment_len;
|
|
memcpy(packet->data + packet->len, comment, comment_len);
|
|
packet->len += comment_len;
|
|
}
|
|
|
|
void cats_append_node_info_whisker(cats_packet *packet, telemetry_data *data)
|
|
{
|
|
uint8_t *d = packet->data;
|
|
size_t *l = &packet->len;
|
|
|
|
bool has_altitude = GPS_HAS_FIX(data->gps);
|
|
float altitude = data->gps.altitude_mm / 1000.0;
|
|
|
|
d[(*l)++] = 0x09; // type = node info
|
|
d[(*l)++] = has_altitude ? 14 : 10; // len
|
|
// bitmask
|
|
d[(*l)++] = 0;
|
|
d[(*l)++] = (CATS_IS_BALLOON << 2) | (has_altitude << 1);
|
|
d[(*l)++] = 0b11110011;
|
|
|
|
cats_push_u16(packet, HARDWARE_ID);
|
|
d[(*l)++] = SOFTWARE_ID;
|
|
d[(*l)++] = 21; // vertical antenna -> ~5.25 dBi gain
|
|
d[(*l)++] = CATS_REPORTED_TX_POWER_DBM * 4.0; // TX power
|
|
d[(*l)++] = data->battery_voltage_millivolts / 100; // voltage
|
|
d[(*l)++] = data->internal_temperature_celsius_100 / 100; // transmitter temperature
|
|
if(has_altitude) {
|
|
cats_push_f32(packet, altitude);
|
|
}
|
|
}
|