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

Add and use AP_SBUSOUTPUT_ENABLED #24070

Merged
merged 4 commits into from Jun 27, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
1 change: 1 addition & 0 deletions Tools/scripts/build_options.py
Expand Up @@ -311,6 +311,7 @@ def __init__(self,
Feature('Actuators', 'Volz', 'AP_VOLZ_ENABLED', 'Enable Volz Protocol', 0, None),
Feature('Actuators', 'Volz_DroneCAN', 'AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', 'Enable Volz DroneCAN Feedback', 0, None),
Feature('Actuators', 'RobotisServo', 'AP_ROBOTISSERVO_ENABLED', 'Enable RobotisServo Protocol', 0, None),
Feature('Actuators', 'SBUS Output', 'AP_SBUSOUTPUT_ENABLED', 'Enable SBUS Output on serial ports', 0, None),
Feature('Actuators', 'FETTecOneWire', 'AP_FETTEC_ONEWIRE_ENABLED', 'Enable FETTec OneWire ESCs', 0, None),
Feature('Actuators', 'KDECAN', 'AP_KDECAN_ENABLED', 'KDE Direct KDECAN ESC', 0, None),

Expand Down
1 change: 1 addition & 0 deletions Tools/scripts/extract_features.py
Expand Up @@ -162,6 +162,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm"):
('AP_DRONECAN_VOLZ_FEEDBACK_ENABLED', r'AP_DroneCAN::handle_actuator_status_Volz\b',),
('AP_ROBOTISSERVO_ENABLED', r'AP_RobotisServo::init\b',),
('AP_FETTEC_ONEWIRE_ENABLED', r'AP_FETtecOneWire::init\b',),
('AP_SBUSOUTPUT_ENABLED', 'AP_SBusOut::sbus_format_frame',),
('AP_KDECAN_ENABLED', r'AP_KDECAN::update\b',),

('AP_RPM_ENABLED', 'AP_RPM::AP_RPM',),
Expand Down
4 changes: 4 additions & 0 deletions libraries/AP_HAL_ChibiOS/hwdef/scripts/chibios_hwdef.py
Expand Up @@ -3079,6 +3079,10 @@ def add_apperiph_defaults(self, f):
#define AP_ROBOTISSERVO_ENABLED 0
#endif

#ifndef AP_SBUSOUTPUT_ENABLED
#define AP_SBUSOUTPUT_ENABLED 0
#endif

// by default an AP_Periph defines as many servo output channels as
// there are PWM outputs:
#ifndef NUM_SERVO_CHANNELS
Expand Down
6 changes: 6 additions & 0 deletions libraries/AP_SBusOut/AP_SBusOut.cpp
Expand Up @@ -37,6 +37,11 @@
* POSSIBILITY OF SUCH DAMAGE.
*
*/

#include "AP_SBusOut_config.h"

#if AP_SBUSOUTPUT_ENABLED

#include "AP_SBusOut.h"
#include <AP_Math/AP_Math.h>
#include <AP_SerialManager/AP_SerialManager.h>
Expand Down Expand Up @@ -189,3 +194,4 @@ void AP_SBusOut::init() {
sbus1_uart = serial_manager->find_serial(AP_SerialManager::SerialProtocol_Sbus1,0);
}

#endif // AP_SBUSOUTPUT_ENABLED
6 changes: 6 additions & 0 deletions libraries/AP_SBusOut/AP_SBusOut.h
Expand Up @@ -7,6 +7,10 @@

#pragma once

#include "AP_SBusOut_config.h"

#if AP_SBUSOUTPUT_ENABLED

#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>

Expand Down Expand Up @@ -35,3 +39,5 @@ class AP_SBusOut {
AP_Int16 sbus_rate;
bool initialised;
};

#endif // AP_SBUSOUTPUT_ENABLED
7 changes: 7 additions & 0 deletions libraries/AP_SBusOut/AP_SBusOut_config.h
@@ -0,0 +1,7 @@
#pragma once

#include <AP_HAL/AP_HAL_Boards.h>

#ifndef AP_SBUSOUTPUT_ENABLED
#define AP_SBUSOUTPUT_ENABLED 1
#endif
4 changes: 2 additions & 2 deletions libraries/SRV_Channel/SRV_Channel.h
Expand Up @@ -601,11 +601,11 @@ class SRV_Channels {
static AP_Volz_Protocol *volz_ptr;
#endif

#ifndef HAL_BUILD_AP_PERIPH
#if AP_SBUSOUTPUT_ENABLED
// support for SBUS protocol
AP_SBusOut sbus;
static AP_SBusOut *sbus_ptr;
#endif // HAL_BUILD_AP_PERIPH
#endif

#if AP_ROBOTISSERVO_ENABLED
// support for Robotis servo protocol
Expand Down
12 changes: 6 additions & 6 deletions libraries/SRV_Channel/SRV_Channels.cpp
Expand Up @@ -45,7 +45,7 @@ SRV_Channels *SRV_Channels::_singleton;
AP_Volz_Protocol *SRV_Channels::volz_ptr;
#endif

#ifndef HAL_BUILD_AP_PERIPH
#if AP_SBUSOUTPUT_ENABLED
AP_SBusOut *SRV_Channels::sbus_ptr;
#endif

Expand Down Expand Up @@ -195,11 +195,11 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
AP_SUBGROUPINFO(volz, "_VOLZ_", 19, SRV_Channels, AP_Volz_Protocol),
#endif

#ifndef HAL_BUILD_AP_PERIPH
#if AP_SBUSOUTPUT_ENABLED
// @Group: _SBUS_
// @Path: ../AP_SBusOut/AP_SBusOut.cpp
AP_SUBGROUPINFO(sbus, "_SBUS_", 20, SRV_Channels, AP_SBusOut),
#endif // HAL_BUILD_AP_PERIPH
#endif

#if HAL_SUPPORT_RCOUT_SERIAL
// @Group: _BLH_
Expand Down Expand Up @@ -379,7 +379,7 @@ SRV_Channels::SRV_Channels(void)
volz_ptr = &volz;
#endif

#ifndef HAL_BUILD_AP_PERIPH
#if AP_SBUSOUTPUT_ENABLED
sbus_ptr = &sbus;
#endif

Expand Down Expand Up @@ -511,10 +511,10 @@ void SRV_Channels::push()
volz_ptr->update();
#endif

#ifndef HAL_BUILD_AP_PERIPH
#if AP_SBUSOUTPUT_ENABLED
// give sbus library a chance to update
sbus_ptr->update();
#endif // HAL_BUILD_AP_PERIPH
#endif

#if AP_ROBOTISSERVO_ENABLED
// give robotis library a chance to update
Expand Down