Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_RCTelemetry: move CRSF link statistics definition to AP_RCProtocol

Browse Source 
correctly calculate attitude
fix GPS alt message  - expects alt in m + 1000,  0 - 5000 m - from vierfuffzig <tunella@gmx.de>
fix used battery capacity - from vierfuffzig <tunella@gmx.de>
This commit is contained in:
Andy Piper 2020-07-15 20:26:54 +01:00 committed by Peter Barker
parent ce970dcf75
commit 4535e749e1
2 changed files with 12 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -126,7 +126,7 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi
switch (frame_type) { switch (frame_type) {
// this means we are connected to an RC receiver and can send telemetry // this means we are connected to an RC receiver and can send telemetry
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED: case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
// the EVO sends battery frames and we should send telemetry back to populate the OS // the EVO sends battery frames and we should send telemetry back to populate the OSD
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR: case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR:
_enable_telemetry = true; _enable_telemetry = true;
break; break;
@ -346,7 +346,7 @@ void AP_CRSF_Telem::calc_battery()
_telem.bcast.battery.remaining = _battery.capacity_remaining_pct(0); _telem.bcast.battery.remaining = _battery.capacity_remaining_pct(0);
int32_t capacity = _battery.pack_capacity_mah(0); const int32_t capacity = used_mah;
_telem.bcast.battery.capacity[0] = (capacity & 0xFF0000) >> 16; _telem.bcast.battery.capacity[0] = (capacity & 0xFF0000) >> 16;
_telem.bcast.battery.capacity[1] = (capacity & 0xFF00) >> 8; _telem.bcast.battery.capacity[1] = (capacity & 0xFF00) >> 8;
_telem.bcast.battery.capacity[2] = (capacity & 0xFF); _telem.bcast.battery.capacity[2] = (capacity & 0xFF);
@ -365,7 +365,7 @@ void AP_CRSF_Telem::calc_gps()
_telem.bcast.gps.latitude = htobe32(loc.lat); _telem.bcast.gps.latitude = htobe32(loc.lat);
_telem.bcast.gps.longitude = htobe32(loc.lng); _telem.bcast.gps.longitude = htobe32(loc.lng);
_telem.bcast.gps.groundspeed = htobe16(roundf(AP::gps().ground_speed() * 100000 / 3600)); _telem.bcast.gps.groundspeed = htobe16(roundf(AP::gps().ground_speed() * 100000 / 3600));
_telem.bcast.gps.altitude = htobe16(loc.alt + 1000); _telem.bcast.gps.altitude = htobe16(constrain_int16(loc.alt / 100, 0, 5000) + 1000);
_telem.bcast.gps.gps_heading = htobe16(roundf(AP::gps().ground_course() * 100.0f)); _telem.bcast.gps.gps_heading = htobe16(roundf(AP::gps().ground_course() * 100.0f));
_telem.bcast.gps.satellites = AP::gps().num_sats(); _telem.bcast.gps.satellites = AP::gps().num_sats();
@ -381,9 +381,11 @@ void AP_CRSF_Telem::calc_attitude()
AP_AHRS &_ahrs = AP::ahrs(); AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore()); WITH_SEMAPHORE(_ahrs.get_semaphore());
_telem.bcast.attitude.roll_angle = htobe16(roundf(_ahrs.roll * 10000.0f)); const int16_t INT_PI = 31415;
_telem.bcast.attitude.pitch_angle = htobe16(roundf(_ahrs.pitch * 10000.0f)); // units are radians * 10000
_telem.bcast.attitude.yaw_angle = htobe16(roundf(_ahrs.yaw * 10000.0f)); _telem.bcast.attitude.roll_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.roll) * 10000.0f), -INT_PI, INT_PI));
_telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.pitch) * 10000.0f), -INT_PI, INT_PI));
_telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.yaw) * 10000.0f), -INT_PI, INT_PI));
_telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame); _telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame);
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE;
@ -465,4 +467,4 @@ namespace AP {
} }
}; };
#endif #endif

19
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -90,22 +90,10 @@ public:
uint8_t pitmode; // disable 0, enable 1 uint8_t pitmode; // disable 0, enable 1
} PACKED; } PACKED;
struct LinkStatisticsFrame {
uint8_t uplink_rssi_ant1; // ( dBm * -1 )
uint8_t uplink_rssi_ant2; // ( dBm * -1 )
uint8_t uplink_status; // Package success rate / Link quality ( % )
int8_t uplink_snr; // ( db )
uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
uint8_t downlink_rssi; // ( dBm * -1 )
uint8_t downlink_status; // Downlink package success rate / Link quality ( % ) ● int8_t Downlink SNR ( db )
} PACKED;
struct AttitudeFrame { struct AttitudeFrame {
int16_t pitch_angle; // ( rad / 10000 ) int16_t pitch_angle; // ( rad * 10000 )
int16_t roll_angle; // ( rad / 10000 ) int16_t roll_angle; // ( rad * 10000 )
int16_t yaw_angle; // ( rad / 10000 ) int16_t yaw_angle; // ( rad * 10000 )
} PACKED; } PACKED;
struct FlightModeFrame { struct FlightModeFrame {
@ -131,7 +119,6 @@ public:
HeartbeatFrame heartbeat; HeartbeatFrame heartbeat;
BatteryFrame battery; BatteryFrame battery;
VTXFrame vtx; VTXFrame vtx;
LinkStatisticsFrame link;
AttitudeFrame attitude; AttitudeFrame attitude;
FlightModeFrame flightmode; FlightModeFrame flightmode;
} PACKED; } PACKED;