mirror of https://github.com/ArduPilot/ardupilot
AP_LTM_Telem: LTM telemetry support
This commit is contained in:
Marian Soban
Andrew Tridgell
parent
bea1502af9
commit
25cf389971
|
|
@ -0,0 +1,216 @@
|
|||
/* #################################################################################################################
|
||||
* LightTelemetry protocol (LTM)
|
||||
*
|
||||
* Ghettostation one way telemetry protocol for really low bitrates (2400 bauds).
|
||||
*
|
||||
* Protocol details: 3 different frames, little endian.
|
||||
* G Frame (GPS position) (2 Hz): 18BYTES
|
||||
* 0x24 0x54 0x47 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
|
||||
* $ T G --------LAT-------- -------LON--------- SPD --------ALT-------- SAT/FIX CRC
|
||||
* A Frame (Attitude) (5 Hz): 10BYTES
|
||||
* 0x24 0x54 0x41 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
|
||||
* $ T A --PITCH-- --ROLL--- -HEADING- CRC
|
||||
* S Frame (Sensors) (2 Hz): 11BYTES
|
||||
* 0x24 0x54 0x53 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xFF 0xC0
|
||||
* $ T S VBAT(mV) Current(mA) RSSI AIRSPEED ARM/FS/FMOD CRC
|
||||
* ################################################################################################################# */
|
||||
|
||||
#include "AP_LTM_Telem.h"
|
||||
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_GPS/AP_GPS.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_RSSI/AP_RSSI.h>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
// init - perform required initialisation
|
||||
void AP_LTM_Telem::init()
|
||||
{
|
||||
const AP_SerialManager &serial_manager = AP::serialmanager();
|
||||
|
||||
// check for LTM_Port
|
||||
if ((_port = serial_manager.find_serial(
|
||||
AP_SerialManager::SerialProtocol_LTM_Telem, 0))) {
|
||||
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||
// initialise UART
|
||||
_port->begin(0, AP_SERIALMANAGER_LTM_BUFSIZE_RX,
|
||||
AP_SERIALMANAGER_LTM_BUFSIZE_TX);
|
||||
hal.scheduler->register_io_process(
|
||||
FUNCTOR_BIND_MEMBER(&AP_LTM_Telem::tick, void));
|
||||
}
|
||||
}
|
||||
|
||||
void AP_LTM_Telem::send_LTM(uint8_t lt_packet[], uint8_t lt_packet_size)
|
||||
{
|
||||
// check space before write
|
||||
if (_port->txspace() < lt_packet_size) {
|
||||
return;
|
||||
}
|
||||
// calculate checksum
|
||||
uint8_t lt_crc = 0x00;
|
||||
for (uint8_t i = 3; i < lt_packet_size - 1; i++) {
|
||||
lt_crc ^= lt_packet[i];
|
||||
}
|
||||
lt_packet[lt_packet_size - 1] = lt_crc;
|
||||
_port->write(lt_packet, lt_packet_size);
|
||||
}
|
||||
|
||||
// GPS frame
|
||||
void AP_LTM_Telem::send_Gframe(void)
|
||||
{
|
||||
const AP_GPS &gps = AP::gps();
|
||||
const uint8_t sats_visible = gps.num_sats();
|
||||
const uint8_t fix_type = (uint8_t)gps.status();
|
||||
int32_t lat = 0; // latitude
|
||||
int32_t lon = 0; // longtitude
|
||||
uint8_t gndspeed = 0; // gps ground speed (m/s)
|
||||
int32_t alt = 0;
|
||||
{
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
WITH_SEMAPHORE(ahrs.get_semaphore());
|
||||
float alt_ahrs;
|
||||
ahrs.get_relative_position_D_home(alt_ahrs);
|
||||
alt = (int32_t) roundf(-alt_ahrs * 100.0); // altitude (cm)
|
||||
Location loc;
|
||||
if (ahrs.get_position(loc)) {
|
||||
lat = loc.lat;
|
||||
lon = loc.lng;
|
||||
gndspeed = (uint8_t) roundf(gps.ground_speed());
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t lt_buff[LTM_GFRAME_SIZE];
|
||||
// protocol: START(2 bytes)FRAMEID(1byte)LAT(cm,4 bytes)LON(cm,4bytes)SPEED(m/s,1bytes)ALT(cm,4bytes)SATS(6bits)FIX(2bits)CRC(xor,1byte)
|
||||
// START
|
||||
lt_buff[0] = 0x24; //$
|
||||
lt_buff[1] = 0x54; //T
|
||||
// FRAMEID
|
||||
lt_buff[2] = 0x47; // G ( gps frame at 5hz )
|
||||
// PAYLOAD
|
||||
lt_buff[3] = (lat >> 8 * 0) & 0xFF;
|
||||
lt_buff[4] = (lat >> 8 * 1) & 0xFF;
|
||||
lt_buff[5] = (lat >> 8 * 2) & 0xFF;
|
||||
lt_buff[6] = (lat >> 8 * 3) & 0xFF;
|
||||
lt_buff[7] = (lon >> 8 * 0) & 0xFF;
|
||||
lt_buff[8] = (lon >> 8 * 1) & 0xFF;
|
||||
lt_buff[9] = (lon >> 8 * 2) & 0xFF;
|
||||
lt_buff[10] = (lon >> 8 * 3) & 0xFF;
|
||||
lt_buff[11] = (gndspeed >> 8 * 0) & 0xFF;
|
||||
lt_buff[12] = (alt >> 8 * 0) & 0xFF;
|
||||
lt_buff[13] = (alt >> 8 * 1) & 0xFF;
|
||||
lt_buff[14] = (alt >> 8 * 2) & 0xFF;
|
||||
lt_buff[15] = (alt >> 8 * 3) & 0xFF;
|
||||
lt_buff[16] = ((sats_visible << 2) & 0xFF)
|
||||
| ((fix_type > 3 ? 3 : fix_type) & 0b00000011); // last 6 bits: sats number, first 2:fix type (0, 1, 2, 3)
|
||||
send_LTM(lt_buff, LTM_GFRAME_SIZE);
|
||||
_ltm_scheduler++;
|
||||
}
|
||||
|
||||
// Sensors frame
|
||||
void AP_LTM_Telem::send_Sframe(void)
|
||||
{
|
||||
const AP_BattMonitor &battery = AP::battery();
|
||||
const uint16_t volt = (uint16_t) roundf(battery.voltage() * 1000.0f); // battery voltage (expects value in mV)
|
||||
float current;
|
||||
if (!battery.current_amps(current)) {
|
||||
current = 0;
|
||||
}
|
||||
// note: max. current value we can send is 65.536 A
|
||||
const uint16_t amp = (uint16_t) roundf(current * 100.0f); // current sensor (expects value in hundredth of A)
|
||||
|
||||
// airspeed in m/s if available and enabled - even if not used - otherwise send 0
|
||||
const AP_Airspeed *aspeed = AP::ahrs().get_airspeed();
|
||||
uint8_t airspeed = 0; // airspeed sensor (m/s)
|
||||
if (aspeed && aspeed->enabled()) {
|
||||
airspeed = (uint8_t) roundf(aspeed->get_airspeed());
|
||||
}
|
||||
|
||||
const uint8_t flightmode = AP_Notify::flags.flight_mode; // flight mode
|
||||
|
||||
uint8_t rssi = 0; // radio RSSI (%a)
|
||||
AP_RSSI *ap_rssi = AP_RSSI::get_singleton();
|
||||
if (ap_rssi) {
|
||||
rssi = ap_rssi->read_receiver_rssi_uint8();
|
||||
}
|
||||
|
||||
const uint8_t armstat = AP_Notify::flags.armed; // 0: disarmed, 1: armed
|
||||
const uint8_t failsafe = AP_Notify::flags.failsafe_radio; // 0: normal, 1: failsafe
|
||||
|
||||
uint8_t lt_buff[LTM_SFRAME_SIZE];
|
||||
// START
|
||||
lt_buff[0] = 0x24; //$
|
||||
lt_buff[1] = 0x54; //T
|
||||
// FRAMEID
|
||||
lt_buff[2] = 0x53; //S
|
||||
// PAYLOAD
|
||||
lt_buff[3] = (volt >> 8 * 0) & 0xFF; // VBAT converted to mV
|
||||
lt_buff[4] = (volt >> 8 * 1) & 0xFF;
|
||||
lt_buff[5] = (amp >> 8 * 0) & 0xFF; // actual current instead of consumed mAh in regular LTM
|
||||
lt_buff[6] = (amp >> 8 * 1) & 0xFF;
|
||||
lt_buff[7] = (rssi >> 8 * 0) & 0xFF;
|
||||
lt_buff[8] = (airspeed >> 8 * 0) & 0xFF;
|
||||
lt_buff[9] = ((flightmode << 2) & 0xFF)
|
||||
| ((failsafe << 1) & 0b00000010) | (armstat & 0b00000001); // last 6 bits: flight mode, 2nd bit: failsafe, 1st bit: arm status.
|
||||
send_LTM(lt_buff, LTM_SFRAME_SIZE);
|
||||
_ltm_scheduler++;
|
||||
}
|
||||
|
||||
// Attitude frame
|
||||
void AP_LTM_Telem::send_Aframe(void)
|
||||
{
|
||||
int16_t pitch;
|
||||
int16_t roll;
|
||||
int16_t heading;
|
||||
{
|
||||
AP_AHRS &ahrs = AP::ahrs();
|
||||
WITH_SEMAPHORE(ahrs.get_semaphore());
|
||||
pitch = roundf(ahrs.pitch_sensor / 100.0); // attitude pitch in degrees
|
||||
roll = roundf(ahrs.roll_sensor / 100.0); // attitude roll in degrees
|
||||
heading = roundf(ahrs.yaw_sensor / 100.0); // heading in degrees
|
||||
}
|
||||
|
||||
uint8_t lt_buff[LTM_AFRAME_SIZE];
|
||||
// A Frame: $T(2 bytes)A(1byte)PITCH(2 bytes)ROLL(2bytes)HEADING(2bytes)CRC(xor,1byte)
|
||||
// START
|
||||
lt_buff[0] = 0x24; //$
|
||||
lt_buff[1] = 0x54; //T
|
||||
// FRAMEID
|
||||
lt_buff[2] = 0x41; //A
|
||||
// PAYLOAD
|
||||
lt_buff[3] = (pitch >> 8 * 0) & 0xFF;
|
||||
lt_buff[4] = (pitch >> 8 * 1) & 0xFF;
|
||||
lt_buff[5] = (roll >> 8 * 0) & 0xFF;
|
||||
lt_buff[6] = (roll >> 8 * 1) & 0xFF;
|
||||
lt_buff[7] = (heading >> 8 * 0) & 0xFF;
|
||||
lt_buff[8] = (heading >> 8 * 1) & 0xFF;
|
||||
send_LTM(lt_buff, LTM_AFRAME_SIZE);
|
||||
_ltm_scheduler++;
|
||||
}
|
||||
|
||||
// send LTM
|
||||
void AP_LTM_Telem::generate_LTM(void)
|
||||
{
|
||||
if (_ltm_scheduler & 1) { // is odd
|
||||
send_Aframe();
|
||||
} else { // is even
|
||||
if (_ltm_scheduler % 4 == 0) {
|
||||
send_Sframe();
|
||||
} else {
|
||||
send_Gframe();
|
||||
}
|
||||
}
|
||||
if (_ltm_scheduler > 9) {
|
||||
_ltm_scheduler = 1;
|
||||
}
|
||||
}
|
||||
|
||||
void AP_LTM_Telem::tick(void)
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (now - _last_frame_ms >= 100) {
|
||||
_last_frame_ms = now;
|
||||
generate_LTM();
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/*
|
||||
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/>.
|
||||
*/
|
||||
#pragma once
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
|
||||
static const uint8_t LTM_GFRAME_SIZE = 18;
|
||||
static const uint8_t LTM_AFRAME_SIZE = 10;
|
||||
static const uint8_t LTM_SFRAME_SIZE = 11;
|
||||
|
||||
class AP_LTM_Telem {
|
||||
public:
|
||||
// Constructor
|
||||
AP_LTM_Telem() {}
|
||||
|
||||
/* Do not allow copies */
|
||||
AP_LTM_Telem(const AP_LTM_Telem &other) = delete;
|
||||
AP_LTM_Telem &operator=(const AP_LTM_Telem&) = delete;
|
||||
|
||||
// init - perform required initialisation
|
||||
void init();
|
||||
|
||||
private:
|
||||
AP_HAL::UARTDriver *_port; // UART
|
||||
|
||||
uint8_t _ltm_scheduler;
|
||||
uint32_t _last_frame_ms;
|
||||
|
||||
void send_Gframe(void);
|
||||
void send_Sframe(void);
|
||||
void send_Aframe(void);
|
||||
void generate_LTM(void);
|
||||
void send_LTM(uint8_t lt_packet[], uint8_t lt_packet_size);
|
||||
void tick(void); // tick - main call to send updates to transmitter (called by scheduler at 1kHz)
|
||||
};
|
||||
Loading…
Reference in New Issue