Copter: FrSky support has moved to GCS

This commit is contained in:
Peter Barker 2019-03-01 17:08:33 +11:00 committed by Peter Barker
parent 680008ba4f
commit e22b29bce4
7 changed files with 12 additions and 27 deletions

View File

@ -109,9 +109,6 @@
# include <AC_PrecLand/AC_PrecLand.h>
# include <AP_IRLock/AP_IRLock.h>
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
# include <AP_Frsky_Telem/AP_Frsky_Telem.h>
#endif
#if ADSB_ENABLED == ENABLED
# include <AP_ADSB/AP_ADSB.h>
#endif
@ -404,10 +401,6 @@ private:
FUNCTOR_BIND_MEMBER(&Copter::handle_battery_failsafe, void, const char*, const int8_t),
_failsafe_priorities};
#if FRSKY_TELEM_ENABLED == ENABLED
// FrSky telemetry support
AP_Frsky_Telem frsky_telemetry;
#endif
#if DEVO_TELEM_ENABLED == ENABLED
AP_DEVO_Telem devo_telemetry;
#endif

View File

@ -2,6 +2,11 @@
#include "Copter.h"
const char* GCS_Copter::frame_string() const
{
return copter.get_frame_string();
}
// update error mask of sensors and subsystems. The mask
// uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
// then it indicates that the sensor or subsystem is present but
@ -220,9 +225,4 @@ void GCS_Copter::update_sensor_status_flags(void)
control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
}
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
// give mask of error flags to Frsky_Telemetry
copter.frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
#endif
}

View File

@ -22,6 +22,11 @@ public:
void update_sensor_status_flags(void) override;
uint32_t custom_mode() const override;
MAV_TYPE frame_type() const override;
const char* frame_string() const override;
private:
GCS_MAVLINK_Copter _chan[MAVLINK_COMM_NUM_BUFFERS];

View File

@ -18,7 +18,7 @@ void Copter::gcs_send_heartbeat(void)
* pattern below when adding any new messages
*/
MAV_TYPE GCS_MAVLINK_Copter::frame_type() const
MAV_TYPE GCS_Copter::frame_type() const
{
return copter.get_frame_mav_type();
}
@ -73,12 +73,11 @@ MAV_MODE GCS_MAVLINK_Copter::base_mode() const
return (MAV_MODE)_base_mode;
}
uint32_t GCS_MAVLINK_Copter::custom_mode() const
uint32_t GCS_Copter::custom_mode() const
{
return copter.control_mode;
}
MAV_STATE GCS_MAVLINK_Copter::system_status() const
{
// set system as critical if any failsafe have triggered

View File

@ -59,9 +59,7 @@ private:
void packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg) override;
MAV_TYPE frame_type() const override;
MAV_MODE base_mode() const override;
uint32_t custom_mode() const override;
MAV_STATE system_status() const override;
int16_t vfr_hud_throttle() const override;

View File

@ -234,9 +234,6 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
fence.manual_recovery_start();
#endif
#if FRSKY_TELEM_ENABLED == ENABLED
frsky_telemetry.update_control_mode(control_mode);
#endif
#if DEVO_TELEM_ENABLED == ENABLED
devo_telemetry.update_control_mode(control_mode);
#endif

View File

@ -87,13 +87,6 @@ void Copter::init_ardupilot()
// setup telem slots with serial ports
gcs().setup_uarts(serial_manager);
#if FRSKY_TELEM_ENABLED == ENABLED
// setup frsky, and pass a number of parameters to the library
frsky_telemetry.init(get_frame_mav_type(),
&ap.value);
frsky_telemetry.set_frame_string(get_frame_string());
#endif
#if DEVO_TELEM_ENABLED == ENABLED
// setup devo
devo_telemetry.init();