Rover: 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:
Stewart Loving-Gibbard 2015-08-27 23:00:12 -07:00 committed by Andrew Tridgell
parent c5f70d22c5
commit a315b980bf
6 changed files with 16 additions and 21 deletions

View File

@ -94,8 +94,8 @@ void Rover::setup()
AP_Notify::flags.pre_arm_check = true;
AP_Notify::flags.pre_arm_gps_check = true;
AP_Notify::flags.failsafe_battery = false;
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
rssi.init();
init_ardupilot();

View File

@ -37,13 +37,6 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
// @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, 2:APM2 A2, 13:APM2 A13, 103:Pixhawk SBUS
// @User: Standard
GSCALAR(rssi_pin, "RSSI_PIN", -1),
// @Param: SYSID_THIS_MAV
// @DisplayName: MAVLink system ID of this vehicle
// @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
@ -540,6 +533,10 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
// @Group: MIS_
// @Path: ../libraries/AP_Mission/AP_Mission.cpp
GOBJECT(mission, "MIS_", AP_Mission),
// @Group: RSSI_
// @Path: ../libraries/AP_RSSI/AP_RSSI.cpp
GOBJECT(rssi, "RSSI_", AP_RSSI),
AP_VAREND
};

View File

@ -40,7 +40,7 @@ public:
k_param_rc_14,
// IO pins
k_param_rssi_pin = 20,
k_param_rssi_pin = 20, // unused, replaced by rssi_ library parameters
k_param_battery_volt_pin,
k_param_battery_curr_pin,
@ -55,7 +55,9 @@ public:
k_param_serial1_baud, // deprecated, can be deleted
k_param_serial2_baud, // deprecated, can be deleted
// 97: RSSI
k_param_rssi = 97,
// 110: Telemetry control
//
k_param_gcs0 = 110, // stream rates for uartA
@ -203,9 +205,6 @@ public:
AP_Int8 reset_switch_chan;
AP_Int8 initial_mode;
// IO pins
AP_Int8 rssi_pin;
// braking
AP_Int8 braking_percent;
AP_Float braking_speederr;

View File

@ -84,6 +84,7 @@
#include <AP_Notify/AP_Notify.h> // Notify library
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
#include <AP_OpticalFlow/AP_OpticalFlow.h> // Optical Flow library
#include <AP_RSSI/AP_RSSI.h> // RSSI Library
// Configuration
#include "config.h"
@ -168,6 +169,9 @@ private:
AP_Mission mission;
OpticalFlow optflow;
// RSSI
AP_RSSI rssi;
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
SITL sitl;
@ -178,10 +182,6 @@ private:
const uint8_t num_gcs;
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
// a pin for reading the receiver RSSI voltage. The scaling by 0.25
// is to take the 0 to 1024 range down to an 8 bit range for MAVLink
AP_HAL::AnalogSource *rssi_analog_source;
// relay support
AP_Relay relay;

View File

@ -51,5 +51,6 @@ LIBRARIES += AP_HAL_Empty
LIBRARIES += AP_Notify
LIBRARIES += AP_BattMonitor
LIBRARIES += AP_OpticalFlow
LIBRARIES += AP_RSSI
LIBRARIES += AP_Declination

View File

@ -25,9 +25,7 @@ void Rover::read_battery(void)
// RC_CHANNELS_SCALED message
void Rover::read_receiver_rssi(void)
{
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->voltage_average() * 50;
receiver_rssi = constrain_int16(ret, 0, 255);
receiver_rssi = rssi.read_receiver_rssi_uint8();
}
// read the sonars