Copter: fixed RSSI reading on PX4

needed for different analog input scaling
This commit is contained in:
Andrew Tridgell 2013-05-13 15:18:32 +10:00
parent 001d18b51d
commit c44fd42349
2 changed files with 3 additions and 4 deletions

View File

@ -782,8 +782,7 @@ static AP_Relay relay;
static AP_Camera camera(&relay);
#endif
// 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
// a pin for reading the receiver RSSI voltage.
static AP_HAL::AnalogSource* rssi_analog_source;
@ -878,7 +877,7 @@ void setup() {
&sonar_mode_filter);
#endif
rssi_analog_source = hal.analogin->channel(g.rssi_pin, 0.25);
rssi_analog_source = hal.analogin->channel(g.rssi_pin);
batt_volt_analog_source = hal.analogin->channel(g.battery_volt_pin);
batt_curr_analog_source = hal.analogin->channel(g.battery_curr_pin);
board_vcc_analog_source = hal.analogin->channel(ANALOG_INPUT_BOARD_VCC);

View File

@ -142,6 +142,6 @@ static void read_battery(void)
void read_receiver_rssi(void)
{
rssi_analog_source->set_pin(g.rssi_pin);
float ret = rssi_analog_source->read_latest();
float ret = rssi_analog_source->voltage_average() * 50;
receiver_rssi = constrain_int16(ret, 0, 255);
}