Copter: Moving to RSSI library for reading various kinds of RSSI, with the possibility of adding more.
* Retains ability to read from Analog Pin * Adds ability to read RSSI from PWM channel value as is done in OpenLRSng, EazyUHF, and various other LRS. * Handles any type of RSSI that provides RSSI values inverted - i.e. when the low value is the best signal and the high value is the worst signal. * Has different key names from all existing RSSI parameters to provide for a clean break and easier distinguishing. * Existing parameters are marked as obsolete
This commit is contained in:
parent
a315b980bf
commit
7cb494d8e2
@ -102,7 +102,6 @@ Copter::Copter(void) :
|
|||||||
#if CAMERA == ENABLED
|
#if CAMERA == ENABLED
|
||||||
camera(&relay),
|
camera(&relay),
|
||||||
#endif
|
#endif
|
||||||
rssi_analog_source(NULL),
|
|
||||||
#if MOUNT == ENABLED
|
#if MOUNT == ENABLED
|
||||||
camera_mount(ahrs, current_loc),
|
camera_mount(ahrs, current_loc),
|
||||||
#endif
|
#endif
|
||||||
|
@ -75,6 +75,7 @@
|
|||||||
#include <AP_Motors/AP_Motors.h> // AP Motors library
|
#include <AP_Motors/AP_Motors.h> // AP Motors library
|
||||||
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
|
#include <AP_RangeFinder/AP_RangeFinder.h> // Range finder library
|
||||||
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
||||||
|
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
|
||||||
#include <Filter/Filter.h> // Filter library
|
#include <Filter/Filter.h> // Filter library
|
||||||
#include <AP_Buffer/AP_Buffer.h> // APM FIFO Buffer
|
#include <AP_Buffer/AP_Buffer.h> // APM FIFO Buffer
|
||||||
#include <AP_Relay/AP_Relay.h> // APM relay
|
#include <AP_Relay/AP_Relay.h> // APM relay
|
||||||
@ -451,9 +452,6 @@ private:
|
|||||||
AP_Camera camera;
|
AP_Camera camera;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
// a pin for reading the receiver RSSI voltage.
|
|
||||||
AP_HAL::AnalogSource* rssi_analog_source;
|
|
||||||
|
|
||||||
// Camera/Antenna mount tracking and stabilisation stuff
|
// Camera/Antenna mount tracking and stabilisation stuff
|
||||||
#if MOUNT == ENABLED
|
#if MOUNT == ENABLED
|
||||||
// current_loc uses the baro/gps soloution for altitude rather than gps only.
|
// current_loc uses the baro/gps soloution for altitude rather than gps only.
|
||||||
@ -470,6 +468,9 @@ private:
|
|||||||
AP_Rally rally;
|
AP_Rally rally;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
// RSSI
|
||||||
|
AP_RSSI rssi;
|
||||||
|
|
||||||
// Crop Sprayer
|
// Crop Sprayer
|
||||||
#if SPRAYER == ENABLED
|
#if SPRAYER == ENABLED
|
||||||
AC_Sprayer sprayer;
|
AC_Sprayer sprayer;
|
||||||
|
@ -205,21 +205,6 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
|
|||||||
// @User: Standard
|
// @User: Standard
|
||||||
GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT),
|
GSCALAR(rtl_climb_min, "RTL_CLIMB_MIN", RTL_CLIMB_MIN_DEFAULT),
|
||||||
|
|
||||||
// @Param: RSSI_PIN
|
|
||||||
// @DisplayName: Receiver RSSI sensing pin
|
|
||||||
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is RSSI_RANGE for max rssi, 0V for minimum
|
|
||||||
// @Values: -1:Disabled, 0:APM2 A0, 1:APM2 A1, 2:APM2 A2, 13:APM2 A13, 103:Pixhawk SBUS
|
|
||||||
// @User: Standard
|
|
||||||
GSCALAR(rssi_pin, "RSSI_PIN", -1),
|
|
||||||
|
|
||||||
// @Param: RSSI_RANGE
|
|
||||||
// @DisplayName: Receiver RSSI voltage range
|
|
||||||
// @Description: Receiver RSSI voltage range
|
|
||||||
// @Units: Volt
|
|
||||||
// @Values: 3.3:3.3V, 5:5V
|
|
||||||
// @User: Standard
|
|
||||||
GSCALAR(rssi_range, "RSSI_RANGE", 5.0f),
|
|
||||||
|
|
||||||
// @Param: WP_YAW_BEHAVIOR
|
// @Param: WP_YAW_BEHAVIOR
|
||||||
// @DisplayName: Yaw behaviour during missions
|
// @DisplayName: Yaw behaviour during missions
|
||||||
// @Description: Determines how the autopilot controls the yaw during missions and RTL
|
// @Description: Determines how the autopilot controls the yaw during missions and RTL
|
||||||
@ -1033,6 +1018,10 @@ const AP_Param::Info Copter::var_info[] PROGMEM = {
|
|||||||
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
|
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
|
||||||
GOBJECT(mission, "MIS_", AP_Mission),
|
GOBJECT(mission, "MIS_", AP_Mission),
|
||||||
|
|
||||||
|
// @Group: RSSI_
|
||||||
|
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
|
||||||
|
GOBJECT(rssi, "RSSI_", AP_RSSI),
|
||||||
|
|
||||||
#if CONFIG_SONAR == ENABLED
|
#if CONFIG_SONAR == ENABLED
|
||||||
// @Group: RNGFND
|
// @Group: RNGFND
|
||||||
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
|
// @Path: ../libraries/AP_RangeFinder/RangeFinder.cpp
|
||||||
|
@ -90,7 +90,7 @@ public:
|
|||||||
// change
|
// change
|
||||||
k_param_toy_yaw_rate, // deprecated - remove
|
k_param_toy_yaw_rate, // deprecated - remove
|
||||||
k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change
|
k_param_crosstrack_min_distance, // deprecated - remove with next eeprom number change
|
||||||
k_param_rssi_pin,
|
k_param_rssi_pin, // unused, replaced by rssi_ library parameters
|
||||||
k_param_throttle_accel_enabled, // deprecated - remove
|
k_param_throttle_accel_enabled, // deprecated - remove
|
||||||
k_param_wp_yaw_behavior,
|
k_param_wp_yaw_behavior,
|
||||||
k_param_acro_trainer,
|
k_param_acro_trainer,
|
||||||
@ -105,7 +105,7 @@ public:
|
|||||||
k_param_battery,
|
k_param_battery,
|
||||||
k_param_fs_batt_mah,
|
k_param_fs_batt_mah,
|
||||||
k_param_angle_rate_max, // remove
|
k_param_angle_rate_max, // remove
|
||||||
k_param_rssi_range,
|
k_param_rssi_range, // unused, replaced by rssi_ library parameters
|
||||||
k_param_rc_feel_rp,
|
k_param_rc_feel_rp,
|
||||||
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group
|
k_param_NavEKF, // Extended Kalman Filter Inertial Navigation Group
|
||||||
k_param_mission, // mission library
|
k_param_mission, // mission library
|
||||||
@ -167,6 +167,9 @@ public:
|
|||||||
//
|
//
|
||||||
k_param_motors = 90,
|
k_param_motors = 90,
|
||||||
|
|
||||||
|
// 97: RSSI
|
||||||
|
k_param_rssi = 97,
|
||||||
|
|
||||||
//
|
//
|
||||||
// 100: Inertial Nav
|
// 100: Inertial Nav
|
||||||
//
|
//
|
||||||
@ -375,8 +378,6 @@ public:
|
|||||||
AP_Int16 rtl_alt_final;
|
AP_Int16 rtl_alt_final;
|
||||||
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
|
AP_Int16 rtl_climb_min; // rtl minimum climb in cm
|
||||||
|
|
||||||
AP_Int8 rssi_pin;
|
|
||||||
AP_Float rssi_range; // allows to set max voltage for rssi pin such as 5.0, 3.3 etc.
|
|
||||||
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
|
AP_Int8 wp_yaw_behavior; // controls how the autopilot controls yaw during missions
|
||||||
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
|
AP_Int8 rc_feel_rp; // controls vehicle response to user input with 0 being extremely soft and 100 begin extremely crisp
|
||||||
|
|
||||||
|
@ -38,6 +38,7 @@ LIBRARIES += RC_Channel
|
|||||||
LIBRARIES += AP_Motors
|
LIBRARIES += AP_Motors
|
||||||
LIBRARIES += AP_RangeFinder
|
LIBRARIES += AP_RangeFinder
|
||||||
LIBRARIES += AP_OpticalFlow
|
LIBRARIES += AP_OpticalFlow
|
||||||
|
LIBRARIES += AP_RSSI
|
||||||
LIBRARIES += Filter
|
LIBRARIES += Filter
|
||||||
LIBRARIES += AP_Buffer
|
LIBRARIES += AP_Buffer
|
||||||
LIBRARIES += AP_Relay
|
LIBRARIES += AP_Relay
|
||||||
|
@ -171,14 +171,7 @@ void Copter::read_battery(void)
|
|||||||
// RC_CHANNELS_SCALED message
|
// RC_CHANNELS_SCALED message
|
||||||
void Copter::read_receiver_rssi(void)
|
void Copter::read_receiver_rssi(void)
|
||||||
{
|
{
|
||||||
// avoid divide by zero
|
receiver_rssi = rssi.read_receiver_rssi_uint8();
|
||||||
if (g.rssi_range <= 0) {
|
|
||||||
receiver_rssi = 0;
|
|
||||||
}else{
|
|
||||||
rssi_analog_source->set_pin(g.rssi_pin);
|
|
||||||
float ret = rssi_analog_source->voltage_average() * 255 / g.rssi_range;
|
|
||||||
receiver_rssi = constrain_int16(ret, 0, 255);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
|
||||||
#if EPM_ENABLED == ENABLED
|
#if EPM_ENABLED == ENABLED
|
||||||
|
@ -122,9 +122,10 @@ void Copter::init_ardupilot()
|
|||||||
|
|
||||||
// initialise battery monitor
|
// initialise battery monitor
|
||||||
battery.init();
|
battery.init();
|
||||||
|
|
||||||
rssi_analog_source = hal.analogin->channel(g.rssi_pin);
|
|
||||||
|
|
||||||
|
// Init RSSI
|
||||||
|
rssi.init();
|
||||||
|
|
||||||
barometer.init();
|
barometer.init();
|
||||||
|
|
||||||
// Register the mavlink service callback. This will run
|
// Register the mavlink service callback. This will run
|
||||||
|
Loading…
Reference in New Issue
Block a user