mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-22 16:48:29 -04:00
Copter: ported RSSI_PIN option from ArduPlane
this is useful for OSD users
This commit is contained in:
parent
24c0413b7e
commit
386ef45fbb
@ -440,6 +440,9 @@ static int8_t control_mode = STABILIZE;
|
||||
// This is set to -1 when we need to re-read the switch
|
||||
static byte oldSwitchPosition;
|
||||
|
||||
// receiver RSSI
|
||||
static uint8_t receiver_rssi;
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
// Motor Output
|
||||
@ -890,6 +893,10 @@ static uint8_t save_trim_counter;
|
||||
// Reference to the AP relay object - APM1 only
|
||||
AP_Relay relay;
|
||||
|
||||
// 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_AnalogSource_Arduino RSSI_pin(-1, 0.25);
|
||||
|
||||
#if CLI_ENABLED == ENABLED
|
||||
static int8_t setup_show (uint8_t argc, const Menu::arg *argv);
|
||||
#endif
|
||||
@ -1105,7 +1112,7 @@ static void medium_loop()
|
||||
//------------------------------------------------
|
||||
case 1:
|
||||
medium_loopCounter++;
|
||||
|
||||
read_receiver_rssi();
|
||||
break;
|
||||
|
||||
// command processing
|
||||
|
@ -295,7 +295,6 @@ static void NOINLINE send_gps_raw(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
|
||||
// This is used for HIL. Do not change without discussing with HIL maintainers
|
||||
|
||||
@ -313,7 +312,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||
0,
|
||||
0,
|
||||
0,
|
||||
rssi);
|
||||
receiver_rssi);
|
||||
#else
|
||||
#if X_PLANE == ENABLED
|
||||
/* update by JLN for X-Plane HIL */
|
||||
@ -330,7 +329,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||
10000 * g.rc_2.norm_output(),
|
||||
10000 * g.rc_3.norm_output(),
|
||||
10000 * g.rc_4.norm_output(),
|
||||
rssi);
|
||||
receiver_rssi);
|
||||
}else{
|
||||
mavlink_msg_rc_channels_scaled_send(
|
||||
chan,
|
||||
@ -344,7 +343,7 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||
10000 * g.rc_2.norm_output(),
|
||||
10000 * g.rc_3.norm_output(),
|
||||
10000 * g.rc_4.norm_output(),
|
||||
rssi);
|
||||
receiver_rssi);
|
||||
}
|
||||
|
||||
#else
|
||||
@ -360,14 +359,13 @@ static void NOINLINE send_servo_out(mavlink_channel_t chan)
|
||||
10000 * g.rc_2.norm_output(),
|
||||
10000 * g.rc_3.norm_output(),
|
||||
10000 * g.rc_4.norm_output(),
|
||||
rssi);
|
||||
receiver_rssi);
|
||||
#endif
|
||||
#endif
|
||||
}
|
||||
|
||||
static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||
{
|
||||
const uint8_t rssi = 1;
|
||||
mavlink_msg_rc_channels_raw_send(
|
||||
chan,
|
||||
millis(),
|
||||
@ -380,7 +378,7 @@ static void NOINLINE send_radio_in(mavlink_channel_t chan)
|
||||
g.rc_6.radio_in,
|
||||
g.rc_7.radio_in,
|
||||
g.rc_8.radio_in,
|
||||
rssi);
|
||||
receiver_rssi);
|
||||
}
|
||||
|
||||
static void NOINLINE send_radio_out(mavlink_channel_t chan)
|
||||
|
@ -70,6 +70,7 @@ public:
|
||||
// 2 = med, 3 = slow
|
||||
|
||||
k_param_crosstrack_min_distance,
|
||||
k_param_rssi_pin,
|
||||
|
||||
// 65: AP_Limits Library
|
||||
k_param_limits = 65,
|
||||
@ -262,6 +263,7 @@ public:
|
||||
|
||||
AP_Int8 battery_volt_pin;
|
||||
AP_Int8 battery_curr_pin;
|
||||
AP_Int8 rssi_pin;
|
||||
|
||||
// Waypoints
|
||||
//
|
||||
|
@ -137,6 +137,13 @@ const AP_Param::Info var_info[] PROGMEM = {
|
||||
// @User: Standard
|
||||
GSCALAR(battery_curr_pin, "BATT_CURR_PIN", BATTERY_CURR_PIN),
|
||||
|
||||
// @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:A0, 1:A1, 2:A2, 13:A13
|
||||
// @User: Standard
|
||||
GSCALAR(rssi_pin, "RSSI_PIN", -1),
|
||||
|
||||
GSCALAR(waypoint_mode, "WP_MODE", 0),
|
||||
GSCALAR(command_total, "WP_TOTAL", 0),
|
||||
GSCALAR(command_index, "WP_INDEX", 0),
|
||||
|
@ -104,3 +104,12 @@ static void read_battery(void)
|
||||
low_battery_counter = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// read the receiver RSSI as an 8 bit number for MAVLink
|
||||
// RC_CHANNELS_SCALED message
|
||||
void read_receiver_rssi(void)
|
||||
{
|
||||
RSSI_pin.set_pin(g.rssi_pin);
|
||||
float ret = RSSI_pin.read();
|
||||
receiver_rssi = constrain(ret, 0, 255);
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user