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

3
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)
{
{// release semaphore as soon as possible
{
// release semaphore as soon as possible
AP_AHRS &_ahrs = AP::ahrs();
Vector3f v;
WITH_SEMAPHORE(_ahrs.get_semaphore());

14
libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
View File

@ -3,7 +3,8 @@
#include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
class AP_Frsky_Backend {
class AP_Frsky_Backend
{
public:
AP_Frsky_Backend(AP_HAL::UARTDriver *port) :
@ -15,10 +16,14 @@ public:
virtual void send() = 0;
// 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
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;
}
@ -38,8 +43,7 @@ protected:
// methods to convert flight controller data to FrSky D or SPort format
float format_gps(float dec);
struct
{
struct {
int32_t vario_vspd;
char lat_ns, lon_ew;
uint16_t latdddmm;

8
libraries/AP_Frsky_Telem/AP_Frsky_D.h
View File

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

9
libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
View File

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

108
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -119,18 +119,18 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty)
{
bool packet_ready = false;
switch (idx) {
case TEXT:
packet_ready = !queue_empty;
break;
case AP_STATUS:
packet_ready = gcs().vehicle_initialised();
break;
case BATT_2:
packet_ready = AP::battery().num_instances() > 1;
break;
default:
packet_ready = true;
break;
case TEXT:
packet_ready = !queue_empty;
break;
case AP_STATUS:
packet_ready = gcs().vehicle_initialised();
break;
case BATT_2:
packet_ready = AP::battery().num_instances() > 1;
break;
default:
packet_ready = true;
break;
}
return packet_ready;
@ -144,47 +144,47 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx)
{
// send packet
switch (idx) {
case TEXT: // 0x5000 status text
if (get_next_msg_chunk()) {
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
}
break;
case ATTITUDE: // 0x5006 Attitude and range
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break;
case GPS_LAT: // 0x800 GPS lat
// 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
_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
// this guarantees that gps coords are sent at max
// _scheduler.avg_polling_period*number_of_downlink_sensors time separation
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
break;
case GPS_LON: // 0x800 GPS lon
send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
break;
case VEL_YAW: // 0x5005 Vel and Yaw
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break;
case AP_STATUS: // 0x5001 AP status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break;
case GPS_STATUS: // 0x5002 GPS Status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break;
case HOME: // 0x5004 Home
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break;
case BATT_2: // 0x5008 Battery 2 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break;
case BATT_1: // 0x5003 Battery 1 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break;
case PARAM: // 0x5007 parameters
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
case TEXT: // 0x5000 status text
if (get_next_msg_chunk()) {
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID, _msg_chunk.chunk);
}
break;
case ATTITUDE: // 0x5006 Attitude and range
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+6, calc_attiandrng());
break;
case GPS_LAT: // 0x800 GPS lat
// 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
_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
// this guarantees that gps coords are sent at max
// _scheduler.avg_polling_period*number_of_downlink_sensors time separation
_scheduler.packet_timer[GPS_LON] = _scheduler.packet_timer[GPS_LAT] - 10000;
break;
case GPS_LON: // 0x800 GPS lon
send_sport_frame(SPORT_DATA_FRAME, GPS_LONG_LATI_FIRST_ID, _passthrough.gps_lng_sample); // gps longitude
break;
case VEL_YAW: // 0x5005 Vel and Yaw
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+5, calc_velandyaw());
break;
case AP_STATUS: // 0x5001 AP status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+1, calc_ap_status());
break;
case GPS_STATUS: // 0x5002 GPS Status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+2, calc_gps_status());
break;
case HOME: // 0x5004 Home
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+4, calc_home());
break;
case BATT_2: // 0x5008 Battery 2 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+8, calc_batt(1));
break;
case BATT_1: // 0x5003 Battery 1 status
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+3, calc_batt(0));
break;
case PARAM: // 0x5007 parameters
send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+7, calc_param());
break;
}
}
@ -297,7 +297,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_param(void)
}
_paramID++;
switch(_paramID) {
switch (_paramID) {
case FRAME_TYPE:
param = gcs().frame_type(); // see MAV_TYPE in Mavlink definition file common.h
break;

13
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -6,14 +6,15 @@
// for fair scheduler
#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:
AP_Frsky_SPort_Passthrough(AP_HAL::UARTDriver *port, bool use_external_data) :
AP_Frsky_SPort(port),
AP_RCTelemetry(TIME_SLOT_MAX),
_use_external_data(use_external_data)
{ }
{ }
bool init() 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;
void queue_text_message(MAV_SEVERITY severity, const char *text) override {
AP_RCTelemetry::queue_message(severity, text);
void queue_text_message(MAV_SEVERITY severity, const char *text) override
{
AP_RCTelemetry::queue_message(severity, text);
}
@ -80,8 +82,7 @@ private:
bool pending;
} external_data;
struct
{
struct {
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 char_index; // index of which character to get in the message