mirror of https://github.com/ArduPilot/ardupilot
457 lines
15 KiB
C++
457 lines
15 KiB
C++
/*
|
|
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/>.
|
|
*/
|
|
|
|
/*
|
|
Graupner Hott Telemetry library
|
|
Hott telemetry runs at 19200 8N1 on a non-inverted half-duplex UART
|
|
|
|
With thanks to Graupner and betaflight
|
|
*/
|
|
|
|
#include "AP_Hott_Telem.h"
|
|
|
|
#if HAL_HOTT_TELEM_ENABLED
|
|
|
|
#include <AP_AHRS/AP_AHRS.h>
|
|
#include <AP_BattMonitor/AP_BattMonitor.h>
|
|
#include <AP_Baro/AP_Baro.h>
|
|
#include <AP_RPM/AP_RPM.h>
|
|
#include <AP_Airspeed/AP_Airspeed.h>
|
|
#include <AP_Stats/AP_Stats.h>
|
|
#include <AP_GPS/AP_GPS.h>
|
|
#include <AP_RTC/AP_RTC.h>
|
|
#include <AP_Notify/AP_Notify.h>
|
|
#include <AP_Mission/AP_Mission.h>
|
|
#include <AP_InertialSensor/AP_InertialSensor.h>
|
|
#include <stdio.h>
|
|
|
|
#define PROT_BINARY 0x80
|
|
#define PROT_ID_GAM 0x8D
|
|
#define PROT_ID_EAM 0x8E
|
|
#define PROT_ID_GPS 0x8A
|
|
#define PROT_ID_VARIO 0x89
|
|
|
|
#define BYTE_DELAY_FIRST_US 4000
|
|
#define BYTE_DELAY_US 1200
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
AP_Hott_Telem *AP_Hott_Telem::singleton;
|
|
|
|
AP_Hott_Telem::AP_Hott_Telem(void)
|
|
{
|
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
|
if (singleton != nullptr) {
|
|
AP_HAL::panic("AP_Hott_Telem must be singleton");
|
|
}
|
|
#endif
|
|
singleton = this;
|
|
}
|
|
|
|
/*
|
|
* initialise uart
|
|
*/
|
|
void AP_Hott_Telem::init()
|
|
{
|
|
const AP_SerialManager &serial_manager = AP::serialmanager();
|
|
|
|
uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Hott, 0);
|
|
if (uart) {
|
|
// register thread
|
|
if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Hott_Telem::loop, void),
|
|
"Hott",
|
|
1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
|
|
hal.console->printf("Failed to create Hott thread\n");
|
|
}
|
|
}
|
|
}
|
|
|
|
/*
|
|
send EAM (Electric Air Model)
|
|
*/
|
|
void AP_Hott_Telem::send_EAM(void)
|
|
{
|
|
// EAM message
|
|
struct PACKED {
|
|
uint8_t start_byte = 0x7C; //#01 start uint8_t
|
|
uint8_t eam_sensor_id = 0x8E;//#02 EAM sensort id. constat value 0x8e
|
|
uint8_t warning_beeps;
|
|
uint8_t sensor_id = 0xE0;
|
|
uint16_t alarms; //#05 alarm bitmask. Value is displayed inverted
|
|
uint8_t cell_low[7]; //#07 cell voltage lower value. 0.02V steps, 124=2.48V
|
|
uint8_t cell_high[7]; //#14 cell voltage high value. 0.02V steps, 124=2.48V
|
|
uint16_t batt1_voltage; //#21 battery 1 voltage in 100mv steps
|
|
uint16_t batt2_voltage; //#23 battery 2 voltage in 100mv steps
|
|
uint8_t temp1; //#25 Temperature sensor 1. 20=0C, 46=26C - offset of 20.
|
|
uint8_t temp2; //#26 temperature sensor 2
|
|
uint16_t altitude; //#27 Attitude unit: meters. Value of 500 = 0m
|
|
uint16_t current; //#29 Current in 0.1A steps
|
|
uint16_t main_voltage; //#31 Main power voltage (drive) in 0.1V steps
|
|
uint16_t batt_used; //#33 used battery capacity in 10mAh steps
|
|
uint16_t climbrate; //#35 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
|
|
uint8_t climbrate3s; //#37 climbrate in m/3sec. Value of 120 = 0m/3sec
|
|
uint16_t rpm; //#38 RPM. Steps: 10 rev/min
|
|
uint8_t electric_min; //#40 Electric minutes. Time starts when motor current is > 3 A
|
|
uint8_t electric_sec; //#41
|
|
uint16_t speed; //#42 speed in km/h. Steps 1km/h
|
|
uint8_t stop_byte = 0x7D; //#44 stop
|
|
} msg {};
|
|
|
|
const AP_BattMonitor &battery = AP::battery();
|
|
if (battery.num_instances() > 0) {
|
|
msg.batt1_voltage = uint16_t(battery.voltage(0) * 10);
|
|
}
|
|
if (battery.num_instances() > 1) {
|
|
msg.batt2_voltage = uint16_t(battery.voltage(1) * 10);
|
|
}
|
|
float current;
|
|
if (battery.current_amps(current)) {
|
|
msg.current = uint16_t(current * 10);
|
|
}
|
|
msg.main_voltage = uint16_t(battery.voltage() * 10);
|
|
float used_mah;
|
|
if (battery.consumed_mah(used_mah)) {
|
|
msg.batt_used = used_mah * 0.1;
|
|
}
|
|
|
|
const AP_Baro &baro = AP::baro();
|
|
msg.temp1 = uint8_t(baro.get_temperature(0) + 20.5);
|
|
if (baro.healthy(1)) {
|
|
msg.temp2 = uint8_t(baro.get_temperature(1) + 20.5);
|
|
}
|
|
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
float alt = 0;
|
|
Vector3f vel;
|
|
{
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
ahrs.get_relative_position_D_home(alt);
|
|
alt = -alt;
|
|
IGNORE_RETURN(ahrs.get_velocity_NED(vel));
|
|
}
|
|
msg.altitude = uint16_t(500.5 + alt);
|
|
|
|
msg.climbrate = uint16_t(30000.5 + vel.z * -100);
|
|
msg.climbrate3s = 120 + vel.z * -3;
|
|
|
|
const AP_RPM *rpm = AP::rpm();
|
|
float rpm_value;
|
|
if (rpm && rpm->get_rpm(0, rpm_value)) {
|
|
msg.rpm = rpm_value * 0.1;
|
|
}
|
|
|
|
AP_Stats *stats = AP::stats();
|
|
if (stats) {
|
|
uint32_t t = stats->get_flight_time_s();
|
|
msg.electric_min = t / 60U;
|
|
msg.electric_sec = t % 60U;
|
|
}
|
|
|
|
AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
|
|
if (airspeed && airspeed->healthy()) {
|
|
msg.speed = uint16_t(airspeed->get_airspeed() * 3.6 + 0.5);
|
|
} else {
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
msg.speed = uint16_t(ahrs.groundspeed() * 3.6 + 0.5);
|
|
}
|
|
|
|
send_packet((const uint8_t *)&msg, sizeof(msg));
|
|
}
|
|
|
|
/*
|
|
convert from a GPS lat/lon in decimal degrees to degrees plus decimal minutes
|
|
*/
|
|
void AP_Hott_Telem::GPS_to_DDM(float decimal, uint8_t &sign, uint16_t &dm, uint16_t &sec) const
|
|
{
|
|
sign = decimal>=0?0:1;
|
|
decimal = fabsf(decimal);
|
|
uint8_t deg = uint16_t(decimal);
|
|
uint8_t min = uint16_t((decimal - deg) * 60);
|
|
dm = deg*100 + min;
|
|
sec = (decimal - (deg + min/60.0)) * 60 * 10000 + 0.5;
|
|
}
|
|
|
|
/*
|
|
send GPS packet
|
|
*/
|
|
void AP_Hott_Telem::send_GPS(void)
|
|
{
|
|
// GPS message
|
|
struct PACKED {
|
|
uint8_t start_byte = 0x7c; //#01 constant value 0x7c
|
|
uint8_t gps_sensor_id = 0x8a; //#02 constant value 0x8a
|
|
uint8_t warning_beeps; //#03
|
|
uint8_t sensor_id = 0xA0; //#04 constant (?) value 0xa0
|
|
uint16_t alarm; //#05
|
|
uint8_t flight_direction; //#07 flight direction in 2 degreees/step (1 = 2degrees);
|
|
uint16_t gps_speed_kmh; //#08 km/h
|
|
uint8_t pos_NS; //#10 north = 0, south = 1
|
|
uint16_t pos_NS_dm; //#11 degree minutes
|
|
uint16_t pos_NS_sec; //#13 position seconds
|
|
uint8_t pos_EW; //#15 east = 0, west = 1
|
|
uint16_t pos_EW_dm; //#16 degree minutes
|
|
uint16_t pos_EW_sec; //#18 position seconds
|
|
uint16_t home_distance; //#20 meters
|
|
uint16_t altitude; //#22 meters. Value of 500 = 0m
|
|
uint16_t climbrate; //#24 m/s 0.01m/s resolution. Value of 30000 = 0.00 m/s
|
|
uint8_t climbrate3s; //#26 climbrate in m/3s resolution, value of 120 = 0 m/3s
|
|
uint8_t gps_satelites; //#27 sat count
|
|
uint8_t gps_fix_char; //#28 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
|
|
uint8_t home_direction; //#29 direction from starting point to Model position (2 degree steps)
|
|
int16_t vel_north; //#30 velocity north mm/s
|
|
uint8_t speed_acc; //#32 speed accuracy cm/s
|
|
uint8_t gps_time_h; //#33 UTC time hours
|
|
uint8_t gps_time_m; //#34 UTC time minutes
|
|
uint8_t gps_time_s; //#35 UTC time seconds
|
|
uint8_t gps_time_hs; //#36 UTC time 0.01s units
|
|
int16_t vel_east; //#37 velocity north mm/s
|
|
uint8_t horiz_acc; //#39 horizontal accuracy
|
|
uint8_t free_char1; //#40 displayed to right of home
|
|
uint8_t free_char2; //#41
|
|
uint8_t free_char3; //#42 GPS fix character. display, 'D' = DGPS, '2' = 2D, '3' = 3D, '-' = no fix
|
|
uint8_t version = 1; //#43 0: GPS Graupner #33600, 1: ArduPilot
|
|
uint8_t stop_byte = 0x7d; //#44
|
|
} msg {};
|
|
|
|
AP_GPS &gps = AP::gps();
|
|
Location loc;
|
|
|
|
{
|
|
WITH_SEMAPHORE(gps.get_semaphore());
|
|
loc = gps.location();
|
|
msg.flight_direction = uint16_t(gps.ground_course() * 0.5 + 0.5);
|
|
msg.gps_speed_kmh = uint16_t(gps.ground_speed() * 3.6 + 0.5);
|
|
float sacc, hacc;
|
|
if (gps.speed_accuracy(sacc)) {
|
|
msg.speed_acc = sacc * 100 + 0.5;
|
|
}
|
|
if (gps.horizontal_accuracy(hacc)) {
|
|
msg.horiz_acc = hacc * 100 + 0.5;
|
|
}
|
|
msg.gps_satelites = gps.num_sats();
|
|
}
|
|
|
|
float lat = loc.lat * 1.0e-7;
|
|
float lon = loc.lng * 1.0e-7;
|
|
|
|
uint16_t dm, sec;
|
|
GPS_to_DDM(lat, msg.pos_NS, dm, sec);
|
|
msg.pos_NS_dm = dm;
|
|
msg.pos_NS_sec = sec;
|
|
|
|
GPS_to_DDM(lon, msg.pos_EW, dm, sec);
|
|
msg.pos_EW_dm = dm;
|
|
msg.pos_EW_sec = sec;
|
|
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
Vector2f home_vec;
|
|
float alt = 0;
|
|
Vector3f vel;
|
|
{
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
if (ahrs.get_relative_position_NE_home(home_vec)) {
|
|
msg.home_distance = home_vec.length();
|
|
}
|
|
ahrs.get_relative_position_D_home(alt);
|
|
alt = -alt;
|
|
IGNORE_RETURN(ahrs.get_velocity_NED(vel));
|
|
}
|
|
|
|
msg.climbrate = uint16_t(30000.5 + vel.z * -100);
|
|
msg.climbrate3s = 120 + vel.z * -3;
|
|
msg.vel_north = vel.x * 1000 + 0.5;
|
|
msg.vel_east = vel.y * 1000 + 0.5;
|
|
msg.altitude = uint16_t(500.5 + alt);
|
|
|
|
switch (gps.status()) {
|
|
case AP_GPS::NO_GPS:
|
|
case AP_GPS::NO_FIX:
|
|
msg.gps_fix_char = '-';
|
|
break;
|
|
case AP_GPS::GPS_OK_FIX_2D:
|
|
msg.gps_fix_char = '2';
|
|
break;
|
|
default:
|
|
msg.gps_fix_char = '3';
|
|
break;
|
|
}
|
|
msg.free_char3 = msg.gps_fix_char;
|
|
|
|
msg.home_direction = degrees(atan2f(home_vec.y, home_vec.x)) * 0.5 + 0.5;
|
|
|
|
AP_RTC &rtc = AP::rtc();
|
|
{
|
|
WITH_SEMAPHORE(rtc.get_semaphore());
|
|
uint16_t ms;
|
|
rtc.get_system_clock_utc(msg.gps_time_h, msg.gps_time_m, msg.gps_time_s, ms);
|
|
}
|
|
|
|
send_packet((const uint8_t *)&msg, sizeof(msg));
|
|
}
|
|
|
|
/*
|
|
send Vario
|
|
*/
|
|
void AP_Hott_Telem::send_Vario(void)
|
|
{
|
|
// Vario message
|
|
struct PACKED {
|
|
uint8_t start_byte = 0x7C; //#01 start uint8_t
|
|
uint8_t vario_id = 0x89; //#02 ID
|
|
uint8_t warning_beeps; //#03 warnings
|
|
uint8_t sensor_id = 0x90; //#04 sensor ID
|
|
uint8_t inv_status; //#05 status
|
|
uint16_t altitude; //#06 Attitude meters. Value of 500 = 0m
|
|
uint16_t altitude_max; //#08 Attitude max meters. Value of 500 = 0m
|
|
uint16_t altitude_min; //#10 Attitude min meters. Value of 500 = 0m
|
|
uint16_t climbrate; //#12 climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
|
|
uint16_t climbrate3s; //#14 climb rate in meters per 3s Value of 30000 = 0.00 m/s
|
|
uint16_t climbrate10s; //#16 climb rate in meters per 10s. Value of 30000 = 0.00 m/s
|
|
char text[3][7]; //#18 #Text display
|
|
char ascii3[3]; //#39 3 extra characters
|
|
uint8_t yaw; //#42 yaw in 2 degree units, 0 = north
|
|
uint8_t version = 1; //#43 protocol version
|
|
uint8_t stop_byte = 0x7D; //#44 stop
|
|
} msg {};
|
|
|
|
AP_AHRS &ahrs = AP::ahrs();
|
|
Vector3f vel;
|
|
float alt = 0;
|
|
{
|
|
WITH_SEMAPHORE(ahrs.get_semaphore());
|
|
ahrs.get_relative_position_D_home(alt);
|
|
alt = -alt;
|
|
IGNORE_RETURN(ahrs.get_velocity_NED(vel));
|
|
msg.yaw = wrap_360_cd(ahrs.yaw_sensor) * 0.005;
|
|
}
|
|
|
|
min_alt = MIN(alt, min_alt);
|
|
max_alt = MAX(alt, max_alt);
|
|
|
|
msg.altitude = uint16_t(500.5 + alt);
|
|
msg.altitude_max = uint16_t(500.5 + max_alt);
|
|
msg.altitude_min = uint16_t(500.5 + min_alt);
|
|
|
|
msg.climbrate = 30000.5 + vel.z * -100;
|
|
msg.climbrate3s = 30000.5 + vel.z * -100*3;
|
|
msg.climbrate10s = 30000.5 + vel.z * -100*10;
|
|
|
|
AP_Notify *notify = AP_Notify::get_singleton();
|
|
char fltmode[5] {};
|
|
if (notify) {
|
|
strncpy(fltmode, notify->get_flight_mode_str(), sizeof(fltmode));
|
|
strncpy(msg.text[0], fltmode, sizeof(msg.text[0]));
|
|
}
|
|
if (hal.util->get_soft_armed()) {
|
|
strncpy(msg.text[1], "ARMED", sizeof(msg.text[1]));
|
|
if (strncmp(fltmode, "AUTO", sizeof(fltmode)) == 0) {
|
|
const AP_Mission *mission = AP::mission();
|
|
if (mission) {
|
|
char wp[10] {};
|
|
snprintf(wp, sizeof(wp), "WP %3u", mission->get_current_nav_index());
|
|
memcpy(msg.text[2], wp, sizeof(msg.text[2]));
|
|
}
|
|
}
|
|
} else {
|
|
strncpy(msg.text[1], "DISARM", sizeof(msg.text[1]));
|
|
const char *ck = AP_Notify::flags.pre_arm_check ? "CK:PASS" : "CK:FAIL";
|
|
memcpy(msg.text[2], ck, MIN(strlen(ck), sizeof(msg.text[2])));
|
|
}
|
|
|
|
send_packet((const uint8_t *)&msg, sizeof(msg));
|
|
}
|
|
|
|
/*
|
|
send a packet out
|
|
*/
|
|
void AP_Hott_Telem::send_packet(const uint8_t *b, uint8_t len)
|
|
{
|
|
// initial delay
|
|
hal.scheduler->delay_microseconds(BYTE_DELAY_FIRST_US);
|
|
uint8_t crc = 0;
|
|
while (len) {
|
|
uint8_t ob = *b;
|
|
if (uart->write(ob) == 1) {
|
|
len--;
|
|
crc += ob;
|
|
b++;
|
|
hal.scheduler->delay_microseconds(BYTE_DELAY_US);
|
|
} else {
|
|
hal.scheduler->delay_microseconds(100);
|
|
}
|
|
}
|
|
uart->write(crc);
|
|
|
|
// discard any bytes received during the send
|
|
hal.scheduler->delay_microseconds(BYTE_DELAY_US*2);
|
|
while (uart->available() != 0) {
|
|
uart->read();
|
|
hal.scheduler->delay_microseconds(100);
|
|
}
|
|
}
|
|
|
|
/*
|
|
thread to process requests
|
|
*/
|
|
void AP_Hott_Telem::loop(void)
|
|
{
|
|
uart->begin(19200, 10, 10);
|
|
uart->set_unbuffered_writes(true);
|
|
uart->set_blocking_writes(true);
|
|
|
|
while (true) {
|
|
hal.scheduler->delay_microseconds(1500);
|
|
uint32_t n = uart->available();
|
|
if (n < 2) {
|
|
// wait for 2 bytes
|
|
continue;
|
|
}
|
|
if (n > 2) {
|
|
while (n--) {
|
|
uart->read();
|
|
hal.scheduler->delay_microseconds(100);
|
|
}
|
|
continue;
|
|
}
|
|
|
|
const uint8_t prot_type = uart->read();
|
|
const uint8_t sensor_id = uart->read();
|
|
if (prot_type != PROT_BINARY) {
|
|
// only do binary protocol for now
|
|
continue;
|
|
}
|
|
|
|
switch (sensor_id) {
|
|
case PROT_ID_EAM:
|
|
send_EAM();
|
|
break;
|
|
case PROT_ID_GPS:
|
|
send_GPS();
|
|
break;
|
|
case PROT_ID_VARIO:
|
|
send_Vario();
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
namespace AP {
|
|
AP_Hott_Telem *hott_telem() {
|
|
return AP_Hott_Telem::get_singleton();
|
|
}
|
|
};
|
|
|
|
#endif // HAL_HOTT_TELEM_ENABLED
|