kopia lustrzana https://github.com/mikaelnousiainen/RS41ng
85 wiersze
3.3 KiB
C
85 wiersze
3.3 KiB
C
#include <string.h>
|
|
#include "horus_packet_v2.h"
|
|
#include "horus_common.h"
|
|
#include "config.h"
|
|
|
|
volatile uint16_t horus_v2_packet_counter = 0;
|
|
|
|
size_t horus_packet_v2_create(uint8_t *payload, size_t length, telemetry_data *data, uint16_t payload_id)
|
|
{
|
|
if (length < sizeof(horus_packet_v2)) {
|
|
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_v2_packet_counter++;
|
|
|
|
// Assemble a binary packet
|
|
horus_packet_v2 horus_packet;
|
|
|
|
horus_packet.PayloadID = payload_id;
|
|
horus_packet.Counter = horus_v2_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 > 0 ? gps_data->altitude_mm : 0) / 1000);
|
|
horus_packet.Speed = (uint8_t) ((float) gps_data->ground_speed_cm_per_second * 0.036);
|
|
|
|
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;
|
|
}
|
|
|
|
memset(horus_packet.CustomData, 0, sizeof(horus_packet.CustomData));
|
|
|
|
uint8_t *custom_data_pointer = horus_packet.CustomData;
|
|
|
|
// Unit: cm/s
|
|
int16_t gps_climb_cm_per_second = (int16_t) gps_data->climb_cm_per_second;
|
|
memcpy(custom_data_pointer, &gps_climb_cm_per_second, sizeof(gps_climb_cm_per_second));
|
|
custom_data_pointer += sizeof(gps_climb_cm_per_second);
|
|
|
|
// Unit: Celsius * 10
|
|
int16_t ext_temp_celsius_10 = (int16_t) (data->temperature_celsius_100 / 10.0f);
|
|
memcpy(custom_data_pointer, &ext_temp_celsius_10, sizeof(ext_temp_celsius_10));
|
|
custom_data_pointer += sizeof(ext_temp_celsius_10);
|
|
|
|
// Unit: %
|
|
uint8_t ext_humidity_percentage = (uint8_t) (data->humidity_percentage_100 / 100.0f);
|
|
memcpy(custom_data_pointer, &ext_humidity_percentage, sizeof(ext_humidity_percentage));
|
|
custom_data_pointer += sizeof(ext_humidity_percentage);
|
|
|
|
// Unit: mbar * 10
|
|
uint16_t ext_pressure_mbar = (uint16_t) (data->pressure_mbar_100 / 10.0f);
|
|
memcpy(custom_data_pointer, &ext_pressure_mbar, sizeof(ext_pressure_mbar));
|
|
|
|
if (pulse_counter_enabled) {
|
|
// Unit: pulse count
|
|
custom_data_pointer += sizeof(ext_pressure_mbar);
|
|
uint16_t ext_pulse_count = (uint16_t) data->pulse_count;
|
|
memcpy(custom_data_pointer, &ext_pulse_count, sizeof(ext_pulse_count));
|
|
}
|
|
|
|
horus_packet.Checksum = (uint16_t) calculate_crc16_checksum((char *) &horus_packet,
|
|
sizeof(horus_packet) - sizeof(horus_packet.Checksum));
|
|
|
|
memcpy(payload, &horus_packet, sizeof(horus_packet));
|
|
|
|
return sizeof(horus_packet);
|
|
}
|