Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

rc channel wasn't completely protected by #define guards #25918

Closed
wants to merge 1 commit into from
Closed
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions libraries/AP_Common/AP_Common.h
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,9 @@
#define UNUSED_PRIVATE_MEMBER
#endif

// for local vars that are unused in some builds
#define UNUSED_LOCAL(x) ((void)(x))

// this can be used to optimize individual functions
#define OPTIMIZE(level) __attribute__((optimize(level)))

Expand Down
1 change: 1 addition & 0 deletions libraries/AP_HAL_ESP32/boards/esp32empty.h
Original file line number Diff line number Diff line change
Expand Up @@ -148,4 +148,5 @@

#define HAL_LOGGING_BACKENDS_DEFAULT 1

#define AP_RC_CHANNEL_ENABLED 0
#define AP_RCPROTOCOL_ENABLED 0
2 changes: 2 additions & 0 deletions libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -185,10 +185,12 @@ void AP_RCProtocol_FPort::decode_downlink(const FPort_Frame &frame)
uint8_t len = 0;
uint8_t buf2[sizeof(buf)*2+1];

#if AP_RC_CHANNEL_ENABLED
if (rc().option_is_enabled(RC_Channels::Option::FPORT_PAD)) {
// this padding helps on some uarts that have hw pullups
buf2[len++] = 0xff;
}
#endif

for (uint8_t i=0; i<sizeof(buf); i++) {
uint8_t c = buf[i];
Expand Down
23 changes: 22 additions & 1 deletion libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -99,10 +99,11 @@ void AP_CRSF_Telem::setup_custom_telemetry()
if (_custom_telem.init_done) {
return;
}

#if AP_RC_CHANNEL_ENABLED
if (!rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) {
return;
}
#endif

// check if passthru already assigned
const int8_t frsky_port = AP::serialmanager().find_portnum(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough,0);
Expand Down Expand Up @@ -204,7 +205,11 @@ bool AP_CRSF_Telem::process_rf_mode_changes()
}
#endif
// note if option was set to show LQ in place of RSSI
#if AP_RC_CHANNEL_ENABLED
bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI);
#else
bool current_lq_as_rssi_active = false;
#endif
if(_telem_bootstrap_msg_pending || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
_noted_lq_as_rssi_active = current_lq_as_rssi_active;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally");
Expand All @@ -214,10 +219,12 @@ bool AP_CRSF_Telem::process_rf_mode_changes()
const bool is_high_speed = is_high_speed_telemetry(current_rf_mode);
if ((now - _telem_last_report_ms > 5000)) {
// report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s
#if AP_RC_CHANNEL_ENABLED
if (!rc().option_is_enabled(RC_Channels::Option::SUPPRESS_CRSF_MESSAGE) && (_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "%s: Link rate %dHz, Telemetry rate %dHz",
get_protocol_string(), crsf->get_link_rate(_crsf_version.protocol), get_telemetry_rate());
}
#endif
// tune the scheduler based on telemetry speed high/low transitions
if (_telem_is_high_speed != is_high_speed) {
update_custom_telemetry_rates(current_rf_mode);
Expand Down Expand Up @@ -308,9 +315,11 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
{
// no need to queue status text messages when crossfire
// custom telemetry is not enabled
#if AP_RC_CHANNEL_ENABLED
if (!rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) {
return;
}
#endif
AP_RCTelemetry::queue_message(severity, text);
}

Expand Down Expand Up @@ -401,9 +410,17 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
return false;
#endif
case PASSTHROUGH:
#if AP_RC_CHANNEL_ENABLED
return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY);
#else
return false;
#endif
case STATUS_TEXT:
#if AP_RC_CHANNEL_ENABLED
return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY) && !queue_empty;
#else
return false;
#endif
case GENERAL_COMMAND:
return _baud_rate_request.pending;
case VERSION_PING:
Expand Down Expand Up @@ -957,7 +974,11 @@ void AP_CRSF_Telem::calc_flight_mode()
sizeof(AP_CRSF_Telem::FlightModeFrame),
"%s%s",
notify->get_flight_mode_str(),
#if AP_RC_CHANNEL_ENABLED
rc().option_is_enabled(RC_Channels::Option::CRSF_FM_DISARM_STAR) && !hal.util->get_soft_armed() ? "*" : ""
#else
""
#endif
);
// Note: strlen(_telem.bcast.flightmode.flight_mode) is safe because called on a guaranteed null terminated string
_telem_size = strlen(_telem.bcast.flightmode.flight_mode) + 1; //send the terminator as well
Expand Down
40 changes: 19 additions & 21 deletions libraries/GCS_MAVLink/GCS_Common.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -2002,19 +2002,24 @@ bool GCS_MAVLINK::sending_mavlink1() const
return ((_channel_status.flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0);
}

#if AP_RC_CHANNEL_ENABLED
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks wrong.

We should avoid calling this method rather than putting stuff in the body.

/*
send RC_CHANNELS messages
*/
void GCS_MAVLINK::send_rc_channels() const
{
uint16_t values[18] = {};
#if AP_RC_CHANNEL_ENABLED
rc().get_radio_in(values, ARRAY_SIZE(values));
#endif

mavlink_msg_rc_channels_send(
chan,
AP_HAL::millis(),
#if AP_RC_CHANNEL_ENABLED
RC_Channels::get_valid_channel_count(),
#else
0,
#endif
values[0],
values[1],
values[2],
Expand Down Expand Up @@ -2050,7 +2055,9 @@ void GCS_MAVLINK::send_rc_channels_raw() const
}

uint16_t values[8] = {};
#if AP_RC_CHANNEL_ENABLED
rc().get_radio_in(values, ARRAY_SIZE(values));
#endif

mavlink_msg_rc_channels_raw_send(
chan,
Expand All @@ -2071,7 +2078,6 @@ void GCS_MAVLINK::send_rc_channels_raw() const
#endif
);
}
#endif // AP_RC_CHANNEL_ENABLED

void GCS_MAVLINK::send_raw_imu()
{
Expand Down Expand Up @@ -2899,22 +2905,22 @@ void GCS_MAVLINK::send_heartbeat() const
system_status());
}

#if AP_RC_CHANNEL_ENABLED
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Likewise, this #if should remain.

MAV_RESULT GCS_MAVLINK::handle_command_do_aux_function(const mavlink_command_int_t &packet)
{
if (packet.param2 > 2) {
return MAV_RESULT_DENIED;
}
#if AP_RC_CHANNEL_ENABLED
const RC_Channel::AUX_FUNC aux_func = (RC_Channel::AUX_FUNC)packet.param1;
const RC_Channel::AuxSwitchPos position = (RC_Channel::AuxSwitchPos)packet.param2;
if (!rc().run_aux_function(aux_func, position, RC_Channel::AuxFuncTriggerSource::MAVLINK)) {
// note that this is not quite right; we could be more nuanced
// about our return code here.
return MAV_RESULT_FAILED;
}
#endif
return MAV_RESULT_ACCEPTED;
}
#endif // AP_RC_CHANNEL_ENABLED

MAV_RESULT GCS_MAVLINK::handle_command_set_message_interval(const mavlink_command_int_t &packet)
{
Expand Down Expand Up @@ -3363,7 +3369,6 @@ MAV_RESULT GCS_MAVLINK::handle_flight_termination(const mavlink_command_int_t &p
#endif
}

#if AP_RC_CHANNEL_ENABLED
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

... and this one

... and all the rest of them...

/*
handle a R/C bind request (for spektrum)
*/
Expand All @@ -3372,12 +3377,13 @@ MAV_RESULT GCS_MAVLINK::handle_START_RX_PAIR(const mavlink_command_int_t &packet
// initiate bind procedure. We accept the DSM type from either
// param1 or param2 due to a past mixup with what parameter is the
// right one
#if AP_RC_CHANNEL_ENABLED
if (!RC_Channels::receiver_bind(packet.param2>0?packet.param2:packet.param1)) {
return MAV_RESULT_FAILED;
}
#endif
return MAV_RESULT_ACCEPTED;
}
#endif // AP_RC_CHANNEL_ENABLED

uint64_t GCS_MAVLINK::timesync_receive_timestamp_ns() const
{
Expand Down Expand Up @@ -3777,7 +3783,6 @@ void GCS_MAVLINK::handle_command_ack(const mavlink_message_t &msg)
#endif
}

#if AP_RC_CHANNEL_ENABLED
// allow override of RC channel values for complete GCS
// control of switch position and RC PWM values.
void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
Expand All @@ -3791,6 +3796,7 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
mavlink_rc_channels_override_t packet;
mavlink_msg_rc_channels_override_decode(&msg, &packet);

#if AP_RC_CHANNEL_ENABLED
const uint16_t override_data[] = {
packet.chan1_raw,
packet.chan2_raw,
Expand Down Expand Up @@ -3826,11 +3832,11 @@ void GCS_MAVLINK::handle_rc_channels_override(const mavlink_message_t &msg)
RC_Channels::set_override(i, value, tnow);
}
}

#endif // AP_RC_CHANNEL_ENABLED

gcs().sysid_myggcs_seen(tnow);

}
#endif // AP_RC_CHANNEL_ENABLED

#if AP_OPTICALFLOW_ENABLED
void GCS_MAVLINK::handle_optical_flow(const mavlink_message_t &msg)
Expand Down Expand Up @@ -4118,11 +4124,9 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
break;
#endif

#if AP_RC_CHANNEL_ENABLED
case MAVLINK_MSG_ID_MANUAL_CONTROL:
handle_manual_control(msg);
break;
#endif

#if AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED
case MAVLINK_MSG_ID_PLAY_TUNE:
Expand Down Expand Up @@ -4185,11 +4189,9 @@ void GCS_MAVLINK::handle_message(const mavlink_message_t &msg)
break;
#endif

#if AP_RC_CHANNEL_ENABLED
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
handle_rc_channels_override(msg);
break;
#endif

#if AP_OPTICALFLOW_ENABLED
case MAVLINK_MSG_ID_OPTICAL_FLOW:
Expand Down Expand Up @@ -5192,10 +5194,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
return MAV_RESULT_FAILED;
#endif

#if AP_RC_CHANNEL_ENABLED
case MAV_CMD_DO_AUX_FUNCTION:
return handle_command_do_aux_function(packet);
#endif

#if AP_FENCE_ENABLED
case MAV_CMD_DO_FENCE_ENABLE:
Expand Down Expand Up @@ -5346,10 +5346,8 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
return handle_command_set_ekf_source_set(packet);
#endif

#if AP_RC_CHANNEL_ENABLED
case MAV_CMD_START_RX_PAIR:
return handle_START_RX_PAIR(packet);
#endif

#if AP_FILESYSTEM_FORMAT_ENABLED
case MAV_CMD_STORAGE_FORMAT:
Expand Down Expand Up @@ -6093,7 +6091,6 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
break;
#endif

#if AP_RC_CHANNEL_ENABLED
case MSG_RC_CHANNELS:
CHECK_PAYLOAD_SIZE(RC_CHANNELS);
send_rc_channels();
Expand All @@ -6103,7 +6100,7 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
CHECK_PAYLOAD_SIZE(RC_CHANNELS_RAW);
send_rc_channels_raw();
break;
#endif


case MSG_RAW_IMU:
CHECK_PAYLOAD_SIZE(RAW_IMU);
Expand Down Expand Up @@ -6779,12 +6776,12 @@ uint64_t GCS_MAVLINK::capabilities() const
}


#if AP_RC_CHANNEL_ENABLED
void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_t offset, const float scaler, const uint32_t tnow, const bool reversed)
{
if (c == nullptr) {
return;
}
#if AP_RC_CHANNEL_ENABLED
int16_t override_value = 0;
if (value_in != INT16_MAX) {
const int16_t radio_min = c->get_radio_min();
Expand All @@ -6795,8 +6792,10 @@ void GCS_MAVLINK::manual_override(RC_Channel *c, int16_t value_in, const uint16_
override_value = radio_min + (radio_max - radio_min) * (value_in + offset) / scaler;
}
c->set_override(override_value, tnow);
#endif
}


void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg)
{
if (msg.sysid != sysid_my_gcs()) {
Expand All @@ -6818,7 +6817,6 @@ void GCS_MAVLINK::handle_manual_control(const mavlink_message_t &msg)
// from the ground station for failsafe purposes
gcs().sysid_myggcs_seen(tnow);
}
#endif // AP_RC_CHANNEL_ENABLED

#if AP_RSSI_ENABLED
uint8_t GCS_MAVLINK::receiver_rssi() const
Expand Down
5 changes: 5 additions & 0 deletions libraries/SRV_Channel/SRV_Channel_aux.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ void SRV_Channel::output_ch(void)
passthrough_mapped = true;
break;
}
#if AP_RC_CHANNEL_ENABLED
if (passthrough_from != -1) {
// we are doing passthrough from input to output for this channel
RC_Channel *c = rc().channel(passthrough_from);
Expand Down Expand Up @@ -94,6 +95,10 @@ void SRV_Channel::output_ch(void)
}
}
}
#else
UNUSED_LOCAL(passthrough_from);
UNUSED_LOCAL(passthrough_mapped);
#endif
#endif // HAL_BUILD_AP_PERIPH

if (!(SRV_Channels::disabled_mask & (1U<<ch_num))) {
Expand Down
Loading