ardupilot/libraries/AP_ADSB/sagetech-sdk/sgEncodeOperating.c

106 lines
2.9 KiB
C
Raw Normal View History

/**
* @copyright Copyright (c) 2021 Sagetech, Inc. All rights reserved.
*
* @file sgEncodeOperating.c
* @author Jacob.Garrison
*
* @date Feb 15, 2021
*
* This file receives a populated operating message struct and
* converts it into a operating message buffer.
*/
#include <stdbool.h>
#include <stdlib.h>
#include "sg.h"
#include "sgUtil.h"
#define SG_PAYLOAD_LEN_OPMSG SG_MSG_LEN_OPMSG - 5 /// the payload length.
#define PBASE 4 /// the payload offset.
#define OFFSET_SQUAWK 0 /// the squawk code offset in the payload.
#define OFFSET_CONFIG 2 /// the mode/config offset in the payload.
#define OFFSET_EMRG_ID 3 /// the emergency flag offset in the payload.
#define OFFSET_ALT 4 /// the altitude offset in the payload.
#define OFFSET_RATE 6 /// the climb rate offset in the payload.
#define OFFSET_HEADING 8 /// the heading offset in the payload.
#define OFFSET_AIRSPEED 10 /// the airspeed offset in the payload.
/*
* Documented in the header file.
*/
bool sgEncodeOperating(uint8_t *buffer, sg_operating_t *op, uint8_t msgId)
{
// populate header
buffer[0] = SG_MSG_START_BYTE;
buffer[1] = SG_MSG_TYPE_HOST_OPMSG;
buffer[2] = msgId;
buffer[3] = SG_PAYLOAD_LEN_OPMSG;
// populate Squawk code
uint162Buf(&buffer[PBASE + OFFSET_SQUAWK], op->squawk);
// populate Mode/Config
buffer[PBASE + OFFSET_CONFIG] = op->milEmergency << 5 |
op->enableXBit << 4 |
op->enableSqt << 3 |
op->savePowerUp << 2 |
op->opMode;
// populate Emergency/Ident
buffer[PBASE + OFFSET_EMRG_ID] = op->identOn << 3 |
op->emergcType;
// populate Altitude
uint16_t altCode = 0;
if (op->altUseIntrnl)
{
altCode = 0x8000;
}
else if (op->altHostAvlbl)
{
// 100 foot encoding conversion
altCode = (op->altitude + 1200) / 100;
if (op->altRes25)
{
altCode *= 4;
}
// 'Host altitude available' flag
altCode += 0x4000;
}
uint162Buf(&buffer[PBASE + OFFSET_ALT], altCode);
// populate Altitude Rate
int16_t rate = op->climbRate / 64;
if (!op->climbValid)
{
rate = 0x8000;
}
uint162Buf(&buffer[PBASE + OFFSET_RATE], rate);
// populate Heading
// conversion: heading * ( pow(2, 15) / 360 )
uint16_t heading = op->heading * 32768 / 360;
if (op->headingValid)
{
heading += 0x8000;
}
uint162Buf(&buffer[PBASE + OFFSET_HEADING], heading);
// populate Airspeed
uint16_t airspeed = op->airspd;
if (op->airspdValid)
{
airspeed += 0x8000;
}
uint162Buf(&buffer[PBASE + OFFSET_AIRSPEED], airspeed);
// populate checksum
appendChecksum(buffer, SG_MSG_LEN_OPMSG);
return true;
}