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

AP_Frsky_Telem: reformat new filed using astyle.sh; no history to lose

Browse Source
This commit is contained in:
Peter Barker 2020-09-29 07:53:17 +10:00 committed by Peter Barker
parent 9c211d9847
commit 6cbb553adc
6 changed files with 95 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp
View File

@ -49,7 +49,8 @@ void AP_Frsky_Backend::loop(void)
float AP_Frsky_Backend::get_vspeed_ms(void) float AP_Frsky_Backend::get_vspeed_ms(void)
{ {
{// release semaphore as soon as possible {
// release semaphore as soon as possible
AP_AHRS &_ahrs = AP::ahrs(); AP_AHRS &_ahrs = AP::ahrs();
Vector3f v; Vector3f v;
WITH_SEMAPHORE(_ahrs.get_semaphore()); WITH_SEMAPHORE(_ahrs.get_semaphore());
@ -70,10 +71,10 @@ float AP_Frsky_Backend::get_vspeed_ms(void)
void AP_Frsky_Backend::calc_nav_alt(void) void AP_Frsky_Backend::calc_nav_alt(void)
{ {
_SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s
Location loc; Location loc;
float current_height = 0; // in centimeters above home float current_height = 0; // in centimeters above home
AP_AHRS &_ahrs = AP::ahrs(); AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore()); WITH_SEMAPHORE(_ahrs.get_semaphore());
if (_ahrs.get_position(loc)) { if (_ahrs.get_position(loc)) {
@ -86,7 +87,7 @@ void AP_Frsky_Backend::calc_nav_alt(void)
_SPort_data.alt_nav_meters = (int16_t)current_height; _SPort_data.alt_nav_meters = (int16_t)current_height;
_SPort_data.alt_nav_cm = (current_height - _SPort_data.alt_nav_meters) * 100; _SPort_data.alt_nav_cm = (current_height - _SPort_data.alt_nav_meters) * 100;
} }
/* /*
* format the decimal latitude/longitude to the required degrees/minutes * format the decimal latitude/longitude to the required degrees/minutes
@ -141,6 +142,6 @@ void AP_Frsky_Backend::calc_gps_position(void)
} }
AP_AHRS &_ahrs = AP::ahrs(); AP_AHRS &_ahrs = AP::ahrs();
_SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS _SPort_data.yaw = (uint16_t)((_ahrs.yaw_sensor / 100) % 360); // heading in degree based on AHRS and not GPS
} }

14
libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
View File

@ -3,7 +3,8 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
class AP_Frsky_Backend { class AP_Frsky_Backend
{
public: public:
AP_Frsky_Backend(AP_HAL::UARTDriver *port) : AP_Frsky_Backend(AP_HAL::UARTDriver *port) :
@ -15,10 +16,14 @@ public:
virtual void send() = 0; virtual void send() = 0;
// SPort is at 57600, D overrides this // SPort is at 57600, D overrides this
virtual uint32_t initial_baud() const { return 57600; } virtual uint32_t initial_baud() const
{
return 57600;
}
// get next telemetry data for external consumers of SPort data // get next telemetry data for external consumers of SPort data
virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) { virtual bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data)
{
return false; return false;
} }
@ -38,8 +43,7 @@ protected:
// methods to convert flight controller data to FrSky D or SPort format // methods to convert flight controller data to FrSky D or SPort format
float format_gps(float dec); float format_gps(float dec);
struct struct {
{
int32_t vario_vspd; int32_t vario_vspd;
char lat_ns, lon_ew; char lat_ns, lon_ew;
uint16_t latdddmm; uint16_t latdddmm;

8
libraries/AP_Frsky_Telem/AP_Frsky_D.h
View File

@ -2,7 +2,8 @@
#include "AP_Frsky_Backend.h" #include "AP_Frsky_Backend.h"
class AP_Frsky_D : public AP_Frsky_Backend { class AP_Frsky_D : public AP_Frsky_Backend
{
public: public:
@ -11,7 +12,10 @@ public:
protected: protected:
void send() override; void send() override;
uint32_t initial_baud() const override { return 9600; } uint32_t initial_baud() const override
{
return 9600;
}
private: private:

9
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

@ -2,7 +2,8 @@
#include "AP_Frsky_Backend.h" #include "AP_Frsky_Backend.h"
class AP_Frsky_SPort : public AP_Frsky_Backend { class AP_Frsky_SPort : public AP_Frsky_Backend
{
public: public:
@ -14,8 +15,7 @@ protected:
void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data); void send_sport_frame(uint8_t frame, uint16_t appid, uint32_t data);
struct PACKED struct PACKED {
{
bool send_latitude; // sizeof(bool) = 4 ? bool send_latitude; // sizeof(bool) = 4 ?
uint32_t gps_lng_sample; uint32_t gps_lng_sample;
uint8_t new_byte; uint8_t new_byte;
@ -25,8 +25,7 @@ protected:
private: private:
struct struct {
{
bool sport_status; bool sport_status;
bool gps_refresh; bool gps_refresh;
bool vario_refresh; bool vario_refresh;

126
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -9,7 +9,7 @@
#include <AP_RangeFinder/AP_RangeFinder.h> #include <AP_RangeFinder/AP_RangeFinder.h>
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
/* /*
for FrSky SPort Passthrough for FrSky SPort Passthrough
*/ */
// data bits preparation // data bits preparation
@ -119,18 +119,18 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
{ {
bool packet_ready = false; bool packet_ready = false;
switch (idx) { switch (idx) {
case TEXT: case TEXT:
packet_ready = !queue_empty; packet_ready = !queue_empty;
break; break;
case AP_STATUS: case AP_STATUS:
packet_ready = gcs().vehicle_initialised(); packet_ready = gcs().vehicle_initialised();
break; break;
case BATT_2: case BATT_2:
packet_ready = AP::battery().num_instances() > 1; packet_ready = AP::battery().num_instances() > 1;
break; break;
default: default:
packet_ready = true; packet_ready = true;
break; break;
} }
return packet_ready; return packet_ready;
@ -144,47 +144,47 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
{ {
// send packet // send packet
switch (idx) { switch (idx) {
case TEXT: // 0x5000 status text case TEXT: // 0x5000 status text
if (get_next_msg_chunk()) { if (get_next_msg_chunk()) {
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
} }
break; break;
case ATTITUDE: // 0x5006 Attitude and range case ATTITUDE: // 0x5006 Attitude and range
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break; break;
case GPS_LAT: // 0x800 GPS lat case GPS_LAT: // 0x800 GPS lat
// sample both lat and lon at the same time // sample both lat and lon at the same time
send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, calc_gps_latlng(&_passthrough.send_latitude)); // gps latitude or longitude
_passthrough.gps_lng_sample = calc_gps_latlng(&_passthrough.send_latitude); _passthrough.gps_lng_sample = calc_gps_latlng(&_passthrough.send_latitude);
// force the scheduler to select GPS lon as packet that's been waiting the most // force the scheduler to select GPS lon as packet that's been waiting the most
// this guarantees that gps coords are sent at max // this guarantees that gps coords are sent at max
// _scheduler.avg_polling_period*number_of_downlink_sensors time separation // _scheduler.avg_polling_period*number_of_downlink_sensors time separation
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000; _scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
break; break;
case GPS_LON: // 0x800 GPS lon case GPS_LON: // 0x800 GPS lon
send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
break; break;
case VEL_YAW: // 0x5005 Vel and Yaw case VEL_YAW: // 0x5005 Vel and Yaw
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break; break;
case AP_STATUS: // 0x5001 AP status case AP_STATUS: // 0x5001 AP status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break; break;
case GPS_STATUS: // 0x5002 GPS Status case GPS_STATUS: // 0x5002 GPS Status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break; break;
case HOME: // 0x5004 Home case HOME: // 0x5004 Home
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break; break;
case BATT_2: // 0x5008 Battery 2 status case BATT_2: // 0x5008 Battery 2 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1)); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break; break;
case BATT_1: // 0x5003 Battery 1 status case BATT_1: // 0x5003 Battery 1 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0)); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break; break;
case PARAM: // 0x5007 parameters case PARAM: // 0x5007 parameters
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param()); send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break; break;
} }
} }
@ -269,7 +269,7 @@ bool AP_Frsky_SPort_Passthrough::get_next_msg_chunk(void)
// send messages twice // send messages twice
extra_chunks = 1; extra_chunks = 1;
} }
if (_msg_chunk.repeats++ > extra_chunks ) { if (_msg_chunk.repeats++ > extra_chunks ) {
_msg_chunk.repeats = 0; _msg_chunk.repeats = 0;
if (_msg_chunk.char_index == 0) { if (_msg_chunk.char_index == 0) {
@ -297,7 +297,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_param(void)
} }
_paramID++; _paramID++;
switch(_paramID) { switch (_paramID) {
case FRAME_TYPE: case FRAME_TYPE:
param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h
break; break;
@ -332,12 +332,12 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_gps_status(void)
// GPS receiver status (limit to 0-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_FLOAT or GPS_OK_FIX_3D_RTK_FIXED = 3) // GPS receiver status (limit to 0-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_FLOAT or GPS_OK_FIX_3D_RTK_FIXED = 3)
gps_status |= ((gps.status() < GPS_STATUS_LIMIT) ? gps.status() : GPS_STATUS_LIMIT)<<GPS_STATUS_OFFSET; gps_status |= ((gps.status() < GPS_STATUS_LIMIT) ? gps.status() : GPS_STATUS_LIMIT)<<GPS_STATUS_OFFSET;
// GPS horizontal dilution of precision in dm // GPS horizontal dilution of precision in dm
gps_status |= prep_number(roundf(gps.get_hdop() * 0.1f),2,1)<<GPS_HDOP_OFFSET; gps_status |= prep_number(roundf(gps.get_hdop() * 0.1f),2,1)<<GPS_HDOP_OFFSET;
// GPS receiver advanced status (0: no advanced fix, 1: GPS_OK_FIX_3D_DGPS, 2: GPS_OK_FIX_3D_RTK_FLOAT, 3: GPS_OK_FIX_3D_RTK_FIXED) // GPS receiver advanced status (0: no advanced fix, 1: GPS_OK_FIX_3D_DGPS, 2: GPS_OK_FIX_3D_RTK_FLOAT, 3: GPS_OK_FIX_3D_RTK_FIXED)
gps_status |= ((gps.status() > GPS_STATUS_LIMIT) ? gps.status()-GPS_STATUS_LIMIT : 0)<<GPS_ADVSTATUS_OFFSET; gps_status |= ((gps.status() > GPS_STATUS_LIMIT) ? gps.status()-GPS_STATUS_LIMIT : 0)<<GPS_ADVSTATUS_OFFSET;
// Altitude MSL in dm // Altitude MSL in dm
const Location &loc = gps.location(); const Location &loc = gps.location();
gps_status |= prep_number(roundf(loc.alt * 0.1f),2,2)<<GPS_ALTMSL_OFFSET; gps_status |= prep_number(roundf(loc.alt * 0.1f),2,2)<<GPS_ALTMSL_OFFSET;
return gps_status; return gps_status;
} }
@ -357,7 +357,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_batt(uint8_t instance)
if (!_battery.consumed_mah(consumed_mah, instance)) { if (!_battery.consumed_mah(consumed_mah, instance)) {
consumed_mah = 0; consumed_mah = 0;
} }
// battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V) // battery voltage in decivolts, can have up to a 12S battery (4.25Vx12S = 51.0V)
batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT); batt = (((uint16_t)roundf(_battery.voltage(instance) * 10.0f)) & BATT_VOLTAGE_LIMIT);
// battery current draw in deciamps // battery current draw in deciamps
@ -393,8 +393,8 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_ap_status(void)
ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET; ap_status |= (uint8_t)(AP_Notify::flags.ekf_bad)<<AP_EKF_FS_OFFSET;
// IMU temperature // IMU temperature
ap_status |= imu_temp << AP_IMU_TEMP_OFFSET; ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
//hal.console->printf("flying=%d\n",AP_Notify::flags.flying); //hal.console->printf("flying=%d\n",AP_Notify::flags.flying);
//hal.console->printf("ap_status=%08X\n",ap_status); //hal.console->printf("ap_status=%08X\n",ap_status);
return ap_status; return ap_status;
} }
@ -417,7 +417,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void)
home_loc = _ahrs.get_home(); home_loc = _ahrs.get_home();
} }
if (get_position) { if (get_position) {
// check home_loc is valid // check home_loc is valid
if (home_loc.lat != 0 || home_loc.lng != 0) { if (home_loc.lat != 0 || home_loc.lng != 0) {
// distance between vehicle and home_loc in meters // distance between vehicle and home_loc in meters
@ -450,7 +450,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
WITH_SEMAPHORE(_ahrs.get_semaphore()); WITH_SEMAPHORE(_ahrs.get_semaphore());
// horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed) // horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed)
const AP_Airspeed *aspeed = _ahrs.get_airspeed(); const AP_Airspeed *aspeed = _ahrs.get_airspeed();
if (aspeed && aspeed->enabled()) { if (aspeed && aspeed->enabled()) {
velandyaw |= prep_number(roundf(aspeed->get_airspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET; velandyaw |= prep_number(roundf(aspeed->get_airspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
} else { // otherwise send groundspeed estimate from ahrs } else { // otherwise send groundspeed estimate from ahrs
velandyaw |= prep_number(roundf(_ahrs.groundspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET; velandyaw |= prep_number(roundf(_ahrs.groundspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;

13
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -6,14 +6,15 @@
// for fair scheduler // for fair scheduler
#define TIME_SLOT_MAX 11 #define TIME_SLOT_MAX 11
class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry { class AP_Frsky_SPort_Passthrough : public AP_Frsky_SPort, public AP_RCTelemetry
{
public: public:
AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) : AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) :
AP_Frsky_SPort(port), AP_Frsky_SPort(port),
AP_RCTelemetry(TIME_SLOT_MAX), AP_RCTelemetry(TIME_SLOT_MAX),
_use_external_data(use_external_data) _use_external_data(use_external_data)
{ } { }
bool init() override; bool init() override;
bool init_serial_port() override; bool init_serial_port() override;
@ -29,8 +30,9 @@ public:
bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override; bool get_telem_data(uint8_t &frame, uint16_t &appid, uint32_t &data) override;
void queue_text_message(MAV_SEVERITY severity, const char *text) override { void queue_text_message(MAV_SEVERITY severity, const char *text) override
AP_RCTelemetry::queue_message(severity, text); {
AP_RCTelemetry::queue_message(severity, text);
} }
@ -80,8 +82,7 @@ private:
bool pending; bool pending;
} external_data; } external_data;
struct struct {
{
uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued message to be sent uint32_t chunk; // a "chunk" (four characters/bytes) at a time of the queued 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 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 uint8_t char_index; // index of which character to get in the message