APM: added RECEIVER_RSSI_PIN option

this allows for the receiver RSSI to be sent over MAVLink

Thanks to Burt Green for the suggestion
This commit is contained in:
Andrew Tridgell 2012-08-22 13:33:12 +10:00
parent ed7eec91f4
commit c0c0b8c976
4 changed files with 25 additions and 2 deletions

View File

@ -280,6 +280,10 @@ AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter); AP_RangeFinder_MaxsonarXL sonar(&pitot_analog_source, &sonar_mode_filter);
#endif #endif
#if RECEIVER_RSSI_PIN != -1
AP_AnalogSource_Arduino RSSI_pin(RECEIVER_RSSI_PIN, 0.25);
#endif
AP_Relay relay; AP_Relay relay;

View File

@ -286,7 +286,6 @@ static void NOINLINE send_gps_raw(mavlink_channel_t chan)
static void NOINLINE send_servo_out(mavlink_channel_t chan) static void NOINLINE send_servo_out(mavlink_channel_t chan)
{ {
const uint8_t rssi = 1;
// normalized values scaled to -10000 to 10000 // normalized values scaled to -10000 to 10000
// This is used for HIL. Do not change without discussing with // This is used for HIL. Do not change without discussing with
// HIL maintainers // HIL maintainers
@ -302,7 +301,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
0, 0,
0, 0,
0, 0,
rssi); receiver_rssi());
} }
static void NOINLINE send_radio_in(mavlink_channel_t chan) static void NOINLINE send_radio_in(mavlink_channel_t chan)

View File

@ -115,6 +115,11 @@
#endif #endif
// pin for receiver RSSI
#ifndef RECEIVER_RSSI_PIN
# define RECEIVER_RSSI_PIN -1
#endif
////////////////////////////////////////////////////////////////////////////// //////////////////////////////////////////////////////////////////////////////
// IMU Selection // IMU Selection
// //

View File

@ -55,3 +55,18 @@ static void read_battery(void)
#endif #endif
} }
// return receiver RSSI as an 8 bit number for MAVLink
// RC_CHANNELS_SCALED message
uint8_t receiver_rssi(void)
{
#if RECEIVER_RSSI_PIN != -1
float ret = RSSI_pin.read();
if (ret >= 255) {
return 255;
}
return (uint8_t)ret;
#else
return 0;
#endif
}