aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Frsky_Telem: added telemetry support for RPM sensors 1 and 2 for SPort, FPort/FPort2 and passthrough over crossfire

Browse Source
This commit is contained in:
yaapu 2021-01-29 09:50:59 +01:00 committed by Peter Barker
parent cfe63f8657
commit d5868e3c7d
6 changed files with 85 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
View File

@ -3,6 +3,7 @@
#include <AP_Baro/AP_Baro.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_RPM/AP_RPM.h>
extern const AP_HAL::HAL& hal;
@ -140,3 +141,21 @@ void AP_Frsky_Backend::calc_gps_position(void)
_SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS
}
/*
* prepare rpm data
* for FrSky D and SPort protocols
*/
bool AP_Frsky_Backend::calc_rpm(const uint8_t instance, int32_t &value) const
{
const AP_RPM* rpm = AP::rpm();
if (rpm == nullptr) {
return false;
}
float rpm_value;
if (!rpm->get_rpm(instance, rpm_value)) {
return false;
}
value = static_cast<int32_t>(roundf(rpm_value));
return true;
}

3
libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
View File

@ -58,6 +58,7 @@ protected:
void calc_nav_alt(void);
void calc_gps_position(void);
bool calc_rpm(const uint8_t instance, int32_t &value) const;
float get_vspeed_ms(void);
@ -108,6 +109,7 @@ protected:
static const uint8_t BYTESTUFF_D = 0x5D;
// FrSky data IDs;
static const uint16_t RPM_LAST_ID = 0x050F;
static const uint16_t GPS_LONG_LATI_FIRST_ID = 0x0800;
static const uint16_t DIY_FIRST_ID = 0x5000;
@ -121,6 +123,7 @@ protected:
static const uint8_t SENSOR_ID_VARIO = 0x00; // Sensor ID 0
static const uint8_t SENSOR_ID_FAS = 0x22; // Sensor ID 2
static const uint8_t SENSOR_ID_GPS = 0x83; // Sensor ID 3
static const uint8_t SENSOR_ID_RPM = 0xE4; // Sensor ID 4
static const uint8_t SENSOR_ID_SP2UR = 0xC6; // Sensor ID 6
static const uint8_t SENSOR_ID_27 = 0x1B; // Sensor ID 27

19
libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp
View File

@ -4,11 +4,14 @@
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_RPM/AP_RPM.h>
#include "AP_Frsky_SPortParser.h"
#include <string.h>
extern const AP_HAL::HAL& hal;
AP_Frsky_SPort *AP_Frsky_SPort::singleton;
/*
@ -114,6 +117,22 @@ void AP_Frsky_SPort::send(void)
_SPort.gps_call = 0;
}
break;
case SENSOR_ID_RPM: // Sensor ID 4
{
const AP_RPM* rpm = AP::rpm();
if (rpm == nullptr) {
break;
}
int32_t value;
if (calc_rpm(_SPort.rpm_call, value)) {
// use high numbered frsky sensor ids to leave low numbered free for externally attached physical frsky sensors
send_sport_frame(SPORT_DATA_FRAME, 1+RPM_LAST_ID-(rpm->num_sensors()-_SPort.rpm_call), value);
}
if (++_SPort.rpm_call > (rpm->num_sensors()-1)) {
_SPort.rpm_call = 0;
}
}
break;
case SENSOR_ID_SP2UR: // Sensor ID 6
switch (_SPort.various_call) {
case 0 :

1
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

@ -54,6 +54,7 @@ private:
uint8_t gps_call;
uint8_t vario_call;
uint8_t various_call;
uint8_t rpm_call;
} _SPort;
static AP_Frsky_SPort *singleton;

39
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -7,6 +7,7 @@
#include <AP_InertialSensor/AP_InertialSensor.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RPM/AP_RPM.h>
#include <GCS_MAVLink/GCS.h>
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
@ -110,6 +111,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void)
set_scheduler_entry(BATT_2, 1300, 500); // 0x5008 Battery 2 status
set_scheduler_entry(BATT_1, 1300, 500); // 0x5003 Battery 1 status
set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters
set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2
set_scheduler_entry(UDATA, 5000, 200); // user data
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
set_scheduler_entry(MAV, 35, 25); // mavlite
@ -188,6 +190,16 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
case BATT_2:
packet_ready = AP::battery().num_instances() > 1;
break;
case RPM:
{
packet_ready = false;
const AP_RPM *rpm = AP::rpm();
if (rpm == nullptr) {
break;
}
packet_ready = rpm->num_sensors() > 0;
}
break;
case UDATA:
// when using fport user data is sent by scheduler
// when using sport user data is sent responding to custom polling
@ -254,6 +266,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
case PARAM: // 0x5007 parameters
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
case RPM: // 0x500A rpm sensors 1 and 2
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0A, calc_rpm());
break;
case UDATA: // user data
{
WITH_SEMAPHORE(_sport_push_buffer.sem);
@ -577,6 +592,30 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_attiandrng(void)
return attiandrng;
}
/*
* prepare rpm for sensors 1 and 2
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
*/
uint32_t AP_Frsky_SPort_Passthrough::calc_rpm(void)
{
const AP_RPM *ap_rpm = AP::rpm();
if (ap_rpm == nullptr) {
return 0;
}
uint32_t value = 0;
// we send: rpm_value*0.1 as 16 bits signed
float rpm;
// bits 0-15 for rpm 0
if (ap_rpm->get_rpm(0,rpm)) {
value |= (int16_t)roundf(rpm * 0.1);
}
// bits 16-31 for rpm 1
if (ap_rpm->get_rpm(1,rpm)) {
value |= (int16_t)roundf(rpm * 0.1) << 16;
}
return value;
}
/*
fetch Sport data for an external transport, such as FPort or crossfire
Note: we need to create a packet array with unique packet types

6
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -67,9 +67,10 @@ public:
BATT_2 = 8, // 0x5008 Battery 2 status
BATT_1 = 9, // 0x5008 Battery 1 status
PARAM = 10, // 0x5007 parameters
UDATA = 11, // user data
RPM = 11, // 0x500A rpm sensors 1 and 2
UDATA = 12, // user data
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
MAV = 12, // mavlite
MAV = 13, // mavlite
#endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
WFQ_LAST_ITEM // must be last
};
@ -97,6 +98,7 @@ private:
uint32_t calc_home(void);
uint32_t calc_velandyaw(void);
uint32_t calc_attiandrng(void);
uint32_t calc_rpm(void);
// use_external_data is set when this library will
// be providing data to another transport, such as FPort