kopia lustrzana https://github.com/mikaelnousiainen/RS41ng
76 wiersze
2.5 KiB
C
76 wiersze
2.5 KiB
C
#include <string.h>
|
|
#include "horus_packet_v1.h"
|
|
|
|
volatile uint16_t horus_packet_counter = 0;
|
|
|
|
static uint16_t calculate_crc16_checksum(char *string, int len)
|
|
{
|
|
uint16_t crc = 0xffff;
|
|
char i;
|
|
int ptr = 0;
|
|
while (ptr < len) {
|
|
ptr++;
|
|
crc = crc ^ (*(string++) << 8);
|
|
for (i = 0; i < 8; i++) {
|
|
if (crc & 0x8000)
|
|
crc = (uint16_t)((crc << 1) ^ 0x1021);
|
|
else
|
|
crc <<= 1;
|
|
}
|
|
}
|
|
return crc;
|
|
}
|
|
|
|
size_t horus_packet_v1_create(uint8_t *payload, size_t length, telemetry_data *data, uint8_t payload_id)
|
|
{
|
|
if (length < sizeof(horus_packet_v1)) {
|
|
return 0;
|
|
}
|
|
|
|
gps_data *gps_data = &data->gps;
|
|
|
|
float float_lat = (float) gps_data->latitude_degrees_1000000 / 10000000.0f;
|
|
float float_lon = (float) gps_data->longitude_degrees_1000000 / 10000000.0f;
|
|
|
|
uint8_t volts_scaled = (uint8_t)(255 * (float) data->battery_voltage_millivolts / 5000.0f);
|
|
|
|
horus_packet_counter++;
|
|
|
|
// Assemble a binary packet
|
|
horus_packet_v1 horus_packet;
|
|
|
|
horus_packet.PayloadID = payload_id % 256;
|
|
horus_packet.Counter = horus_packet_counter;
|
|
horus_packet.Hours = gps_data->hours;
|
|
horus_packet.Minutes = gps_data->minutes;
|
|
horus_packet.Seconds = gps_data->seconds;
|
|
horus_packet.Latitude = float_lat;
|
|
horus_packet.Longitude = float_lon;
|
|
horus_packet.Altitude = (uint16_t)(gps_data->altitude_mm / 1000);
|
|
horus_packet.Speed = (uint8_t)((float) gps_data->ground_speed_cm_per_second * 0.036);
|
|
|
|
// Temporary pDOP info, to determine suitable pDOP limits.
|
|
// float pDop = (float)gps_data.pDOP/10.0;
|
|
// if (pDop>255.0){
|
|
// pDop = 255.0;
|
|
// }
|
|
// mfsk_buffer.Speed = (uint8_t)pDop;
|
|
horus_packet.BattVoltage = volts_scaled;
|
|
horus_packet.Sats = gps_data->satellites_visible;
|
|
horus_packet.Temp = (int8_t) ((float) data->internal_temperature_celsius_100 / 100.0f);
|
|
|
|
// Add onto the sats_raw value to indicate if the GPS is in regular tracking (+100)
|
|
// or power optimized tracker (+200) modes.
|
|
if (gps_data->power_safe_mode_state == POWER_SAFE_MODE_STATE_TRACKING) {
|
|
horus_packet.Sats += 100;
|
|
} else if (gps_data->power_safe_mode_state == POWER_SAFE_MODE_STATE_POWER_OPTIMIZED_TRACKING) {
|
|
horus_packet.Sats += 200;
|
|
}
|
|
|
|
horus_packet.Checksum = (uint16_t) calculate_crc16_checksum((char *) &horus_packet, sizeof(horus_packet) - 2);
|
|
|
|
memcpy(payload, &horus_packet, sizeof(horus_packet));
|
|
|
|
return sizeof(horus_packet);
|
|
}
|