zrzk
/
ardupilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_ADSB: add Sagetech XP driver

zr-v5.1
Tom Pittenger 4 years ago committed by Tom Pittenger
parent
ffd3d1da1d
commit
8e6278f269
5 changed files with 744 additions and 5 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 24
      libraries/AP_ADSB/AP_ADSB.cpp
  2. 2
      libraries/AP_ADSB/AP_ADSB.h
  3. 5
      libraries/AP_ADSB/AP_ADSB_Backend.h
  4. 557
      libraries/AP_ADSB/AP_ADSB_Sagetech.cpp
  5. 161
      libraries/AP_ADSB/AP_ADSB_Sagetech.h

24
libraries/AP_ADSB/AP_ADSB.cpp
Unescape View File

@ -24,6 +24,7 @@ @@ -24,6 +24,7 @@
#if HAL_ADSB_ENABLED
#include "AP_ADSB_uAvionix_MAVLink.h"
#include "AP_ADSB_Sagetech.h"
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_Logger/AP_Logger.h>
@ -53,7 +54,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = { @@ -53,7 +54,7 @@ const AP_Param::GroupInfo AP_ADSB::var_info[] = {
// @Param: TYPE
// @DisplayName: ADSB Type
// @Description: Type of ADS-B hardware for ADSB-in and ADSB-out configuration and operation. If any type is selected then MAVLink based ADSB-in messages will always be enabled
// @Values: 0:Disabled,1:uAvionix-MAVLink
// @Values: 0:Disabled,1:uAvionix-MAVLink,2:Sagetech
// @User: Standard
// @RebootRequired: True
AP_GROUPINFO_FLAGS("TYPE", 0, AP_ADSB, _type[0], 0, AP_PARAM_FLAG_ENABLE),
@ -198,11 +199,16 @@ void AP_ADSB::init(void) @@ -198,11 +199,16 @@ void AP_ADSB::init(void)
if (detected_num_instances == 0) {
for (uint8_t i=0; i<ADSB_MAX_INSTANCES; i++) {
detect_instance(i);
if (_backend[i] != nullptr) {
// we loaded a driver for this instance, so it must be
// present (although it may not be healthy)
detected_num_instances = i+1;
if (_backend[i] == nullptr) {
continue;
}
if (!_backend[i]->init()) {
delete _backend[i];
_backend[i] = nullptr;
continue;
}
// success
detected_num_instances = i+1;
}
}
@ -243,12 +249,20 @@ void AP_ADSB::detect_instance(uint8_t instance) @@ -243,12 +249,20 @@ void AP_ADSB::detect_instance(uint8_t instance)
switch (get_type(instance)) {
case Type::None:
return;
case Type::uAvionix_MAVLink:
if (AP_ADSB_uAvionix_MAVLink::detect()) {
_backend[instance] = new AP_ADSB_uAvionix_MAVLink(*this, instance);
return;
}
break;
case Type::Sagetech:
if (AP_ADSB_Sagetech::detect()) {
_backend[instance] = new AP_ADSB_Sagetech(*this, instance);
return;
}
break;
}
}

2
libraries/AP_ADSB/AP_ADSB.h
Unescape View File

@ -42,6 +42,7 @@ class AP_ADSB { @@ -42,6 +42,7 @@ class AP_ADSB {
public:
friend class AP_ADSB_Backend;
friend class AP_ADSB_uAvionix_MAVLink;
friend class AP_ADSB_Sagetech;
// constructor
AP_ADSB();
@ -59,6 +60,7 @@ public: @@ -59,6 +60,7 @@ public:
enum class Type {
None = 0,
uAvionix_MAVLink = 1,
Sagetech = 2,
};
struct adsb_vehicle_t {

5
libraries/AP_ADSB/AP_ADSB_Backend.h
Unescape View File

@ -31,10 +31,15 @@ public: @@ -31,10 +31,15 @@ public:
static bool detect();
virtual void update() = 0;
virtual bool init() { return true; }
protected:
uint8_t _instance;
AP_HAL::UARTDriver *_port;
// references
AP_ADSB &_frontend;
};

557
libraries/AP_ADSB/AP_ADSB_Sagetech.cpp
Unescape View File

@ -0,0 +1,557 @@ @@ -0,0 +1,557 @@
/*
Copyright (C) 2020 Kraus Hamdani Aerospace Inc. All rights reserved.
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_ADSB_Sagetech.h"
#if HAL_ADSB_SAGETECH_ENABLED
#include <GCS_MAVLink/GCS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_RTC/AP_RTC.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <stdio.h>
#include <time.h>
#include <string.h>
#include <math.h>
#define SAGETECH_SCALER_LATLNG (1.0f/2.145767E-5f) // 180/(2^23)
#define SAGETECH_SCALER_KNOTS_TO_CMS ((KNOTS_TO_M_PER_SEC/0.125f) * 100.0f)
#define SAGETECH_SCALER_ALTITUDE (1.0f/0.015625f)
#define SAGETECH_SCALER_HEADING_CM ((360.0f/256.0f) * 100.0f)
#define SAGETECH_VALIDFLAG_LATLNG (1U<<0)
#define SAGETECH_VALIDFLAG_ALTITUDE (1U<<1)
#define SAGETECH_VALIDFLAG_VELOCITY (1U<<2)
#define SAGETECH_VALIDFLAG_GND_SPEED (1U<<3)
#define SAGETECH_VALIDFLAG_HEADING (1U<<4)
#define SAGETECH_VALIDFLAG_V_RATE_GEO (1U<<5)
#define SAGETECH_VALIDFLAG_V_RATE_BARO (1U<<6)
#define SAGETECH_VALIDFLAG_EST_LATLNG (1U<<7)
#define SAGETECH_VALIDFLAG_EST_VELOCITY (1U<<8)
// detect if any port is configured as Sagetech
bool AP_ADSB_Sagetech::detect()
{
return (AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0) != nullptr);
}
// Init, called once after class is constructed
bool AP_ADSB_Sagetech::init()
{
_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_ADSB, 0);
return (_port != nullptr);
}
void AP_ADSB_Sagetech::update()
{
if (_port == nullptr) {
return;
}
const uint32_t now_ms = AP_HAL::millis();
// -----------------------------
// read any available data on serial port
// -----------------------------
uint32_t nbytes = MIN(_port->available(), 10 * PAYLOAD_XP_MAX_SIZE);
while (nbytes-- > 0) {
const int16_t data = (uint8_t)_port->read();
if (data < 0) {
break;
}
if (parse_byte_XP((uint8_t)data)) {
handle_packet_XP(message_in.packet);
}
} // while nbytes
// -----------------------------
// handle timers for generating data
// -----------------------------
if (!last_packet_initialize_ms || (now_ms - last_packet_initialize_ms >= 5000)) {
last_packet_initialize_ms = now_ms;
send_packet(MsgType_XP::Installation_Set);
} else if (!last_packet_PreFlight_ms || (now_ms - last_packet_PreFlight_ms >= 8200)) {
last_packet_PreFlight_ms = now_ms;
// TODO: allow callsign to not require a reboot
send_packet(MsgType_XP::Preflight_Set);
} else if (now_ms - last_packet_Operating_ms >= 1000 && (
last_packet_Operating_ms == 0 || // send once at boot
// send as data changes
last_operating_squawk != _frontend.out_state.cfg.squawk_octal ||
abs(last_operating_alt - _frontend._my_loc.alt) > 1555 || // 1493cm == 49ft. The output resolution is 100ft per bit
last_operating_rf_select != _frontend.out_state.cfg.rfSelect))
{
last_packet_Operating_ms = now_ms;
last_operating_squawk = _frontend.out_state.cfg.squawk_octal;
last_operating_alt = _frontend._my_loc.alt;
last_operating_rf_select = _frontend.out_state.cfg.rfSelect;
send_packet(MsgType_XP::Operating_Set);
} else if (now_ms - last_packet_GPS_ms >= (_frontend.out_state.is_flying ? 200 : 1000)) {
// 1Hz when not flying, 5Hz when flying
last_packet_GPS_ms = now_ms;
send_packet(MsgType_XP::GPS_Set);
}
}
void AP_ADSB_Sagetech::send_packet(const MsgType_XP type)
{
switch (type) {
case MsgType_XP::Installation_Set:
send_msg_Installation();
break;
case MsgType_XP::Preflight_Set:
send_msg_PreFlight();
break;
case MsgType_XP::Operating_Set:
send_msg_Operating();
break;
case MsgType_XP::GPS_Set:
send_msg_GPS();
break;
default:
break;
}
}
void AP_ADSB_Sagetech::request_packet(const MsgType_XP type)
{
// set all bytes in packet to 0 via {} so we only need to set the ones we need to
Packet_XP pkt {};
pkt.type = MsgType_XP::Request;
pkt.id = 0;
pkt.payload_length = 4;
pkt.payload[0] = static_cast<uint8_t>(type);
send_msg(pkt);
}
void AP_ADSB_Sagetech::handle_packet_XP(const Packet_XP &msg)
{
switch (msg.type) {
case MsgType_XP::ACK:
handle_ack(msg);
break;
case MsgType_XP::Installation_Response:
case MsgType_XP::Preflight_Response:
case MsgType_XP::Status_Response:
// TODO add support for these
break;
case MsgType_XP::ADSB_StateVector_Report:
case MsgType_XP::ADSB_ModeStatus_Report:
case MsgType_XP::TISB_StateVector_Report:
case MsgType_XP::TISB_ModeStatus_Report:
case MsgType_XP::TISB_CorasePos_Report:
case MsgType_XP::TISB_ADSB_Mgr_Report:
handle_adsb_in_msg(msg);
break;
case MsgType_XP::Installation_Set:
case MsgType_XP::Preflight_Set:
case MsgType_XP::Operating_Set:
case MsgType_XP::GPS_Set:
case MsgType_XP::Request:
// these are out-bound only and are not expected to be received
case MsgType_XP::INVALID:
break;
}
}
void AP_ADSB_Sagetech::handle_ack(const Packet_XP &msg)
{
// ACK received!
const uint8_t system_state = msg.payload[2];
transponder_type = (Transponder_Type)msg.payload[6];
const char* rfmode = "RF mode: ";
const uint8_t prev_transponder_mode = last_ack_transponder_mode;
last_ack_transponder_mode = (system_state >> 6) & 0x03;
if (prev_transponder_mode != last_ack_transponder_mode) {
switch (last_ack_transponder_mode) {
case 0: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sOFF", rfmode); break;
case 1: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sSTBY", rfmode); break;
case 2: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sON", rfmode); break;
case 3: gcs().send_text(MAV_SEVERITY_INFO, "ADSB: %sON-ALT",rfmode); break;
default: break;
}
}
}
void AP_ADSB_Sagetech::handle_adsb_in_msg(const Packet_XP &msg)
{
AP_ADSB::adsb_vehicle_t vehicle {};
vehicle.last_update_ms = AP_HAL::millis();
switch (msg.type) {
case MsgType_XP::ADSB_StateVector_Report: { // 0x91
const uint16_t validFlags = le16toh_ptr(&msg.payload[8]);
vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[10]);
if (validFlags & SAGETECH_VALIDFLAG_LATLNG) {
vehicle.info.lat = ((int32_t)le24toh_ptr(&msg.payload[20])) * SAGETECH_SCALER_LATLNG;
vehicle.info.lon = ((int32_t)le24toh_ptr(&msg.payload[23])) * SAGETECH_SCALER_LATLNG;
vehicle.info.flags |= ADSB_FLAGS_VALID_COORDS;
}
if (validFlags & SAGETECH_VALIDFLAG_ALTITUDE) {
vehicle.info.altitude = (int32_t)le24toh_ptr(&msg.payload[26]);
vehicle.info.flags |= ADSB_FLAGS_VALID_ALTITUDE;
}
if (validFlags & SAGETECH_VALIDFLAG_VELOCITY) {
const float velNS = ((int32_t)le16toh_ptr(&msg.payload[29])) * SAGETECH_SCALER_KNOTS_TO_CMS;
const float velEW = ((int32_t)le16toh_ptr(&msg.payload[31])) * SAGETECH_SCALER_KNOTS_TO_CMS;
vehicle.info.hor_velocity = Vector2f(velEW, velNS).length();
vehicle.info.flags |= ADSB_FLAGS_VALID_VELOCITY;
}
if (validFlags & SAGETECH_VALIDFLAG_HEADING) {
vehicle.info.heading = ((float)msg.payload[29]) * SAGETECH_SCALER_HEADING_CM;
vehicle.info.flags |= ADSB_FLAGS_VALID_HEADING;
}
if ((validFlags & SAGETECH_VALIDFLAG_V_RATE_GEO) || (validFlags & SAGETECH_VALIDFLAG_V_RATE_BARO)) {
vehicle.info.ver_velocity = (int16_t)le16toh_ptr(&msg.payload[38]);
vehicle.info.flags |= ADSB_FLAGS_VERTICAL_VELOCITY_VALID;
}
_frontend.handle_adsb_vehicle(vehicle);
break;
}
case MsgType_XP::ADSB_ModeStatus_Report: // 0x92
vehicle.info.ICAO_address = le24toh_ptr(&msg.payload[9]);
if (msg.payload[16] != 0) {
// if string is non-null, consider it valid
memcpy(&vehicle.info, &msg.payload[16], 8);
vehicle.info.flags |= ADSB_FLAGS_VALID_CALLSIGN;
}
_frontend.handle_adsb_vehicle(vehicle);
break;
case MsgType_XP::TISB_StateVector_Report:
case MsgType_XP::TISB_ModeStatus_Report:
case MsgType_XP::TISB_CorasePos_Report:
case MsgType_XP::TISB_ADSB_Mgr_Report:
// TODO
return;
default:
return;
}
}
// handling inbound byte and process it in the state machine
// return true when a full packet has been received
bool AP_ADSB_Sagetech::parse_byte_XP(const uint8_t data)
{
switch (message_in.state) {
default:
case ParseState::WaitingFor_Start:
if (data == 0xA5) {
message_in.state = ParseState::WaitingFor_AssmAddr;
}
break;
case ParseState::WaitingFor_AssmAddr:
message_in.state = (data == 0x01) ? ParseState::WaitingFor_MsgType : ParseState::WaitingFor_Start;
break;
case ParseState::WaitingFor_MsgType:
message_in.packet.type = static_cast<MsgType_XP>(data);
message_in.state = ParseState::WaitingFor_MsgId;
break;
case ParseState::WaitingFor_MsgId:
message_in.packet.id = data;
message_in.state = ParseState::WaitingFor_PayloadLen;
break;
case ParseState::WaitingFor_PayloadLen:
message_in.packet.payload_length = data;
message_in.index = 0;
message_in.state = (data == 0) ? ParseState::WaitingFor_ChecksumFletcher : ParseState::WaitingFor_PayloadContents;
break;
case ParseState::WaitingFor_PayloadContents:
message_in.packet.payload[message_in.index++] = data;
if (message_in.index >= message_in.packet.payload_length) {
message_in.state = ParseState::WaitingFor_ChecksumFletcher;
message_in.index = 0;
}
break;
case ParseState::WaitingFor_ChecksumFletcher:
message_in.packet.checksumFletcher = data;
message_in.state = ParseState::WaitingFor_Checksum;
break;
case ParseState::WaitingFor_Checksum:
message_in.packet.checksum = data;
message_in.state = ParseState::WaitingFor_End;
if (checksum_verify_XP(message_in.packet)) {
handle_packet_XP(message_in.packet);
}
break;
case ParseState::WaitingFor_End:
// we don't care if the end value matches
message_in.state = ParseState::WaitingFor_Start;
break;
}
return false;
}
// compute Sum and FletcherSum values into a single value
// returns uint16_t with MSByte as Sum and LSByte FletcherSum
uint16_t AP_ADSB_Sagetech::checksum_generate_XP(Packet_XP &msg) const
{
uint8_t sum = 0;
uint8_t sumFletcher = 0;
const uint8_t header_message_format[5] {
0xA5, // start
0x01, // assembly address
static_cast<uint8_t>(msg.type),
msg.id,
msg.payload_length
};
for (uint8_t i=0; i<5; i++) {
sum += header_message_format[i];
sumFletcher += sum;
}
for (uint8_t i=0; i<msg.payload_length; i++) {
sum += msg.payload[i];
sumFletcher += sum;
}
return UINT16_VALUE(sum, sumFletcher);
}
// computes checksum and returns true if it matches msg checksum
bool AP_ADSB_Sagetech::checksum_verify_XP(Packet_XP &msg) const
{
const uint16_t checksum = checksum_generate_XP(msg);
return (HIGHBYTE(checksum) == msg.checksum) && (LOWBYTE(checksum) == msg.checksumFletcher);
}
// computes checksum and assigns checksum values to msg
void AP_ADSB_Sagetech::checksum_assign_XP(Packet_XP &msg)
{
const uint16_t checksum = checksum_generate_XP(msg);
msg.checksum = HIGHBYTE(checksum);
msg.checksumFletcher = LOWBYTE(checksum);
}
// send message to serial port
void AP_ADSB_Sagetech::send_msg(Packet_XP &msg)
{
// generate and populate checksums.
checksum_assign_XP(msg);
const uint8_t message_format_header[5] {
0xA5, // start
0x01, // assembly address
static_cast<uint8_t>(msg.type),
msg.id,
msg.payload_length
};
const uint8_t message_format_tail[3] {
msg.checksumFletcher,
msg.checksum,
0x5A // end
};
if (_port != nullptr) {
_port->write(message_format_header, sizeof(message_format_header));
_port->write(msg.payload, msg.payload_length);
_port->write(message_format_tail, sizeof(message_format_tail));
}
}
void AP_ADSB_Sagetech::send_msg_Installation()
{
Packet_XP pkt {};
pkt.type = MsgType_XP::Installation_Set;
pkt.payload_length = 28; // 28== 0x1C
// Mode C = 3, Mode S = 0
pkt.id = (transponder_type == Transponder_Type::Mode_C) ? 3 : 0;
// // convert a decimal 123456 to 0x123456
// TODO: do a proper conversion. The param contains "131313" but what gets transmitted over the air is 0x200F1.
const uint32_t icao_hex = convert_base_to_decimal(16, _frontend.out_state.cfg.ICAO_id_param);
put_le24_ptr(&pkt.payload[0], icao_hex);
memcpy(&pkt.payload[3], &_frontend.out_state.cfg.callsign, 8);
pkt.payload[11] = 0; // airspeed MAX
pkt.payload[12] = 0; // COM Port 0 baud, fixed at 57600
pkt.payload[13] = 0; // COM Port 1 baud, fixed at 57600
pkt.payload[14] = 0; // COM Port 2 baud, fixed at 57600
pkt.payload[15] = 1; // GPS from COM port 0 (this port)
pkt.payload[16] = 1; // GPS Integrity
pkt.payload[17] = _frontend.out_state.cfg.emitterType / 8; // Emitter Set
pkt.payload[18] = _frontend.out_state.cfg.emitterType & 0x0F; // Emitter Type
pkt.payload[19] = _frontend.out_state.cfg.lengthWidth; // Aircraft Size
pkt.payload[20] = 0; // Altitude Encoder Offset
pkt.payload[21] = 0; // Altitude Encoder Offset
pkt.payload[22] = 0x07; // ADSB In Control, enable reading everything
pkt.payload[23] = 30; // ADSB In Report max length COM Port 0 (this one)
pkt.payload[24] = 0; // ADSB In Report max length COM Port 1
send_msg(pkt);
}
void AP_ADSB_Sagetech::send_msg_PreFlight()
{
Packet_XP pkt {};
pkt.type = MsgType_XP::Preflight_Set;
pkt.id = 0;
pkt.payload_length = 10;
memcpy(&pkt.payload[0], &_frontend.out_state.cfg.callsign, 8);
memset(&pkt.payload[8], 0, 2);
send_msg(pkt);
}
void AP_ADSB_Sagetech::send_msg_Operating()
{
Packet_XP pkt {};
pkt.type = MsgType_XP::Operating_Set;
pkt.id = 0;
pkt.payload_length = 8;
// squawk
// param is saved as native octal so we need convert back to
// decimal because Sagetech will convert it back to octal
uint16_t squawk = convert_base_to_decimal(8, last_operating_squawk);
put_le16_ptr(&pkt.payload[0], squawk);
// altitude
if (_frontend.out_state.cfg.rf_capable & 0x01) {
const float alt_meters = last_operating_alt * 0.01f;
const int32_t alt_feet = (int32_t)(alt_meters * FEET_TO_METERS);
const int16_t alt_feet_adj = (alt_feet + 50) / 100; // 1 = 100 feet, 1 = 149 feet, 5 = 500 feet
put_le16_ptr(&pkt.payload[2], alt_feet_adj);
} else {
// use integrated altitude - recommend by sagetech
pkt.payload[2] = 0x80;
pkt.payload[3] = 0x00;
}
// RF mode
pkt.payload[4] = last_operating_rf_select;
send_msg(pkt);
}
void AP_ADSB_Sagetech::send_msg_GPS()
{
Packet_XP pkt {};
pkt.type = MsgType_XP::GPS_Set;
pkt.payload_length = 52;
pkt.id = 0;
const int32_t longitude = _frontend._my_loc.lng;
const int32_t latitude = _frontend._my_loc.lat;
// longitude and latitude
// NOTE: these MUST be done in double or else we get roundoff in the maths
const double lon_deg = longitude * (double)1.0e-7 * (longitude < 0 ? -1 : 1);
const double lon_minutes = (lon_deg - int(lon_deg)) * 60;
snprintf((char*)&pkt.payload[0], 12, "%03u%02u.%05u", (unsigned)lon_deg, (unsigned)lon_minutes, unsigned((lon_minutes - (int)lon_minutes) * 1.0E5));
const double lat_deg = latitude * (double)1.0e-7 * (latitude < 0 ? -1 : 1);
const double lat_minutes = (lat_deg - int(lat_deg)) * 60;
snprintf((char*)&pkt.payload[11], 11, "%02u%02u.%05u", (unsigned)lat_deg, (unsigned)lat_minutes, unsigned((lat_minutes - (int)lat_minutes) * 1.0E5));
// ground speed
const Vector2f speed = AP::ahrs().groundspeed_vector();
float speed_knots = norm(speed.x, speed.y) * M_PER_SEC_TO_KNOTS;
snprintf((char*)&pkt.payload[21], 7, "%03u.%02u", (unsigned)speed_knots, unsigned((speed_knots - (int)speed_knots) * 1.0E2));
// heading
float heading = wrap_360(degrees(speed.angle()));
snprintf((char*)&pkt.payload[27], 10, "%03u.%04u", unsigned(heading), unsigned((heading - (int)heading) * 1.0E4));
// hemisphere
uint8_t hemisphere = 0;
hemisphere |= (latitude >= 0) ? 0x01 : 0; // isNorth
hemisphere |= (longitude >= 0) ? 0x02 : 0; // isEast
hemisphere |= (AP::gps().status() < AP_GPS::GPS_OK_FIX_2D) ? 0x80 : 0; // isInvalid
pkt.payload[35] = hemisphere;
// time
uint64_t time_usec;
if (AP::rtc().get_utc_usec(time_usec)) {
// not completely accurate, our time includes leap seconds and time_t should be without
const time_t time_sec = time_usec / 1000000;
struct tm* tm = gmtime(&time_sec);
// format time string
snprintf((char*)&pkt.payload[36], 11, "%02u%02u%06.3f", tm->tm_hour, tm->tm_min, tm->tm_sec + (time_usec % 1000000) * 1.0e-6);
} else {
memset(&pkt.payload[36],' ', 10);
}
send_msg(pkt);
}
/*
* Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value stored on a GCS as a string field in different format, but then transmitted over mavlink as a float which is always a decimal.
* baseIn: base of input number
* inputNumber: value currently in base "baseIn" to be converted to base "baseOut"
*
* Example: convert ADSB squawk octal "1200" stored in memory as 0x0280 to 0x04B0
* uint16_t squawk_decimal = convertMathBase(8, squawk_octal);
*/
uint32_t AP_ADSB_Sagetech::convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber)
{
// Our only sensible input bases are 16 and 8
if (baseIn != 8 && baseIn != 16) {
return inputNumber;
}
uint32_t outputNumber = 0;
for (uint8_t i=0; inputNumber != 0; i++) {
outputNumber += (inputNumber % 10) * powf(10, i);
inputNumber /= 10;
}
return outputNumber;
}
#endif // HAL_ADSB_SAGETECH_ENABLED

161
libraries/AP_ADSB/AP_ADSB_Sagetech.h
Unescape View File

@ -0,0 +1,161 @@ @@ -0,0 +1,161 @@
#pragma once
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
#include "AP_ADSB_Backend.h"
#ifndef HAL_ADSB_SAGETECH_ENABLED
#define HAL_ADSB_SAGETECH_ENABLED HAL_ADSB_ENABLED
#endif
#if HAL_ADSB_SAGETECH_ENABLED
class AP_ADSB_Sagetech : public AP_ADSB_Backend {
public:
using AP_ADSB_Backend::AP_ADSB_Backend;
// init - performs any required initialisation for this instance
bool init() override;
// update - should be called periodically
void update() override;
// static detection function
static bool detect();
private:
static const uint32_t PAYLOAD_XP_MAX_SIZE = 52;
enum class SystemStateBits {
Error_Transponder = (1U<<0),
Altitidue_Source = (1U<<1),
Error_GPS = (1U<<2),
Error_ICAO = (1U<<3),
Error_Over_Temperature = (1U<<4),
Error_Extended_Squitter = (1U<<5),
Mode_Transponder = (3U<<6), // 2 bit status:
};
enum class Transponder_Type {
Mode_C = 0x00,
Mode_S_ADSB_OUT = 0x01,
Mode_S_ADSB_OUT_and_IN = 0x02,
Unknown = 0xFF,
};
enum class MsgType_XP {
INVALID = 0,
Installation_Set = 0x01,
Preflight_Set = 0x02,
Operating_Set = 0x03,
GPS_Set = 0x04,
Request = 0x05,
ACK = 0x80,
Installation_Response = 0x81,
Preflight_Response = 0x82,
Status_Response = 0x83,
ADSB_StateVector_Report = 0x91,
ADSB_ModeStatus_Report = 0x92,
TISB_StateVector_Report = 0x93,
TISB_ModeStatus_Report = 0x94,
TISB_CorasePos_Report = 0x95,
TISB_ADSB_Mgr_Report = 0x96,
};
enum class ParseState {
WaitingFor_Start,
WaitingFor_AssmAddr,
WaitingFor_MsgType,
WaitingFor_MsgId,
WaitingFor_PayloadLen,
WaitingFor_PayloadContents,
WaitingFor_ChecksumFletcher,
WaitingFor_Checksum,
WaitingFor_End,
};
struct Packet_XP {
const uint8_t start = 0xA5;
const uint8_t assemAddr = 0x01;
MsgType_XP type;
uint8_t id;
uint8_t payload_length;
uint8_t payload[PAYLOAD_XP_MAX_SIZE];
uint8_t checksumFletcher;
uint8_t checksum;
const uint8_t end = 0x5A;
};
struct {
ParseState state;
uint8_t index;
Packet_XP packet;
} message_in;
// compute Sum and FletcherSum values
uint16_t checksum_generate_XP(Packet_XP &msg) const;
bool checksum_verify_XP(Packet_XP &msg) const;
void checksum_assign_XP(Packet_XP &msg);
// handling inbound byte and process it in the state machine
bool parse_byte_XP(const uint8_t data);
// handle inbound packet
void handle_packet_XP(const Packet_XP &msg);
// send message to serial port
void send_msg(Packet_XP &msg);
// handle inbound msgs
void handle_adsb_in_msg(const Packet_XP &msg);
void handle_ack(const Packet_XP &msg);
// send messages to to transceiver
void send_msg_Installation();
void send_msg_PreFlight();
void send_msg_Operating();
void send_msg_GPS();
// send packet by type
void send_packet(const MsgType_XP type);
// send msg to request a packet by type
void request_packet(const MsgType_XP type);
// Convert base 8 or 16 to decimal. Used to convert an octal/hexadecimal value
// stored on a GCS as a string field in different format, but then transmitted
// over mavlink as a float which is always a decimal.
uint32_t convert_base_to_decimal(const uint8_t baseIn, uint32_t inputNumber);
// timers for each out-bound packet
uint32_t last_packet_initialize_ms;
uint32_t last_packet_PreFlight_ms;
uint32_t last_packet_GPS_ms;
uint32_t last_packet_Operating_ms;
// cached variables to compare against params so we can send msg on param change.
uint16_t last_operating_squawk;
int32_t last_operating_alt;
uint8_t last_operating_rf_select;
// track status changes in acks
uint8_t last_ack_transponder_mode;
Transponder_Type transponder_type = Transponder_Type::Unknown;
};
#endif // HAL_ADSB_SAGETECH_ENABLED
Write Preview
Loading…
Cancel
Save