mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-12 01:23:56 -03:00
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:
parent
c5f70d22c5
commit
a315b980bf
@ -95,7 +95,7 @@ void Rover::setup()
|
|||||||
AP_Notify::flags.pre_arm_gps_check = true;
|
AP_Notify::flags.pre_arm_gps_check = true;
|
||||||
AP_Notify::flags.failsafe_battery = false;
|
AP_Notify::flags.failsafe_battery = false;
|
||||||
|
|
||||||
rssi_analog_source = hal.analogin->channel(ANALOG_INPUT_NONE);
|
rssi.init();
|
||||||
|
|
||||||
init_ardupilot();
|
init_ardupilot();
|
||||||
|
|
||||||
|
@ -37,13 +37,6 @@ const AP_Param::Info Rover::var_info[] PROGMEM = {
|
|||||||
// @User: Advanced
|
// @User: Advanced
|
||||||
GSCALAR(initial_mode, "INITIAL_MODE", MANUAL),
|
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
|
// @Param: SYSID_THIS_MAV
|
||||||
// @DisplayName: MAVLink system ID of this vehicle
|
// @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
|
// @Description: Allows setting an individual MAVLink system id for this vehicle to distinguish it from others on the same network
|
||||||
@ -541,6 +534,10 @@ const AP_Param::Info Rover::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),
|
||||||
|
|
||||||
AP_VAREND
|
AP_VAREND
|
||||||
};
|
};
|
||||||
|
|
||||||
|
@ -40,7 +40,7 @@ public:
|
|||||||
k_param_rc_14,
|
k_param_rc_14,
|
||||||
|
|
||||||
// IO pins
|
// 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_volt_pin,
|
||||||
k_param_battery_curr_pin,
|
k_param_battery_curr_pin,
|
||||||
|
|
||||||
@ -55,6 +55,8 @@ public:
|
|||||||
k_param_serial1_baud, // deprecated, can be deleted
|
k_param_serial1_baud, // deprecated, can be deleted
|
||||||
k_param_serial2_baud, // deprecated, can be deleted
|
k_param_serial2_baud, // deprecated, can be deleted
|
||||||
|
|
||||||
|
// 97: RSSI
|
||||||
|
k_param_rssi = 97,
|
||||||
|
|
||||||
// 110: Telemetry control
|
// 110: Telemetry control
|
||||||
//
|
//
|
||||||
@ -203,9 +205,6 @@ public:
|
|||||||
AP_Int8 reset_switch_chan;
|
AP_Int8 reset_switch_chan;
|
||||||
AP_Int8 initial_mode;
|
AP_Int8 initial_mode;
|
||||||
|
|
||||||
// IO pins
|
|
||||||
AP_Int8 rssi_pin;
|
|
||||||
|
|
||||||
// braking
|
// braking
|
||||||
AP_Int8 braking_percent;
|
AP_Int8 braking_percent;
|
||||||
AP_Float braking_speederr;
|
AP_Float braking_speederr;
|
||||||
|
@ -84,6 +84,7 @@
|
|||||||
#include <AP_Notify/AP_Notify.h> // Notify library
|
#include <AP_Notify/AP_Notify.h> // Notify library
|
||||||
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor library
|
#include <AP_BattMonitor/AP_BattMonitor.h> // Battery monitor 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
|
||||||
|
|
||||||
// Configuration
|
// Configuration
|
||||||
#include "config.h"
|
#include "config.h"
|
||||||
@ -169,6 +170,9 @@ private:
|
|||||||
|
|
||||||
OpticalFlow optflow;
|
OpticalFlow optflow;
|
||||||
|
|
||||||
|
// RSSI
|
||||||
|
AP_RSSI rssi;
|
||||||
|
|
||||||
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
|
||||||
SITL sitl;
|
SITL sitl;
|
||||||
#endif
|
#endif
|
||||||
@ -178,10 +182,6 @@ private:
|
|||||||
const uint8_t num_gcs;
|
const uint8_t num_gcs;
|
||||||
GCS_MAVLINK gcs[MAVLINK_COMM_NUM_BUFFERS];
|
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
|
// relay support
|
||||||
AP_Relay relay;
|
AP_Relay relay;
|
||||||
|
|
||||||
|
@ -51,5 +51,6 @@ LIBRARIES += AP_HAL_Empty
|
|||||||
LIBRARIES += AP_Notify
|
LIBRARIES += AP_Notify
|
||||||
LIBRARIES += AP_BattMonitor
|
LIBRARIES += AP_BattMonitor
|
||||||
LIBRARIES += AP_OpticalFlow
|
LIBRARIES += AP_OpticalFlow
|
||||||
|
LIBRARIES += AP_RSSI
|
||||||
LIBRARIES += AP_Declination
|
LIBRARIES += AP_Declination
|
||||||
|
|
||||||
|
@ -25,9 +25,7 @@ void Rover::read_battery(void)
|
|||||||
// RC_CHANNELS_SCALED message
|
// RC_CHANNELS_SCALED message
|
||||||
void Rover::read_receiver_rssi(void)
|
void Rover::read_receiver_rssi(void)
|
||||||
{
|
{
|
||||||
rssi_analog_source->set_pin(g.rssi_pin);
|
receiver_rssi = rssi.read_receiver_rssi_uint8();
|
||||||
float ret = rssi_analog_source->voltage_average() * 50;
|
|
||||||
receiver_rssi = constrain_int16(ret, 0, 255);
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// read the sonars
|
// read the sonars
|
||||||
|
Loading…
Reference in New Issue
Block a user