mirror of https://github.com/ArduPilot/ardupilot
AP_FrSky_Telem: added SPort passthrough protocol
This commit is contained in:
floaledm
Andrew Tridgell
parent
baa3357b21
commit
b71e0d73b9
|
|
@ -23,19 +23,72 @@
|
|||
#include "AP_Frsky_Telem.h"
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
AP_Frsky_Telem::msg_t _msg;
|
||||
|
||||
//constructor
|
||||
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery) :
|
||||
AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav) :
|
||||
_ahrs(ahrs),
|
||||
_battery(battery),
|
||||
_rng(rng),
|
||||
_inav(inav),
|
||||
_port(NULL),
|
||||
_protocol(),
|
||||
_initialised_uart(),
|
||||
_crc(0),
|
||||
_control_mode(),
|
||||
_gps()
|
||||
_ap(),
|
||||
_relative_home_altitude(0.0f),
|
||||
_control_sensors_timer(),
|
||||
_paramID(),
|
||||
_gps(),
|
||||
_passthrough(),
|
||||
_SPort(),
|
||||
_D(),
|
||||
_msg_chunk()
|
||||
{}
|
||||
|
||||
/*
|
||||
* init - perform required initialisation
|
||||
* for Copter
|
||||
*/
|
||||
void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, const char *firmware_str, const char *frame_config_str, const uint8_t mav_type, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, uint8_t *control_mode, uint32_t *ap_value, uint32_t *control_sensors_present, uint32_t *control_sensors_enabled, uint32_t *control_sensors_health, int32_t *home_distance, int32_t *home_bearing)
|
||||
{
|
||||
// check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports)
|
||||
if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
|
||||
_protocol = AP_SerialManager::SerialProtocol_FrSky_D; // FrSky D protocol (D-receivers)
|
||||
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
|
||||
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers)
|
||||
} else if ((_port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
|
||||
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough; // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
|
||||
// add firmware and frame info to message queue
|
||||
char text[50];
|
||||
strncpy(text, firmware_str, sizeof(text));
|
||||
strncat(text, " ", 1); // add a space before adding FRAME_CONFIG_STRING
|
||||
strncat(text, frame_config_str, 10); // FRAME_CONFIG_STRING shouldn't be more that 10 chars (see config.h)
|
||||
queue_message(MAV_SEVERITY_INFO, text);
|
||||
// save main parameters locally
|
||||
_params.mav_type = mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h)
|
||||
_params.fs_batt_voltage = fs_batt_voltage; // failsafe battery voltage in volts
|
||||
_params.fs_batt_mah = fs_batt_mah; // failsafe reserve capacity in mAh
|
||||
_ap.value = ap_value; // ap bit-field
|
||||
_ap.control_sensors_present = control_sensors_present;
|
||||
_ap.control_sensors_enabled = control_sensors_enabled;
|
||||
_ap.control_sensors_health = control_sensors_health;
|
||||
_ap.home_distance = home_distance;
|
||||
_ap.home_bearing = home_bearing;
|
||||
}
|
||||
|
||||
_ap.control_mode = control_mode; // flight mode
|
||||
|
||||
if (_port != NULL) {
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::tick, void));
|
||||
// we don't want flow control for either protocol
|
||||
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* init - perform required initialisation
|
||||
* for Plane and Rover
|
||||
*/
|
||||
void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, uint8_t *control_mode)
|
||||
{
|
||||
|
|
@ -46,7 +99,7 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, uint8_t *contr
|
|||
_protocol = AP_SerialManager::SerialProtocol_FrSky_SPort; // FrSky SPort protocol (X-receivers)
|
||||
}
|
||||
|
||||
_control_mode = control_mode;
|
||||
_ap.control_mode = control_mode;
|
||||
|
||||
if (_port != NULL) {
|
||||
hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_Frsky_Telem::tick, void));
|
||||
|
|
@ -55,100 +108,200 @@ void AP_Frsky_Telem::init(const AP_SerialManager &serial_manager, uint8_t *contr
|
|||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* send telemetry data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_SPort_Passthrough(void)
|
||||
{
|
||||
int16_t numc;
|
||||
numc = _port->available();
|
||||
|
||||
// check if available is negative
|
||||
if (numc < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// this is the constant for hub data frame
|
||||
if (_port->txspace() < 19) {
|
||||
return;
|
||||
}
|
||||
|
||||
// keep only the last two bytes of the data found in the serial buffer, as we shouldn't respond to old poll requests
|
||||
uint8_t prev_byte = 0;
|
||||
for (int16_t i = 0; i < numc; i++) {
|
||||
prev_byte = _passthrough.new_byte;
|
||||
_passthrough.new_byte = _port->read();
|
||||
}
|
||||
|
||||
if ((prev_byte == START_STOP_SPORT) && (_passthrough.new_byte == SENSOR_ID_28)) { // byte 0x7E is the header of each poll request
|
||||
if (_passthrough.send_attiandrng) { // skip other data, send attitude (roll, pitch) and range only this iteration
|
||||
_passthrough.send_attiandrng = false; // next iteration, check if we should send something other
|
||||
} else { // check if there's other data to send
|
||||
_passthrough.send_attiandrng = true; // next iteration, send attitude b/c it needs frequent updates to remain smooth
|
||||
// build mavlink message queue for control_sensors flags
|
||||
control_sensors_check();
|
||||
// if there's any message in the queue, start sending them chunk by chunk; three times each chunk
|
||||
if (_msg.sent_idx != _msg.queued_idx) {
|
||||
send_uint32(DIY_FIRST_ID, get_next_msg_chunk());
|
||||
return;
|
||||
}
|
||||
// send other sensor data if it's time for them, and reset the corresponding timer if sent
|
||||
uint32_t now = AP_HAL::millis();
|
||||
if (((now - _passthrough.timer_params) > 1000) && (!AP_Notify::flags.armed)) {
|
||||
send_uint32(DIY_FIRST_ID+7, calc_param());
|
||||
_passthrough.timer_params = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_ap_status) > 500) {
|
||||
send_uint32(DIY_FIRST_ID+1, calc_ap_status());
|
||||
_passthrough.timer_ap_status = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_batt) > 1000) {
|
||||
send_uint32(DIY_FIRST_ID+3, calc_batt());
|
||||
_passthrough.timer_batt = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_gps_status) > 1000) {
|
||||
send_uint32(DIY_FIRST_ID+2, calc_gps_status());
|
||||
_passthrough.timer_gps_status = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_home) > 500) {
|
||||
send_uint32(DIY_FIRST_ID+4, calc_home());
|
||||
_passthrough.timer_home = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_velandyaw) > 500) {
|
||||
send_uint32(DIY_FIRST_ID+5, calc_velandyaw());
|
||||
_passthrough.timer_velandyaw = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_gps_latlng) > 1000) {
|
||||
send_uint32(GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
|
||||
if (!_passthrough.send_latitude) { // we've cycled and sent one each of longitude then latitude, so reset the timer
|
||||
_passthrough.timer_gps_latlng = AP_HAL::millis();
|
||||
}
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_vario) > 500) {
|
||||
send_uint32(VARIO_FIRST_ID, (int32_t)roundf(_inav.get_velocity_z())); // vertical velocity in cm/s
|
||||
_passthrough.timer_vario = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_alt) > 1000) {
|
||||
send_uint32(ALT_FIRST_ID, (int32_t)_relative_home_altitude); // altitude in cm above home position
|
||||
_passthrough.timer_alt = AP_HAL::millis();
|
||||
return;
|
||||
} else if ((now - _passthrough.timer_vfas) > 1000) {
|
||||
send_uint32(VFAS_FIRST_ID, (uint32_t)roundf(_battery.voltage() * 100.0f)); // battery pack voltage in volts
|
||||
_passthrough.timer_vfas = AP_HAL::millis();
|
||||
return;
|
||||
}
|
||||
}
|
||||
// if nothing else needed to be sent, send attitude (roll, pitch) and range data
|
||||
send_uint32(DIY_FIRST_ID+6, calc_attiandrng());
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* send telemetry data
|
||||
* for FrSky SPort protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_SPort(void)
|
||||
{
|
||||
// variables keeping track of which data to send
|
||||
static uint8_t fas_call = 1;
|
||||
static uint8_t gps_call = 1;
|
||||
static uint8_t vario_call = 1;
|
||||
static uint8_t various_call = 1;
|
||||
int16_t numc;
|
||||
numc = _port->available();
|
||||
|
||||
while (_port->available()) {
|
||||
uint8_t readbyte = _port->read();
|
||||
if (readbyte == START_STOP_SPORT) { // byte 0x7E is the header of each poll request
|
||||
readbyte = _port->read();
|
||||
// check if available is negative
|
||||
if (numc < 0) {
|
||||
return;
|
||||
}
|
||||
|
||||
// this is the constant for hub data frame
|
||||
if (_port->txspace() < 19) {
|
||||
return;
|
||||
}
|
||||
|
||||
for (int16_t i = 0; i < numc; i++) {
|
||||
int16_t readbyte = _port->read();
|
||||
if (_SPort.sport_status == false) {
|
||||
if (readbyte == START_STOP_SPORT) {
|
||||
_SPort.sport_status = true;
|
||||
}
|
||||
} else {
|
||||
switch(readbyte) {
|
||||
case SENSOR_ID_FAS:
|
||||
switch (fas_call) {
|
||||
switch (_SPort.fas_call) {
|
||||
case 0:
|
||||
send_uint32(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
break;
|
||||
case 1:
|
||||
send_data(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
send_uint32(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
break;
|
||||
case 2:
|
||||
send_data(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
break;
|
||||
case 3:
|
||||
send_data(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
send_uint32(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
break;
|
||||
}
|
||||
if (fas_call++ > 3) fas_call = 0;
|
||||
if (_SPort.fas_call++ > 2) _SPort.fas_call = 0;
|
||||
break;
|
||||
case SENSOR_ID_GPS:
|
||||
switch (gps_call) {
|
||||
case 1:
|
||||
switch (_SPort.gps_call) {
|
||||
case 0:
|
||||
calc_gps_position(); // gps data is not recalculated until all of it has been sent
|
||||
send_data(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
send_uint32(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
break;
|
||||
case 1:
|
||||
send_uint32(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
break;
|
||||
case 2:
|
||||
send_data(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
send_uint32(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
break;
|
||||
case 3:
|
||||
send_data(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
send_uint32(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
break;
|
||||
case 4:
|
||||
send_data(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
send_uint32(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
break;
|
||||
case 5:
|
||||
send_data(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
send_uint32(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
break;
|
||||
case 6:
|
||||
send_data(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
send_uint32(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
break;
|
||||
case 7:
|
||||
send_data(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
send_uint32(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
break;
|
||||
case 8:
|
||||
send_data(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
send_uint32(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
break;
|
||||
case 9:
|
||||
send_data(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
send_uint32(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
|
||||
break;
|
||||
case 10:
|
||||
send_data(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimals
|
||||
break;
|
||||
case 11:
|
||||
send_data(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
send_uint32(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
break;
|
||||
}
|
||||
if (gps_call++ > 11) gps_call = 1;
|
||||
if (_SPort.gps_call++ > 10) _SPort.gps_call = 0;
|
||||
break;
|
||||
case SENSOR_ID_VARIO:
|
||||
switch (vario_call) {
|
||||
case 1 :
|
||||
switch (_SPort.vario_call) {
|
||||
case 0 :
|
||||
calc_nav_alt(); // nav altitude is not recalculated until all of it has been sent
|
||||
send_data(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
|
||||
send_uint32(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send altitude integer part
|
||||
break;
|
||||
case 2:
|
||||
send_data(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
|
||||
case 1:
|
||||
send_uint32(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send altitude decimal part
|
||||
break;
|
||||
}
|
||||
if (vario_call++ > 2) vario_call = 1;
|
||||
if (_SPort.vario_call++ > 1) _SPort.vario_call = 0;
|
||||
break;
|
||||
case SENSOR_ID_SP2UR:
|
||||
switch (various_call) {
|
||||
case 1 :
|
||||
send_data(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
switch (_SPort.various_call) {
|
||||
case 0 :
|
||||
send_uint32(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
break;
|
||||
case 2:
|
||||
send_data(DATA_ID_TEMP1, (uint16_t)*_control_mode); // send flight mode
|
||||
case 1:
|
||||
send_uint32(DATA_ID_TEMP1, (uint16_t)*_ap.control_mode); // send flight mode
|
||||
break;
|
||||
}
|
||||
if (various_call++ > 2) various_call = 1;
|
||||
if (_SPort.various_call++ > 1) _SPort.various_call = 0;
|
||||
break;
|
||||
}
|
||||
_SPort.sport_status = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -161,38 +314,35 @@ void AP_Frsky_Telem::send_SPort(void)
|
|||
*/
|
||||
void AP_Frsky_Telem::send_D(void)
|
||||
{
|
||||
static uint32_t last_200ms_frame = 0;
|
||||
static uint32_t last_1000ms_frame = 0;
|
||||
|
||||
uint32_t now = AP_HAL::millis();
|
||||
// send frame1 every 200ms
|
||||
if (now - last_200ms_frame > 200) {
|
||||
last_200ms_frame = now;
|
||||
send_data(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
send_data(DATA_ID_TEMP1, (uint16_t)*_control_mode); // send flight mode
|
||||
send_data(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
send_data(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
send_data(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
if (now - _D.last_200ms_frame > 200) {
|
||||
_D.last_200ms_frame = now;
|
||||
send_uint16(DATA_ID_TEMP2, (uint16_t)(_ahrs.get_gps().num_sats() * 10 + _ahrs.get_gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
|
||||
send_uint16(DATA_ID_TEMP1, (uint16_t)*_ap.control_mode); // send flight mode
|
||||
send_uint16(DATA_ID_FUEL, (uint16_t)roundf(_battery.capacity_remaining_pct())); // send battery remaining
|
||||
send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
|
||||
send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(_battery.current_amps() * 10.0f)); // send current consumption
|
||||
calc_nav_alt();
|
||||
send_data(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
|
||||
send_data(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
|
||||
send_uint16(DATA_ID_BARO_ALT_BP, _gps.alt_nav_meters); // send nav altitude integer part
|
||||
send_uint16(DATA_ID_BARO_ALT_AP, _gps.alt_nav_cm); // send nav altitude decimal part
|
||||
}
|
||||
// send frame2 every second
|
||||
if (now - last_1000ms_frame > 1000) {
|
||||
last_1000ms_frame = now;
|
||||
send_data(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
if (now - _D.last_1000ms_frame > 1000) {
|
||||
_D.last_1000ms_frame = now;
|
||||
send_uint16(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS
|
||||
calc_gps_position();
|
||||
if (_ahrs.get_gps().status() >= 3) {
|
||||
send_data(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
send_data(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
send_data(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
send_data(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
send_data(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
send_data(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
send_data(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
send_data(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
send_data(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
send_data(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimal part
|
||||
send_uint16(DATA_ID_GPS_LAT_BP, _gps.latdddmm); // send gps lattitude degree and minute integer part
|
||||
send_uint16(DATA_ID_GPS_LAT_AP, _gps.latmmmm); // send gps lattitude minutes decimal part
|
||||
send_uint16(DATA_ID_GPS_LAT_NS, _gps.lat_ns); // send gps North / South information
|
||||
send_uint16(DATA_ID_GPS_LONG_BP, _gps.londddmm); // send gps longitude degree and minute integer part
|
||||
send_uint16(DATA_ID_GPS_LONG_AP, _gps.lonmmmm); // send gps longitude minutes decimal part
|
||||
send_uint16(DATA_ID_GPS_LONG_EW, _gps.lon_ew); // send gps East / West information
|
||||
send_uint16(DATA_ID_GPS_SPEED_BP, _gps.speed_in_meter); // send gps speed integer part
|
||||
send_uint16(DATA_ID_GPS_SPEED_AP, _gps.speed_in_centimeter); // send gps speed decimal part
|
||||
send_uint16(DATA_ID_GPS_ALT_BP, _gps.alt_gps_meters); // send gps altitude integer part
|
||||
send_uint16(DATA_ID_GPS_ALT_AP, _gps.alt_gps_cm); // send gps altitude decimal part
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -202,22 +352,23 @@ void AP_Frsky_Telem::send_D(void)
|
|||
*/
|
||||
void AP_Frsky_Telem::tick(void)
|
||||
{
|
||||
static bool initialised_uart = false; // true when we have detected the protocol and UART has been initialised
|
||||
// check UART has been initialised
|
||||
if (!initialised_uart) {
|
||||
if (!_initialised_uart) {
|
||||
// initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
_port->begin(AP_SERIALMANAGER_FRSKY_D_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
|
||||
} else { // FrSky SPort protocol (X-receivers)
|
||||
} else { // FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
|
||||
_port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX);
|
||||
}
|
||||
initialised_uart = true;
|
||||
_initialised_uart = true;// true when we have detected the protocol and UART has been initialised
|
||||
}
|
||||
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
send_D();
|
||||
} else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort) { // FrSky SPort protocol (X-receivers)
|
||||
send_SPort();
|
||||
} else if (_protocol == AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough) { // FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
send_SPort_Passthrough();
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -273,29 +424,378 @@ void AP_Frsky_Telem::send_byte(uint8_t byte)
|
|||
}
|
||||
|
||||
/*
|
||||
* send one frame of FrSky data
|
||||
* send one uint32 frame of FrSky data - for FrSky SPort protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_data(uint16_t id, uint32_t data)
|
||||
void AP_Frsky_Telem::send_uint32(uint16_t id, uint32_t data)
|
||||
{
|
||||
if (_protocol == AP_SerialManager::SerialProtocol_FrSky_D) { // FrSky D protocol (D-receivers)
|
||||
_port->write(START_STOP_D); // send a 0x5E start byte
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
send_byte(bytes[0]);
|
||||
bytes = (uint8_t*)&data;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]); // MSB
|
||||
} else { // FrSky SPort protocol (X-receivers)
|
||||
send_byte(0x10); // DATA_FRAME
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]); // MSB
|
||||
bytes = (uint8_t*)&data;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]);
|
||||
send_byte(bytes[2]);
|
||||
send_byte(bytes[3]); // MSB
|
||||
send_crc();
|
||||
send_byte(0x10); // DATA_FRAME
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]); // MSB
|
||||
bytes = (uint8_t*)&data;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]);
|
||||
send_byte(bytes[2]);
|
||||
send_byte(bytes[3]); // MSB
|
||||
send_crc();
|
||||
}
|
||||
|
||||
/*
|
||||
* send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::send_uint16(uint16_t id, uint16_t data)
|
||||
{
|
||||
_port->write(START_STOP_D); // send a 0x5E start byte
|
||||
uint8_t *bytes = (uint8_t*)&id;
|
||||
send_byte(bytes[0]);
|
||||
bytes = (uint8_t*)&data;
|
||||
send_byte(bytes[0]); // LSB
|
||||
send_byte(bytes[1]); // MSB
|
||||
}
|
||||
|
||||
/*
|
||||
* grabs one "chunk" (4 bytes) of the mavlink statustext message to be transmitted
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::get_next_msg_chunk(void)
|
||||
{
|
||||
if (_msg_chunk.repeats == 0) {
|
||||
_msg_chunk.chunk = 0;
|
||||
uint8_t character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
|
||||
if (character) {
|
||||
_msg_chunk.chunk |= character<<24;
|
||||
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
|
||||
if (character) {
|
||||
_msg_chunk.chunk |= character<<16;
|
||||
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
|
||||
if (character) {
|
||||
_msg_chunk.chunk |= character<<8;
|
||||
character = _msg.data[_msg.sent_idx].text[_msg_chunk.char_index++];
|
||||
if (character) {
|
||||
_msg_chunk.chunk |= character;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
if (!character) { // we've reached the end of the message (string terminated by '\0')
|
||||
_msg_chunk.char_index = 0;
|
||||
// add severity which is sent as the MSB of the last three bytes of the last chunk (bits 24, 16, and 8) since a character is on 7 bits
|
||||
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x4)<<21;
|
||||
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x2)<<14;
|
||||
_msg_chunk.chunk |= (_msg.data[_msg.sent_idx].severity & 0x1)<<7;
|
||||
}
|
||||
}
|
||||
_msg_chunk.repeats++;
|
||||
if (_msg_chunk.repeats > 2) {
|
||||
_msg_chunk.repeats = 0;
|
||||
_msg.sent_idx = (_msg.sent_idx + 1) % MSG_BUFFER_LENGTH;
|
||||
}
|
||||
return _msg_chunk.chunk;
|
||||
}
|
||||
|
||||
/*
|
||||
* add message to message cue for transmission through FrSky link
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::queue_message(MAV_SEVERITY severity, const char *text)
|
||||
{
|
||||
_msg.data[_msg.queued_idx].severity = severity;
|
||||
strncpy((char *)_msg.data[_msg.queued_idx].text, text, 50);
|
||||
_msg.queued_idx = (_msg.queued_idx + 1) % MSG_BUFFER_LENGTH;
|
||||
}
|
||||
|
||||
/*
|
||||
* add control_sensors information to message cue, normally passed as sys_status mavlink messages to the GCS, for transmission through FrSky link
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
void AP_Frsky_Telem::control_sensors_check(void)
|
||||
{
|
||||
uint32_t now = AP_HAL::millis();
|
||||
|
||||
if ((now - _control_sensors_timer) > 5000) { // prevent repeating any system_status messages unless 5 seconds have passed
|
||||
uint32_t _control_sensors_flags = (*_ap.control_sensors_health ^ *_ap.control_sensors_enabled) & *_ap.control_sensors_present;
|
||||
// only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner.
|
||||
if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach");
|
||||
_control_sensors_timer = now;
|
||||
} else if ((_control_sensors_flags & MAV_SYS_STATUS_AHRS) > 0) {
|
||||
queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS");
|
||||
_control_sensors_timer = now;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare parameter data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_param(void)
|
||||
{
|
||||
uint32_t param = 0;
|
||||
|
||||
// cycle through paramIDs
|
||||
if (_paramID >= 4) {
|
||||
_paramID = 0;
|
||||
}
|
||||
_paramID++;
|
||||
switch(_paramID) {
|
||||
case 1:
|
||||
param = _params.mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h)
|
||||
break;
|
||||
case 2:
|
||||
param = (uint32_t)roundf(*_params.fs_batt_voltage * 100.0f); // battery failsafe voltage in centivolts
|
||||
break;
|
||||
case 3:
|
||||
param = (uint32_t)roundf(*_params.fs_batt_mah); // battery failsafe capacity in mAh
|
||||
break;
|
||||
case 4:
|
||||
param = (uint32_t)roundf(_battery.pack_capacity_mah()); // battery pack capacity in mAh as configured by user
|
||||
break;
|
||||
}
|
||||
//Reserve first 8 bits for param ID, use other 24 bits to store parameter value
|
||||
param = (_paramID << 24) | (param & 0xFFFFFF);
|
||||
|
||||
return param;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare gps latitude/longitude data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_gps_latlng(bool *send_latitude)
|
||||
{
|
||||
uint32_t latlng;
|
||||
const Location &loc = _ahrs.get_gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
|
||||
|
||||
// alternate between latitude and longitude
|
||||
if (*send_latitude == true) {
|
||||
if (loc.lat < 0) {
|
||||
latlng = ((abs(loc.lat)/100)*6) | 0x40000000;
|
||||
} else {
|
||||
latlng = ((abs(loc.lat)/100)*6);
|
||||
}
|
||||
*send_latitude = false;
|
||||
} else {
|
||||
if (loc.lng < 0) {
|
||||
latlng = ((abs(loc.lng)/100)*6) | 0xC0000000;
|
||||
} else {
|
||||
latlng = ((abs(loc.lng)/100)*6) | 0x80000000;
|
||||
}
|
||||
*send_latitude = true;
|
||||
}
|
||||
return latlng;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare gps status data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_gps_status(void)
|
||||
{
|
||||
uint32_t gps_status;
|
||||
|
||||
// number of GPS satellites visible (limit to 15 (0xF) since the value is stored on 4 bits)
|
||||
gps_status = (_ahrs.get_gps().num_sats() < GPS_SATS_LIMIT) ? _ahrs.get_gps().num_sats() : GPS_SATS_LIMIT;
|
||||
// GPS receiver status (limit to 3 (0x3) since the value is stored on 2 bits: NO_GPS = 0, NO_FIX = 1, GPS_OK_FIX_2D = 2, GPS_OK_FIX_3D or GPS_OK_FIX_3D_DGPS or GPS_OK_FIX_3D_RTK = 3)
|
||||
gps_status |= ((_ahrs.get_gps().status() < GPS_STATUS_LIMIT) ? _ahrs.get_gps().status() : GPS_STATUS_LIMIT)<<GPS_STATUS_OFFSET;
|
||||
// GPS horizontal dilution of precision in dm
|
||||
gps_status |= prep_number(roundf(_ahrs.get_gps().get_hdop() * 0.1f),2,1)<<GPS_HDOP_OFFSET;
|
||||
// GPS vertical dilution of precision in dm
|
||||
gps_status |= prep_number(roundf(_ahrs.get_gps().get_vdop() * 0.1f),2,1)<<GPS_VDOP_OFFSET;
|
||||
// Altitude MSL in dm
|
||||
Location loc = _ahrs.get_gps().location();
|
||||
gps_status |= prep_number(roundf(loc.alt * 0.1f),2,2)<<GPS_ALTMSL_OFFSET;
|
||||
return gps_status;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare battery data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_batt(void)
|
||||
{
|
||||
uint32_t batt;
|
||||
|
||||
// battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
|
||||
batt = (((uint16_t)roundf(_battery.voltage() * 10.0f)) & BATT_VOLTAGE_LIMIT);
|
||||
// battery current draw in deciamps
|
||||
batt |= prep_number(roundf(_battery.current_amps() * 10.0f), 2, 1)<<BATT_CURRENT_OFFSET;
|
||||
// battery current drawn since power on in mAh (limit to 32767 (0x7FFF) since value is stored on 15 bits)
|
||||
batt |= ((_battery.current_total_mah() < BATT_TOTALMAH_LIMIT) ? ((uint16_t)roundf(_battery.current_total_mah()) & BATT_TOTALMAH_LIMIT) : BATT_TOTALMAH_LIMIT)<<BATT_TOTALMAH_OFFSET;
|
||||
return batt;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare various autopilot status data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_ap_status(void)
|
||||
{
|
||||
uint32_t ap_status;
|
||||
|
||||
// control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits)
|
||||
ap_status = (uint8_t)(*_ap.control_mode & AP_CONTROL_MODE_LIMIT);
|
||||
// simple/super simple modes flags
|
||||
ap_status |= (uint8_t)(*_ap.value & AP_SSIMPLE_FLAGS)<<AP_SSIMPLE_OFFSET;
|
||||
// land complete flag
|
||||
ap_status |= (uint8_t)(*_ap.value & AP_LANDCOMPLETE_FLAG);
|
||||
// armed flag
|
||||
ap_status |= (uint8_t)(AP_Notify::flags.armed)<<AP_ARMED_OFFSET;
|
||||
// battery failsafe flag
|
||||
ap_status |= (uint8_t)(AP_Notify::flags.failsafe_battery)<<AP_BATT_FS_OFFSET;
|
||||
// bad ekf flag
|
||||
ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
|
||||
return ap_status;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare home position related data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_home(void)
|
||||
{
|
||||
uint32_t home;
|
||||
|
||||
// distance between vehicle and home location in meters
|
||||
home = prep_number(roundf(*_ap.home_distance * 0.01f), 3, 2);
|
||||
// altitude between vehicle and home location in decimeters
|
||||
Location loc;
|
||||
if (_ahrs.get_position(loc)) {
|
||||
_relative_home_altitude = loc.alt;
|
||||
if (!loc.flags.relative_alt) {
|
||||
// loc.alt has home altitude added, remove it
|
||||
_relative_home_altitude -= _ahrs.get_home().alt;
|
||||
}
|
||||
} else {
|
||||
_relative_home_altitude = 0;
|
||||
}
|
||||
home |= prep_number(roundf(_relative_home_altitude * 0.1f), 3, 2)<<HOME_ALT_OFFSET;
|
||||
// angle between vehicle and home loc (relative to North) in 3 degree increments (just in case, limit to 127 (0x7F) since the value is stored on 7 bits)
|
||||
home |= (((uint8_t)roundf(*_ap.home_bearing * 0.00333f)) & HOME_BEARING_LIMIT)<<HOME_BEARING_OFFSET;
|
||||
return home;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare velocity and yaw data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_velandyaw(void)
|
||||
{
|
||||
uint32_t velandyaw;
|
||||
|
||||
// vertical velocity in dm/s
|
||||
velandyaw = prep_number(roundf(_inav.get_velocity_z() * 0.1f), 2, 1);
|
||||
// horizontal velocity in dm/s
|
||||
velandyaw |= prep_number(roundf(_inav.get_velocity_xy() * 0.1f), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
|
||||
// yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
|
||||
velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
|
||||
return velandyaw;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare attitude (roll, pitch) and range data
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint32_t AP_Frsky_Telem::calc_attiandrng(void)
|
||||
{
|
||||
uint32_t attiandrng;
|
||||
|
||||
// roll from [-18000;18000] centidegrees to unsigned .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
|
||||
attiandrng = ((uint16_t)roundf((_ahrs.roll_sensor + 18000) * 0.05f) & ATTIANDRNG_ROLL_LIMIT);
|
||||
// pitch from [-9000;9000] centidegrees to unsigned .2 degree increments [0;900] (just in case, limit to 1023 (0x3FF) since the value is stored on 10 bits)
|
||||
attiandrng |= ((uint16_t)roundf((_ahrs.pitch_sensor + 9000) * 0.05f) & ATTIANDRNG_PITCH_LIMIT)<<ATTIANDRNG_PITCH_OFFSET;
|
||||
// rangefinder measurement in cm
|
||||
attiandrng |= prep_number(_rng.distance_cm(), 3, 1)<<ATTIANDRNG_RNGFND_OFFSET;
|
||||
return attiandrng;
|
||||
}
|
||||
|
||||
/*
|
||||
* prepare value for transmission through FrSky link
|
||||
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
|
||||
*/
|
||||
uint16_t AP_Frsky_Telem::prep_number(int32_t number, uint8_t digits, uint8_t power)
|
||||
{
|
||||
uint16_t res = 0;
|
||||
uint32_t abs_number = abs(number);
|
||||
|
||||
if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power
|
||||
if (abs_number < 100) {
|
||||
res = abs_number<<1;
|
||||
} else if (abs_number < 1270) {
|
||||
res = ((uint8_t)roundf(abs_number * 0.1f)<<1)|0x1;
|
||||
} else { // transmit max possible value (0x7F x 10^1 = 1270)
|
||||
res = 0xFF;
|
||||
}
|
||||
if (number < 0) { // if number is negative, add sign bit in front
|
||||
res |= 0x1<<8;
|
||||
}
|
||||
} else if ((digits == 2) && (power == 2)) { // number encoded on 9 bits: 7 bits for digits + 2 for 10^power
|
||||
if (abs_number < 100) {
|
||||
res = abs_number<<2;
|
||||
} else if (abs_number < 1000) {
|
||||
res = ((uint8_t)roundf(abs_number * 0.1f)<<2)|0x1;
|
||||
} else if (abs_number < 10000) {
|
||||
res = ((uint8_t)roundf(abs_number * 0.01f)<<2)|0x2;
|
||||
} else if (abs_number < 127000) {
|
||||
res = ((uint8_t)roundf(abs_number * 0.001f)<<2)|0x3;
|
||||
} else { // transmit max possible value (0x7F x 10^3 = 127000)
|
||||
res = 0x1FF;
|
||||
}
|
||||
if (number < 0) { // if number is negative, add sign bit in front
|
||||
res |= 0x1<<9;
|
||||
}
|
||||
} else if ((digits == 3) && (power == 1)) { // number encoded on 11 bits: 10 bits for digits + 1 for 10^power
|
||||
if (abs_number < 1000) {
|
||||
res = abs_number<<1;
|
||||
} else if (abs_number < 10240) {
|
||||
res = ((uint16_t)roundf(abs_number * 0.1f)<<1)|0x1;
|
||||
} else { // transmit max possible value (0x3FF x 10^1 = 10240)
|
||||
res = 0x7FF;
|
||||
}
|
||||
if (number < 0) { // if number is negative, add sign bit in front
|
||||
res |= 0x1<<11;
|
||||
}
|
||||
} else if ((digits == 3) && (power == 2)) { // number encoded on 12 bits: 10 bits for digits + 2 for 10^power
|
||||
if (abs_number < 1000) {
|
||||
res = abs_number<<2;
|
||||
} else if (abs_number < 10000) {
|
||||
res = ((uint16_t)roundf(abs_number * 0.1f)<<2)|0x1;
|
||||
} else if (abs_number < 100000) {
|
||||
res = ((uint16_t)roundf(abs_number * 0.01f)<<2)|0x2;
|
||||
} else if (abs_number < 1024000) {
|
||||
res = ((uint16_t)roundf(abs_number * 0.001f)<<2)|0x3;
|
||||
} else { // transmit max possible value (0x3FF x 10^3 = 127000)
|
||||
res = 0xFFF;
|
||||
}
|
||||
if (number < 0) { // if number is negative, add sign bit in front
|
||||
res |= 0x1<<12;
|
||||
}
|
||||
}
|
||||
return res;
|
||||
}
|
||||
|
||||
/*
|
||||
|
|
@ -304,17 +804,18 @@ void AP_Frsky_Telem::send_data(uint16_t id, uint32_t data)
|
|||
*/
|
||||
void AP_Frsky_Telem::calc_nav_alt(void)
|
||||
{
|
||||
Location current_loc;
|
||||
Location loc;
|
||||
float current_height; // in centimeters above home
|
||||
if (_ahrs.get_position(current_loc)) {
|
||||
if (current_loc.flags.relative_alt) {
|
||||
current_height = current_loc.alt*0.01f;
|
||||
} else {
|
||||
current_height = (current_loc.alt - _ahrs.get_home().alt)*0.01f;
|
||||
if (_ahrs.get_position(loc)) {
|
||||
current_height = loc.alt*0.01f;
|
||||
if (!loc.flags.relative_alt) {
|
||||
// loc.alt has home altitude added, remove it
|
||||
current_height -= _ahrs.get_home().alt*0.01f;
|
||||
}
|
||||
} else {
|
||||
current_height = 0;
|
||||
}
|
||||
|
||||
_gps.alt_nav_meters = (int16_t)current_height;
|
||||
_gps.alt_nav_cm = (current_height - _gps.alt_nav_meters) * 100;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,80 +18,206 @@
|
|||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_AHRS/AP_AHRS.h>
|
||||
#include <AP_BattMonitor/AP_BattMonitor.h>
|
||||
#include <AP_InertialNav/AP_InertialNav.h>
|
||||
#include <AP_Notify/AP_Notify.h>
|
||||
#include <AP_RangeFinder/AP_RangeFinder.h>
|
||||
#include <AP_SerialManager/AP_SerialManager.h>
|
||||
|
||||
#define MSG_BUFFER_LENGTH 5 // size of the message buffer queue (number of messages waiting to be sent)
|
||||
|
||||
/*
|
||||
for FrSky D protocol (D-receivers)
|
||||
*/
|
||||
// FrSky sensor hub data IDs
|
||||
#define DATA_ID_GPS_ALT_BP 0x01
|
||||
#define DATA_ID_TEMP1 0x02
|
||||
#define DATA_ID_FUEL 0x04
|
||||
#define DATA_ID_TEMP2 0x05
|
||||
#define DATA_ID_GPS_ALT_AP 0x09
|
||||
#define DATA_ID_BARO_ALT_BP 0x10
|
||||
#define DATA_ID_GPS_SPEED_BP 0x11
|
||||
#define DATA_ID_GPS_LONG_BP 0x12
|
||||
#define DATA_ID_GPS_LAT_BP 0x13
|
||||
#define DATA_ID_GPS_COURS_BP 0x14
|
||||
#define DATA_ID_GPS_SPEED_AP 0x19
|
||||
#define DATA_ID_GPS_LONG_AP 0x1A
|
||||
#define DATA_ID_GPS_LAT_AP 0x1B
|
||||
#define DATA_ID_BARO_ALT_AP 0x21
|
||||
#define DATA_ID_GPS_LONG_EW 0x22
|
||||
#define DATA_ID_GPS_LAT_NS 0x23
|
||||
#define DATA_ID_CURRENT 0x28
|
||||
#define DATA_ID_VFAS 0x39
|
||||
#define DATA_ID_GPS_ALT_BP 0x01
|
||||
#define DATA_ID_TEMP1 0x02
|
||||
#define DATA_ID_FUEL 0x04
|
||||
#define DATA_ID_TEMP2 0x05
|
||||
#define DATA_ID_GPS_ALT_AP 0x09
|
||||
#define DATA_ID_BARO_ALT_BP 0x10
|
||||
#define DATA_ID_GPS_SPEED_BP 0x11
|
||||
#define DATA_ID_GPS_LONG_BP 0x12
|
||||
#define DATA_ID_GPS_LAT_BP 0x13
|
||||
#define DATA_ID_GPS_COURS_BP 0x14
|
||||
#define DATA_ID_GPS_SPEED_AP 0x19
|
||||
#define DATA_ID_GPS_LONG_AP 0x1A
|
||||
#define DATA_ID_GPS_LAT_AP 0x1B
|
||||
#define DATA_ID_BARO_ALT_AP 0x21
|
||||
#define DATA_ID_GPS_LONG_EW 0x22
|
||||
#define DATA_ID_GPS_LAT_NS 0x23
|
||||
#define DATA_ID_CURRENT 0x28
|
||||
#define DATA_ID_VFAS 0x39
|
||||
|
||||
#define START_STOP_D 0x5E
|
||||
#define BYTESTUFF_D 0x5D
|
||||
#define START_STOP_D 0x5E
|
||||
#define BYTESTUFF_D 0x5D
|
||||
|
||||
/*
|
||||
for FrSky SPort protocol (X-receivers)
|
||||
for FrSky SPort and SPort Passthrough (OpenTX) protocols (X-receivers)
|
||||
*/
|
||||
// FrSky Sensor IDs
|
||||
#define SENSOR_ID_VARIO 0x00 // Sensor ID 0
|
||||
#define SENSOR_ID_FAS 0x22 // Sensor ID 2
|
||||
#define SENSOR_ID_GPS 0x83 // Sensor ID 3
|
||||
#define SENSOR_ID_SP2UR 0xC6 // Sensor ID 6
|
||||
#define SENSOR_ID_28 0x1B // Sensor ID 28
|
||||
#define SENSOR_ID_VARIO 0x00 // Sensor ID 0
|
||||
#define SENSOR_ID_FAS 0x22 // Sensor ID 2
|
||||
#define SENSOR_ID_GPS 0x83 // Sensor ID 3
|
||||
#define SENSOR_ID_SP2UR 0xC6 // Sensor ID 6
|
||||
#define SENSOR_ID_28 0x1B // Sensor ID 28
|
||||
|
||||
// FrSky data IDs
|
||||
#define ALT_FIRST_ID 0x0100
|
||||
#define VARIO_FIRST_ID 0x0110
|
||||
#define VFAS_FIRST_ID 0x0210
|
||||
#define GPS_LONG_LATI_FIRST_ID 0x0800
|
||||
#define DIY_FIRST_ID 0x5000
|
||||
|
||||
#define START_STOP_SPORT 0x7E
|
||||
#define BYTESTUFF_SPORT 0x7D
|
||||
|
||||
/*
|
||||
for FrSky SPort Passthrough
|
||||
*/
|
||||
// data bits preparation
|
||||
// for gps status data
|
||||
#define GPS_SATS_LIMIT 0xF
|
||||
#define GPS_STATUS_LIMIT 0x3
|
||||
#define GPS_STATUS_OFFSET 4
|
||||
#define GPS_HDOP_OFFSET 6
|
||||
#define GPS_VDOP_OFFSET 14
|
||||
#define GPS_ALTMSL_OFFSET 22
|
||||
// for battery data
|
||||
#define BATT_VOLTAGE_LIMIT 0x1FF
|
||||
#define BATT_CURRENT_OFFSET 9
|
||||
#define BATT_TOTALMAH_LIMIT 0x7FFF
|
||||
#define BATT_TOTALMAH_OFFSET 17
|
||||
// for autopilot status data
|
||||
#define AP_CONTROL_MODE_LIMIT 0x1F
|
||||
#define AP_SSIMPLE_FLAGS 0x6
|
||||
#define AP_SSIMPLE_OFFSET 4
|
||||
#define AP_LANDCOMPLETE_FLAG 0x80
|
||||
#define AP_ARMED_OFFSET 8
|
||||
#define AP_BATT_FS_OFFSET 9
|
||||
#define AP_EKF_FS_OFFSET 10
|
||||
// for home position related data
|
||||
#define HOME_ALT_OFFSET 12
|
||||
#define HOME_BEARING_LIMIT 0x7F
|
||||
#define HOME_BEARING_OFFSET 25
|
||||
// for velocity and yaw data
|
||||
#define VELANDYAW_XYVEL_OFFSET 9
|
||||
#define VELANDYAW_YAW_LIMIT 0x7FF
|
||||
#define VELANDYAW_YAW_OFFSET 17
|
||||
// for attitude (roll, pitch) and range data
|
||||
#define ATTIANDRNG_ROLL_LIMIT 0x7FF
|
||||
#define ATTIANDRNG_PITCH_LIMIT 0x3FF
|
||||
#define ATTIANDRNG_PITCH_OFFSET 11
|
||||
#define ATTIANDRNG_RNGFND_OFFSET 21
|
||||
|
||||
|
||||
#define START_STOP_SPORT 0x7E
|
||||
#define BYTESTUFF_SPORT 0x7D
|
||||
|
||||
class AP_Frsky_Telem
|
||||
{
|
||||
public:
|
||||
//constructor
|
||||
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery);
|
||||
AP_Frsky_Telem(AP_AHRS &ahrs, const AP_BattMonitor &battery, const RangeFinder &rng, const AP_InertialNav_NavEKF &inav);
|
||||
|
||||
// init - perform required initialisation
|
||||
void init(const AP_SerialManager &serial_manager, const char *firmware_str, const char *frame_config_str, const uint8_t mav_type, AP_Float *fs_batt_voltage, AP_Float *fs_batt_mah, uint8_t *control_mode, uint32_t *ap_value, uint32_t *control_sensors_present, uint32_t *control_sensors_enabled, uint32_t *control_sensors_health, int32_t *home_distance, int32_t *home_bearing);
|
||||
void init(const AP_SerialManager &serial_manager, uint8_t *control_mode);
|
||||
// add statustext message to FrSky lib queue. This function is static so it can be called from any library.
|
||||
static void queue_message(MAV_SEVERITY severity, const char *text);
|
||||
|
||||
struct msg_t
|
||||
{
|
||||
mavlink_statustext_t data[MSG_BUFFER_LENGTH];
|
||||
uint8_t queued_idx;
|
||||
uint8_t sent_idx;
|
||||
};
|
||||
|
||||
private:
|
||||
AP_AHRS &_ahrs;
|
||||
const AP_BattMonitor &_battery;
|
||||
const RangeFinder &_rng;
|
||||
const AP_InertialNav &_inav;
|
||||
AP_HAL::UARTDriver *_port; // UART used to send data to FrSky receiver
|
||||
AP_SerialManager::SerialProtocol _protocol; // protocol used - detected using SerialManager's SERIAL#_PROTOCOL parameter
|
||||
bool _initialised_uart;
|
||||
uint16_t _crc;
|
||||
|
||||
uint8_t *_control_mode;
|
||||
struct
|
||||
{
|
||||
uint8_t mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h)
|
||||
AP_Float *fs_batt_voltage; // failsafe battery voltage in volts
|
||||
AP_Float *fs_batt_mah; // failsafe reserve capacity in mAh
|
||||
} _params;
|
||||
|
||||
struct
|
||||
{
|
||||
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;
|
||||
uint16_t alt_nav_meters;
|
||||
uint16_t alt_nav_cm;
|
||||
int16_t speed_in_meter;
|
||||
uint16_t speed_in_centimeter;
|
||||
uint8_t *control_mode;
|
||||
uint32_t *value;
|
||||
uint32_t *control_sensors_present;
|
||||
uint32_t *control_sensors_enabled;
|
||||
uint32_t *control_sensors_health;
|
||||
int32_t *home_distance;
|
||||
int32_t *home_bearing;
|
||||
} _ap;
|
||||
|
||||
float _relative_home_altitude; // altitude in centimeters above home
|
||||
uint32_t _control_sensors_timer;
|
||||
uint8_t _paramID;
|
||||
|
||||
struct
|
||||
{
|
||||
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;
|
||||
uint16_t alt_nav_meters;
|
||||
uint16_t alt_nav_cm;
|
||||
int16_t speed_in_meter;
|
||||
uint16_t speed_in_centimeter;
|
||||
} _gps;
|
||||
|
||||
struct
|
||||
{
|
||||
uint8_t new_byte;
|
||||
bool send_attiandrng;
|
||||
bool send_latitude;
|
||||
uint32_t timer_params;
|
||||
uint32_t timer_ap_status;
|
||||
uint32_t timer_batt;
|
||||
uint32_t timer_gps_status;
|
||||
uint32_t timer_home;
|
||||
uint32_t timer_velandyaw;
|
||||
uint32_t timer_gps_latlng;
|
||||
uint32_t timer_vario;
|
||||
uint32_t timer_alt;
|
||||
uint32_t timer_vfas;
|
||||
} _passthrough;
|
||||
|
||||
struct
|
||||
{
|
||||
bool sport_status;
|
||||
uint8_t fas_call;
|
||||
uint8_t gps_call;
|
||||
uint8_t vario_call;
|
||||
uint8_t various_call;
|
||||
} _SPort;
|
||||
|
||||
struct
|
||||
{
|
||||
uint32_t last_200ms_frame;
|
||||
uint32_t last_1000ms_frame;
|
||||
} _D;
|
||||
|
||||
struct
|
||||
{
|
||||
uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the mavlink message to be sent
|
||||
uint8_t repeats; // send each message "chunk" 3 times to make sure the entire messsage gets through without getting cut
|
||||
uint8_t char_index; // index of which character to get in the message
|
||||
} _msg_chunk;
|
||||
|
||||
// main transmission function when protocol is FrSky SPort Passthrough (OpenTX)
|
||||
void send_SPort_Passthrough(void);
|
||||
// main transmission function when protocol is FrSky SPort
|
||||
void send_SPort(void);
|
||||
// main transmission function when protocol is FrSky D
|
||||
|
|
@ -103,7 +229,21 @@ private:
|
|||
void calc_crc(uint8_t byte);
|
||||
void send_crc(void);
|
||||
void send_byte(uint8_t value);
|
||||
void send_data(uint16_t id, uint32_t data);
|
||||
void send_uint32(uint16_t id, uint32_t data);
|
||||
void send_uint16(uint16_t id, uint16_t data);
|
||||
|
||||
// methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format
|
||||
uint32_t get_next_msg_chunk(void);
|
||||
void control_sensors_check(void);
|
||||
uint32_t calc_param(void);
|
||||
uint32_t calc_gps_latlng(bool *send_latitude);
|
||||
uint32_t calc_gps_status(void);
|
||||
uint32_t calc_batt(void);
|
||||
uint32_t calc_ap_status(void);
|
||||
uint32_t calc_home(void);
|
||||
uint32_t calc_velandyaw(void);
|
||||
uint32_t calc_attiandrng(void);
|
||||
uint16_t prep_number(int32_t number, uint8_t digits, uint8_t power);
|
||||
|
||||
// methods to convert flight controller data to FrSky D or SPort format
|
||||
void calc_nav_alt(void);
|
||||
|
|
|
|||
Loading…
Reference in New Issue