zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

AP_Frsky_Telem: move FRsky telemetry up into common GCS telemetry class

master
Peter Barker 6 years ago committed by Peter Barker
parent
ab1c42696c
commit
e5818308b9
2 changed files with 31 additions and 33 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 49
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
  2. 15
      libraries/AP_Frsky_Telem/AP_Frsky_Telem.h

49
libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp
Unescape View File

@ -37,8 +37,7 @@ ObjectArray<mavlink_statustext_t> AP_Frsky_Telem::_statustext_queue(FRSKY_TELEM_ @@ -37,8 +37,7 @@ ObjectArray<mavlink_statustext_t> AP_Frsky_Telem::_statustext_queue(FRSKY_TELEM_
/*
* init - perform required initialisation
*/
void AP_Frsky_Telem::init(const uint8_t mav_type,
const uint32_t *ap_valuep)
void AP_Frsky_Telem::init(const uint32_t *ap_valuep)
{
const AP_SerialManager &serial_manager = AP::serialmanager();
@ -50,7 +49,6 @@ void AP_Frsky_Telem::init(const uint8_t mav_type, @@ -50,7 +49,6 @@ void AP_Frsky_Telem::init(const uint8_t mav_type,
} 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)
// make frsky_telemetry available to GCS_MAVLINK (used to queue statustext messages from GCS_MAVLINK)
gcs().register_frsky_telemetry_callback(this);
// add firmware and frame info to message queue
if (_frame_string == nullptr) {
queue_message(MAV_SEVERITY_INFO, AP::fwversion().fw_string);
@ -60,7 +58,6 @@ void AP_Frsky_Telem::init(const uint8_t mav_type, @@ -60,7 +58,6 @@ void AP_Frsky_Telem::init(const uint8_t mav_type,
queue_message(MAV_SEVERITY_INFO, firmware_buf);
}
// save main parameters locally
_params.mav_type = mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h)
if (ap_valuep == nullptr) { // ap bit-field
_ap.value = 0x2000; // set "initialised" to 1 for rover and plane
_ap.valuep = &_ap.value;
@ -271,7 +268,7 @@ void AP_Frsky_Telem::send_SPort(void) @@ -271,7 +268,7 @@ void AP_Frsky_Telem::send_SPort(void)
send_uint32(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
break;
case 1:
send_uint32(DATA_ID_TEMP1, _ap.control_mode); // send flight mode
send_uint32(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
break;
}
if (_SPort.various_call++ > 1) _SPort.various_call = 0;
@ -298,7 +295,7 @@ void AP_Frsky_Telem::send_D(void) @@ -298,7 +295,7 @@ void AP_Frsky_Telem::send_D(void)
if (now - _D.last_200ms_frame >= 200) {
_D.last_200ms_frame = now;
send_uint16(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::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, _ap.control_mode); // send flight mode
send_uint16(DATA_ID_TEMP1, gcs().custom_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
@ -499,42 +496,44 @@ void AP_Frsky_Telem::check_sensor_status_flags(void) @@ -499,42 +496,44 @@ void AP_Frsky_Telem::check_sensor_status_flags(void)
{
uint32_t now = AP_HAL::millis();
const uint32_t _sensor_status_flags = sensor_status_flags();
if ((now - check_sensor_status_timer) >= 5000) { // prevent repeating any system_status messages unless 5 seconds have passed
// only one error is reported at a time (in order of preference). Same setup and displayed messages as Mission Planner.
if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_GPS) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad GPS Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_GYRO) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Gyro Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_ACCEL) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Accel Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_3D_MAG) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Compass Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Baro Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_LASER_POSITION) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad LiDAR Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad OptFlow Health");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_TERRAIN) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad or No Terrain Data");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_GEOFENCE) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Geofence Breach");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_AHRS) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad AHRS");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_SENSOR_RC_RECEIVER) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "No RC Receiver");
check_sensor_status_timer = now;
} else if ((_ap.sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) {
} else if ((_sensor_status_flags & MAV_SYS_STATUS_LOGGING) > 0) {
queue_message(MAV_SEVERITY_CRITICAL, "Bad Logging");
check_sensor_status_timer = now;
}
@ -598,7 +597,7 @@ uint32_t AP_Frsky_Telem::calc_param(void) @@ -598,7 +597,7 @@ uint32_t AP_Frsky_Telem::calc_param(void)
_paramID++;
switch(_paramID) {
case 1:
param = _params.mav_type; // 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;
case 2: // was used to send the battery failsafe voltage
case 3: // was used to send the battery failsafe capacity in mAh
@ -699,7 +698,7 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void) @@ -699,7 +698,7 @@ uint32_t AP_Frsky_Telem::calc_ap_status(void)
uint8_t imu_temp = (uint8_t) roundf(constrain_float(AP::ins().get_temperature(0), AP_IMU_TEMP_MIN, AP_IMU_TEMP_MAX) - AP_IMU_TEMP_MIN);
// control/flight mode number (limit to 31 (0x1F) since the value is stored on 5 bits)
ap_status = (uint8_t)((_ap.control_mode+1) & AP_CONTROL_MODE_LIMIT);
ap_status = (uint8_t)((gcs().custom_mode()+1) & AP_CONTROL_MODE_LIMIT);
// simple/super simple modes flags
ap_status |= (uint8_t)((*_ap.valuep) & AP_SSIMPLE_FLAGS)<<AP_SSIMPLE_OFFSET;
// is_flying flag
@ -935,3 +934,13 @@ void AP_Frsky_Telem::calc_gps_position(void) @@ -935,3 +934,13 @@ void AP_Frsky_Telem::calc_gps_position(void)
_gps.speed_in_centimeter = 0;
}
}
uint32_t AP_Frsky_Telem::sensor_status_flags() const
{
uint32_t present;
uint32_t enabled;
uint32_t health;
gcs().get_sensor_status_flags(present, enabled, health);
return ~health & enabled & present;
}

15
libraries/AP_Frsky_Telem/AP_Frsky_Telem.h
Unescape View File

@ -120,15 +120,11 @@ public: @@ -120,15 +120,11 @@ public:
AP_Frsky_Telem &operator=(const AP_Frsky_Telem&) = delete;
// init - perform required initialisation
void init(const uint8_t mav_type,
const uint32_t *ap_valuep = nullptr);
void init(const uint32_t *ap_valuep = nullptr);
// add statustext message to FrSky lib message queue
void queue_message(MAV_SEVERITY severity, const char *text);
// update flight control mode. The control mode is vehicle type specific
void update_control_mode(uint8_t mode) { _ap.control_mode = mode; }
// update whether we're flying (used for Plane)
// set land_complete flag to 0 if is_flying
// set land_complete flag to 1 if not flying
@ -138,7 +134,7 @@ public: @@ -138,7 +134,7 @@ public:
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
// indicates that the sensor or subsystem is present but not
// functioning correctly
void update_sensor_status_flags(uint32_t error_mask) { _ap.sensor_status_flags = error_mask; }
uint32_t sensor_status_flags() const;
static ObjectArray<mavlink_statustext_t> _statustext_queue;
@ -154,15 +150,8 @@ private: @@ -154,15 +150,8 @@ private:
struct
{
uint8_t mav_type; // frame type (see MAV_TYPE in Mavlink definition file common.h)
} _params;
struct
{
uint8_t control_mode;
uint32_t value;
const uint32_t *valuep;
uint32_t sensor_status_flags;
} _ap;
uint32_t check_sensor_status_timer;

Write Preview
Loading…
Cancel
Save