mirror of https://github.com/ArduPilot/ardupilot
Plane: 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
7cb494d8e2
commit
722dd29370
|
@ -88,7 +88,7 @@ void Plane::setup()
|
|||
|
||||
notify.init(false);
|
||||
|
||||
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
|
||||
rssi.init();
|
||||
|
||||
init_ardupilot();
|
||||
|
||||
|
|
|
@ -912,21 +912,6 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
|
|||
GSCALAR(override_channel, "OVERRIDE_CHAN", 0),
|
||||
#endif
|
||||
|
||||
// @Param: RSSI_PIN
|
||||
// @DisplayName: Receiver RSSI sensing pin
|
||||
// @Description: This selects an analog pin for the receiver RSSI voltage. It assumes the voltage is 5V for max rssi, 0V for minimum
|
||||
// @Values: -1:Disabled, 0:APM2 A0, 1:APM2 A1, 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.0:5V
|
||||
// @User: Standard
|
||||
GSCALAR(rssi_range, "RSSI_RANGE", 5.0),
|
||||
|
||||
// @Param: INVERTEDFLT_CH
|
||||
// @DisplayName: Inverted flight channel
|
||||
// @Description: A RC input channel number to enable inverted flight. If this is non-zero then the APM will monitor the corresponding RC input channel and will enable inverted flight when the channel goes above 1750.
|
||||
|
@ -1204,6 +1189,10 @@ const AP_Param::Info Plane::var_info[] PROGMEM = {
|
|||
// @Path: ../libraries/AP_NavEKF/AP_NavEKF.cpp
|
||||
GOBJECTN(EKF, NavEKF, "EKF_", NavEKF),
|
||||
#endif
|
||||
|
||||
// @Group: RSSI_
|
||||
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
|
||||
GOBJECT(rssi, "RSSI_", AP_RSSI),
|
||||
|
||||
AP_VAREND
|
||||
};
|
||||
|
|
|
@ -101,7 +101,7 @@ public:
|
|||
k_param_sonar_old, // unused
|
||||
k_param_log_bitmask,
|
||||
k_param_BoardConfig,
|
||||
k_param_rssi_range,
|
||||
k_param_rssi_range, // unused, replaced by rssi_ library parameters
|
||||
k_param_flapin_channel,
|
||||
k_param_flaperon_output,
|
||||
k_param_gps,
|
||||
|
@ -141,6 +141,9 @@ public:
|
|||
k_param_gcs_pid_mask,
|
||||
k_param_crash_detection_enable,
|
||||
|
||||
// 97: RSSI
|
||||
k_param_rssi = 97,
|
||||
|
||||
// 100: Arming parameters
|
||||
k_param_arming = 100,
|
||||
|
||||
|
@ -222,9 +225,9 @@ public:
|
|||
// Battery monitoring parameters
|
||||
//
|
||||
k_param_battery = 166,
|
||||
k_param_rssi_pin,
|
||||
k_param_battery_volt_pin, // unused
|
||||
k_param_battery_curr_pin, // unused - 169
|
||||
k_param_rssi_pin, // unused, replaced by rssi_ library parameters - 167
|
||||
k_param_battery_volt_pin, // unused - 168
|
||||
k_param_battery_curr_pin, // unused - 169
|
||||
|
||||
//
|
||||
// 170: Radio settings
|
||||
|
@ -460,9 +463,7 @@ public:
|
|||
AP_Int8 flap_2_percent;
|
||||
AP_Int8 flap_2_speed;
|
||||
AP_Int8 land_flap_percent;
|
||||
AP_Int8 takeoff_flap_percent;
|
||||
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 takeoff_flap_percent;
|
||||
AP_Int8 inverted_flight_ch; // 0=disabled, 1-8 is channel for inverted flight trigger
|
||||
AP_Int8 stick_mixing;
|
||||
AP_Float takeoff_throttle_min_speed;
|
||||
|
|
|
@ -91,6 +91,7 @@
|
|||
#include <AP_Rally/AP_Rally.h>
|
||||
|
||||
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
|
||||
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
|
||||
|
||||
// Configuration
|
||||
#include "config.h"
|
||||
|
@ -248,10 +249,6 @@ private:
|
|||
// selected navigation controller
|
||||
AP_SpdHgtControl *SpdHgt_Controller = &TECS_controller;
|
||||
|
||||
// Analog Inputs
|
||||
// a pin for reading the receiver RSSI voltage.
|
||||
AP_HAL::AnalogSource *rssi_analog_source;
|
||||
|
||||
// Relay
|
||||
AP_Relay relay;
|
||||
|
||||
|
@ -270,6 +267,9 @@ private:
|
|||
|
||||
// Rally Ponints
|
||||
AP_Rally rally {ahrs};
|
||||
|
||||
// RSSI
|
||||
AP_RSSI rssi;
|
||||
|
||||
// remember if USB is connected, so we can adjust baud rate
|
||||
bool usb_connected;
|
||||
|
|
|
@ -47,6 +47,7 @@ LIBRARIES += AP_Frsky_Telem
|
|||
LIBRARIES += AP_ServoRelayEvents
|
||||
LIBRARIES += AP_Rally
|
||||
LIBRARIES += AP_OpticalFlow
|
||||
LIBRARIES += AP_RSSI
|
||||
LIBRARIES += AP_HAL_AVR
|
||||
LIBRARIES += AP_HAL_SITL
|
||||
LIBRARIES += AP_HAL_PX4
|
||||
|
|
|
@ -1,6 +1,7 @@
|
|||
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
||||
|
||||
#include "Plane.h"
|
||||
#include <AP_RSSI/AP_RSSI.h>
|
||||
|
||||
void Plane::init_barometer(void)
|
||||
{
|
||||
|
@ -82,18 +83,10 @@ void Plane::read_battery(void)
|
|||
}
|
||||
}
|
||||
|
||||
|
||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||
// RC_CHANNELS_SCALED message
|
||||
void Plane::read_receiver_rssi(void)
|
||||
{
|
||||
// avoid divide by zero
|
||||
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);
|
||||
}
|
||||
receiver_rssi = rssi.read_receiver_rssi_uint8();
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue