You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
180 lines
4.9 KiB
180 lines
4.9 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
/* |
|
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/>. |
|
*/ |
|
|
|
#ifndef __AP_FRSKY_TELEM_H__ |
|
#define __AP_FRSKY_TELEM_H__ |
|
|
|
#include <AP_HAL.h> |
|
#include <AP_Param.h> |
|
#include <AP_Math.h> |
|
#include <AP_GPS.h> |
|
#include <AP_AHRS.h> |
|
#include <AP_Baro.h> |
|
#include <AP_BattMonitor.h> |
|
|
|
/* FrSky sensor hub data IDs */ |
|
#define FRSKY_ID_GPS_ALT_BP 0x01 |
|
#define FRSKY_ID_TEMP1 0x02 |
|
#define FRSKY_ID_RPM 0x03 |
|
#define FRSKY_ID_FUEL 0x04 |
|
#define FRSKY_ID_TEMP2 0x05 |
|
#define FRSKY_ID_VOLTS 0x06 |
|
#define FRSKY_ID_GPS_ALT_AP 0x09 |
|
#define FRSKY_ID_BARO_ALT_BP 0x10 |
|
#define FRSKY_ID_GPS_SPEED_BP 0x11 |
|
#define FRSKY_ID_GPS_LONG_BP 0x12 |
|
#define FRSKY_ID_GPS_LAT_BP 0x13 |
|
#define FRSKY_ID_GPS_COURS_BP 0x14 |
|
#define FRSKY_ID_GPS_DAY_MONTH 0x15 |
|
#define FRSKY_ID_GPS_YEAR 0x16 |
|
#define FRSKY_ID_GPS_HOUR_MIN 0x17 |
|
#define FRSKY_ID_GPS_SEC 0x18 |
|
#define FRSKY_ID_GPS_SPEED_AP 0x19 |
|
#define FRSKY_ID_GPS_LONG_AP 0x1A |
|
#define FRSKY_ID_GPS_LAT_AP 0x1B |
|
#define FRSKY_ID_GPS_COURS_AP 0x1C |
|
#define FRSKY_ID_BARO_ALT_AP 0x21 |
|
#define FRSKY_ID_GPS_LONG_EW 0x22 |
|
#define FRSKY_ID_GPS_LAT_NS 0x23 |
|
#define FRSKY_ID_ACCEL_X 0x24 |
|
#define FRSKY_ID_ACCEL_Y 0x25 |
|
#define FRSKY_ID_ACCEL_Z 0x26 |
|
#define FRSKY_ID_CURRENT 0x28 |
|
#define FRSKY_ID_VARIO 0x30 |
|
#define FRSKY_ID_VFAS 0x39 |
|
#define FRSKY_ID_VOLTS_BP 0x3A |
|
#define FRSKY_ID_VOLTS_AP 0x3B |
|
|
|
// Default sensor data IDs (Physical IDs + CRC) |
|
#define DATA_ID_VARIO 0x00 // 0 |
|
#define DATA_ID_FLVSS 0xA1 // 1 |
|
#define DATA_ID_FAS 0x22 // 2 |
|
#define DATA_ID_GPS 0x83 // 3 |
|
#define DATA_ID_RPM 0xE4 // 4 |
|
#define DATA_ID_SP2UH 0x45 // 5 |
|
#define DATA_ID_SP2UR 0xC6 // 6 |
|
|
|
#define SPORT_START_FRAME 0x7E |
|
|
|
#define HUB_PORT true |
|
#define S_PORT false |
|
|
|
class AP_Frsky_Telem |
|
{ |
|
public: |
|
//constructor |
|
AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) : |
|
_initialised(false), |
|
_ahrs(ahrs), |
|
_battery(battery) |
|
{} |
|
|
|
// these enums must match up with TELEM2_PROTOCOL in vehicle code |
|
enum FrSkyProtocol { |
|
FrSkyDPORT = 2, |
|
FrSkySPORT = 3 |
|
}; |
|
|
|
void init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol); |
|
void send_frames(uint8_t control_mode); |
|
|
|
|
|
private: |
|
|
|
void calc_crc (uint8_t byte); |
|
void send_crc(); |
|
|
|
void frsky_send_byte(uint8_t value); |
|
void frsky_send_hub_startstop(); |
|
void frsky_send_sport_prim(); |
|
|
|
void frsky_send_data(uint8_t id, int16_t data); |
|
|
|
void calc_baro_alt(); |
|
float frsky_format_gps(float dec); |
|
void calc_gps_position(); |
|
void calc_battery(); |
|
void calc_gps_sats(); |
|
|
|
void send_gps_sats (void); |
|
void send_mode (void); |
|
void send_baro_alt_m (void); |
|
void send_baro_alt_cm (void); |
|
void send_batt_remain (void); |
|
void send_batt_volts (void); |
|
void send_current (void); |
|
void send_prearm_error (void); |
|
void send_heading (void); |
|
void send_gps_lat_dd (void); |
|
void send_gps_lat_mm (void); |
|
void send_gps_lat_ns (void); |
|
void send_gps_lon_dd (void); |
|
void send_gps_lon_mm (void); |
|
void send_gps_lon_ew (void); |
|
void send_gps_speed_meter (void); |
|
void send_gps_speed_cm (void); |
|
void send_gps_alt_meter (void); |
|
void send_gps_alt_cm (void); |
|
|
|
void send_hub_frame(); |
|
void sport_tick (); |
|
|
|
AP_HAL::UARTDriver *_port; |
|
bool _initialised; |
|
AP_AHRS &_ahrs; |
|
AP_BattMonitor &_battery; |
|
enum FrSkyProtocol _protocol; |
|
|
|
uint16_t _crc; |
|
uint32_t _last_frame1_ms; |
|
uint32_t _last_frame2_ms; |
|
|
|
bool _battery_data_ready; |
|
uint16_t _batt_remaining; |
|
uint16_t _batt_volts; |
|
uint16_t _batt_amps; |
|
|
|
bool _sats_data_ready; |
|
uint16_t gps_sats; |
|
|
|
bool _gps_data_ready; |
|
bool _pos_gps_ok; |
|
uint16_t _course_in_degrees; |
|
char _lat_ns, _lon_ew; |
|
uint16_t _latdddmm; |
|
uint16_t _latmmmm; |
|
uint16_t _londddmm; |
|
uint16_t _lonmmmm; |
|
uint16_t _alt_gps_meters; |
|
uint16_t _alt_gps_cm ; |
|
int16_t _speed_in_meter; |
|
uint16_t _speed_in_centimeter; |
|
|
|
bool _baro_data_ready; |
|
int16_t _baro_alt_meters ; |
|
uint16_t _baro_alt_cm ; |
|
|
|
bool _mode_data_ready; |
|
uint8_t _mode; |
|
|
|
uint8_t _fas_call ; |
|
uint8_t _gps_call; |
|
uint8_t _vario_call; |
|
uint8_t _various_call; |
|
|
|
uint8_t _sport_status; |
|
}; |
|
#endif
|
|
|