├── lib ├── RosbotDrive │ ├── mbed_lib.json │ ├── internal │ │ └── rosbot-regulator │ │ │ ├── RosbotRegulator.h │ │ │ └── RosbotRegulatorCMSIS.h │ ├── RosbotDrive.h │ └── RosbotDrive.cpp └── MultiDistanceSensor │ ├── MultiDistanceSensor.h │ └── MultiDistanceSensor.cpp ├── compose.yaml ├── .gitignore ├── include ├── microros_transport │ └── mbed_serial_transport.hpp ├── main.hpp ├── leds.hpp ├── buttons.hpp ├── ranges.hpp ├── imu.hpp ├── battery.hpp ├── servos.hpp ├── memory_debug_message_info.hpp ├── wheels.hpp └── microros.hpp ├── .mbedignore ├── src ├── buttons.cpp ├── microros_transport │ ├── mbed_clock_gettime.cpp │ └── mbed_serial_transport.cpp ├── leds.cpp ├── ranges.cpp ├── battery.cpp ├── imu.cpp ├── TARGET_CORE2 │ ├── PeripheralNames.h │ ├── system_clock.c │ ├── PinNames.h │ └── PeripheralPins.c ├── servos.cpp ├── wheels.cpp ├── main.cpp └── microros.cpp ├── gen_hex.py ├── .vscode └── extensions.json ├── .github ├── pull_request_template.md └── workflows │ ├── firmware_build.yml │ └── firmware_release.yml ├── mbed_app.json ├── ignore_packages.py ├── .gitmodules ├── colcon.meta ├── CHANGELOG.md ├── boards └── core2.json ├── Dockerfile ├── platformio.ini ├── .devcontainer ├── devcontainer.json └── Dockerfile ├── custom_targets.json ├── ROS_API.md ├── README.md └── LICENSE /lib/RosbotDrive/mbed_lib.json: -------------------------------------------------------------------------------- 1 | { 2 | "name":"rosbot-drive", 3 | "macros":[], 4 | "config":{} 5 | } -------------------------------------------------------------------------------- /compose.yaml: -------------------------------------------------------------------------------- 1 | services: 2 | pio: 3 | build: . 4 | volumes: 5 | - .:/app 6 | # command: tail -f /dev/null -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .pio 2 | .vscode/.browse.c_cpp.db* 3 | .vscode/c_cpp_properties.json 4 | .vscode/launch.json 5 | .vscode/ipch 6 | .vscode/settings.json 7 | core2-flasher -------------------------------------------------------------------------------- /include/microros_transport/mbed_serial_transport.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "UARTSerial.h" 5 | 6 | void set_microros_serial_transports(mbed::UARTSerial *serial); -------------------------------------------------------------------------------- /.mbedignore: -------------------------------------------------------------------------------- 1 | **/cellular/* 2 | **/cryptocell/* 3 | **/deprecated_warnings/* 4 | **/lorawan/* 5 | **/lwipstack/* 6 | **/nanostack/* 7 | **/netsocket/* 8 | **/nfc/* 9 | **/unsupported/* -------------------------------------------------------------------------------- /src/buttons.cpp: -------------------------------------------------------------------------------- 1 | #include "buttons.hpp" 2 | 3 | void init_button_and_attach_to_callbacks(mbed::InterruptIn *interrupt, void (*rise)(), void (*fall)()){ 4 | interrupt->mode(PullUp); 5 | interrupt->fall(fall); 6 | interrupt->rise(rise); 7 | } -------------------------------------------------------------------------------- /gen_hex.py: -------------------------------------------------------------------------------- 1 | Import("env") 2 | 3 | env.AddPostAction( 4 | "$BUILD_DIR/${PROGNAME}.elf", 5 | env.VerboseAction(" ".join([ 6 | "$OBJCOPY", "-O", "ihex", "-R", ".eeprom", 7 | "$BUILD_DIR/${PROGNAME}.elf", "$BUILD_DIR/${PROGNAME}.hex" 8 | ]), "Building $BUILD_DIR/${PROGNAME}.hex") 9 | ) -------------------------------------------------------------------------------- /include/main.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | static UARTSerial microros_serial_rpi(RPI_SERIAL_TX, RPI_SERIAL_RX); 5 | static UARTSerial microros_serial_ftdi(FT_SERIAL_TX, FT_SERIAL_RX); 6 | 7 | static volatile bool distance_sensors_enabled = false; 8 | static DigitalOut sens_power(SENS_POWER_ON, 0); 9 | -------------------------------------------------------------------------------- /.vscode/extensions.json: -------------------------------------------------------------------------------- 1 | { 2 | // See http://go.microsoft.com/fwlink/?LinkId=827846 3 | // for the documentation about the extensions.json format 4 | "recommendations": [ 5 | "platformio.platformio-ide" 6 | ], 7 | "unwantedRecommendations": [ 8 | "ms-vscode.cpptools-extension-pack" 9 | ] 10 | } 11 | -------------------------------------------------------------------------------- /src/microros_transport/mbed_clock_gettime.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | extern "C" int clock_gettime(clockid_t unused, struct timespec *tp) { 5 | (void)unused; 6 | 7 | uint64_t ms = Kernel::get_ms_count(); 8 | tp->tv_sec = ms / 1000; 9 | tp->tv_nsec = (ms % 1000) * 1000000; 10 | 11 | return 0; 12 | } -------------------------------------------------------------------------------- /include/leds.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | 5 | enum LEDs 6 | { 7 | led_left, 8 | led_right, 9 | LED_COUNT 10 | }; 11 | 12 | static DigitalOut led2(LED2, 0); 13 | static DigitalOut led3(LED3, 0); 14 | 15 | void led1_callback(const void* message); 16 | void led2_callback(const void* message); 17 | -------------------------------------------------------------------------------- /.github/pull_request_template.md: -------------------------------------------------------------------------------- 1 | # TODO: specify bump command and remove this description 2 | 3 | Firmware release workflow is triggered when PR is closed, it bumps version, creates tag and release with firmware binary. To use it you have to specify bump type, available commands: `bump::patch`, `bump::minor` and `bump::major` (simply leave one of them in PR description and remove this description). -------------------------------------------------------------------------------- /src/leds.cpp: -------------------------------------------------------------------------------- 1 | #include "leds.hpp" 2 | 3 | void led1_callback(const void* msgin) 4 | { 5 | const std_msgs__msg__Bool* msg = (const std_msgs__msg__Bool*)msgin; 6 | if (msg != NULL) 7 | { 8 | led2 = msg->data; 9 | } 10 | } 11 | 12 | void led2_callback(const void* msgin) 13 | { 14 | const std_msgs__msg__Bool* msg = (const std_msgs__msg__Bool*)msgin; 15 | if (msg != NULL) 16 | { 17 | led3 = msg->data; 18 | } 19 | } -------------------------------------------------------------------------------- /include/buttons.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | enum Buttons 8 | { 9 | button_left, 10 | button_right, 11 | BUTTONS_COUNT 12 | }; 13 | 14 | static volatile bool buttons_publish_flag[BUTTONS_COUNT] = { false, false }; 15 | static std_msgs__msg__Bool button_msgs[BUTTONS_COUNT]; 16 | void init_button_and_attach_to_callbacks(mbed::InterruptIn *interrupt, void (*rise)(), void (*fall)()); -------------------------------------------------------------------------------- /mbed_app.json: -------------------------------------------------------------------------------- 1 | { 2 | "target_overrides": { 3 | "CORE2": { 4 | "platform.all-stats-enabled": false, 5 | "events.shared-dispatch-from-application": 0, 6 | "events.shared-eventsize": 1024, 7 | "events.shared-stacksize": 2048, 8 | "platform.default-serial-baud-rate": 576000, 9 | "platform.stdio-baud-rate": 576000, 10 | "drivers.uart-serial-rxbuf-size": 768, 11 | "drivers.uart-serial-txbuf-size": 768 12 | } 13 | } 14 | } -------------------------------------------------------------------------------- /ignore_packages.py: -------------------------------------------------------------------------------- 1 | import os 2 | user_name = os.getenv('USER') 3 | if user_name == None: 4 | print("There is no global variable 'USER'!!!") 5 | exit(-1) 6 | try: 7 | os.makedirs(f'/home/{user_name}/.platformio/packages/framework-mbed/features/') 8 | except FileExistsError: 9 | print('Folder already exists') 10 | 11 | with open(f'/home/{user_name}/.platformio/packages/framework-mbed/features/.mbedignore', 'w') as f: 12 | f.write('cellular/*\n') 13 | f.write('netsocket/cellular/*\n') 14 | f.write('nanostack/*\n') 15 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "encoder-mbed"] 2 | path = lib/encoder-mbed 3 | url = https://github.com/byq77/encoder-mbed.git 4 | branch = master 5 | [submodule "motor-driver-mbed"] 6 | path = lib/motor-driver-mbed 7 | url = https://github.com/byq77/drv88xx-driver-mbed.git 8 | branch = master 9 | [submodule "vl53l0x-mbed"] 10 | path = lib/vl53l0x-mbed 11 | url = https://github.com/byq77/vl53l0x-mbed.git 12 | branch = master 13 | [submodule "lib/imu-driver"] 14 | path = lib/imu-driver 15 | url = https://github.com/delihus/core2-imu-driver-mbed 16 | -------------------------------------------------------------------------------- /colcon.meta: -------------------------------------------------------------------------------- 1 | { 2 | "names": { 3 | "rmw_microxrcedds": { 4 | "cmake-args": [ 5 | "-DRMW_UXRCE_MAX_NODES=1", 6 | "-DRMW_UXRCE_MAX_PUBLISHERS=11", 7 | "-DRMW_UXRCE_MAX_SUBSCRIPTIONS=6", 8 | "-DRMW_UXRCE_MAX_SERVICES=6", 9 | "-DRMW_UXRCE_MAX_CLIENTS=1", 10 | "-DRMW_UXRCE_MAX_HISTORY=4", 11 | "-DRMW_UXRCE_TRANSPORT=custom", 12 | "-DRMW_UXRCE_ENTITY_CREATION_TIMEOUT=100", 13 | "-DRMW_UXRCE_ENTITY_DESTROY_TIMEOUT=0" 14 | ] 15 | } 16 | } 17 | } 18 | -------------------------------------------------------------------------------- /.github/workflows/firmware_build.yml: -------------------------------------------------------------------------------- 1 | name: Firmware release 2 | on: 3 | pull_request: 4 | 5 | jobs: 6 | firmware-build: 7 | name: Firmware build 8 | runs-on: ubuntu-22.04 9 | steps: 10 | - name: Checkout 11 | uses: actions/checkout@v2 12 | with: 13 | submodules: true 14 | # - name: Installing platformio 15 | # run: pip3 install -U platformio 16 | 17 | # - name: Building a firmware for STM32 18 | # run: git submodule update --init --recursive && pio run 19 | 20 | - name: Building a firmware for STM32 21 | run: | 22 | set -e 23 | docker compose build --no-cache 24 | docker compose up --exit-code-from pio -------------------------------------------------------------------------------- /include/ranges.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | enum Ranges { 11 | range_right_front, 12 | range_left_front, 13 | range_right_rear, 14 | range_left_rear, 15 | RANGES_COUNT 16 | }; 17 | 18 | static const char *range_frame_names[] = {"fr_range", "fl_range", "rr_range", "rl_range"}; 19 | 20 | static sensor_msgs__msg__Range range_msgs[RANGES_COUNT]; 21 | 22 | void init_ranges(); 23 | void fill_range_msg(sensor_msgs__msg__Range *msg, uint8_t id); 24 | void fill_range_msg_with_measurements(sensor_msgs__msg__Range *msg, float range); -------------------------------------------------------------------------------- /include/imu.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | 11 | static ImuDriver *imu_driver_ptr; 12 | static sensor_msgs__msg__Imu imu_msg; 13 | extern Mail imu_sensor_mail_box; 14 | 15 | 16 | static const char* imu_sensor_type_string[] = { "BNO055_ADDR_A", "BNO055_ADDR_B", "MPU9250", "MPU9255", "BHI260", "UNKNOWN" }; 17 | static char imu_description_string[64] = ""; 18 | 19 | void init_imu(mbed::I2C *i2c_ptr); 20 | void fill_imu_msg(sensor_msgs__msg__Imu *msg); 21 | void fill_imu_msg_with_measurements(sensor_msgs__msg__Imu* msg, ImuDriver::ImuMeasurement* measurements); -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # ROSbot firmware CHANGELOG 2 | 3 | All notable changes to this project will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](http://keepachangelog.com/) and this project adheres to [Semantic Versioning](http://semver.org/). 6 | 7 | ## [0.1.0] - 2022-10-27 8 | 9 | ### Changed 10 | - Separated ros1 and ros2 firmware. Moved from https://github.com/husarion/rosbot-stm32-firmware to https://github.com/husarion/rosbot_ros2_firmware 11 | - Changed communication interface to [Micro-ROS](https://micro.ros.org/). 12 | - Created compatibility with [rosbot_hardware_interfaces](https://github.com/husarion/rosbot_hardware_interfaces). 13 | - Added communication blinking LEDs. 14 | 15 | ### Fixed 16 | - Supported communication with ROS2 `humble` version. 17 | -------------------------------------------------------------------------------- /boards/core2.json: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "core": "stm32", 4 | "cpu": "cortex-m4", 5 | "extra_flags": "-DSTM32F4 -DSTM32F407xx -DSTM32F40_41xxx", 6 | "f_cpu": "168000000L", 7 | "mcu": "stm32f407zgt6", 8 | "product_line": "STM32F407xx", 9 | "variant": "BLACK_F407XX" 10 | }, 11 | "connectivity": [ 12 | "can" 13 | ], 14 | "debug": { 15 | "jlink_device": "STM32F407ZE", 16 | "openocd_target": "stm32f4x", 17 | "svd_path": "STM32F40x.svd" 18 | }, 19 | "frameworks": [ 20 | "mbed" 21 | ], 22 | "name": "CORE2", 23 | "upload": { 24 | "maximum_ram_size": 196608, 25 | "maximum_size": 1048576, 26 | "protocol": "stlink", 27 | "protocols": [ 28 | "stlink" 29 | ] 30 | }, 31 | "url": "www.husarion.com", 32 | "vendor": "Husarion" 33 | } 34 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ubuntu:22.04 2 | 3 | RUN apt update && apt install -y \ 4 | python3 \ 5 | python3-pip \ 6 | python3.10-venv \ 7 | git \ 8 | cmake 9 | 10 | RUN pip3 install --upgrade pip setuptools 11 | 12 | RUN pip3 install -U platformio==6.1.11 13 | 14 | WORKDIR /app 15 | 16 | COPY . . 17 | 18 | ENV USER=root 19 | 20 | RUN git config --global --add safe.directory /app && \ 21 | git config --global --add safe.directory /app/lib/encoder-mbed && \ 22 | git config --global --add safe.directory /app/lib/imu-driver && \ 23 | git config --global --add safe.directory /app/lib/motor-driver-mbed && \ 24 | git config --global --add safe.directory /app/lib/vl53l0x-mbed && \ 25 | git submodule update --init --recursive && \ 26 | pio lib install 27 | 28 | RUN python3 -m venv /root/.platformio/penv/bin/activate 29 | 30 | CMD pio run -------------------------------------------------------------------------------- /platformio.ini: -------------------------------------------------------------------------------- 1 | [env:core2] 2 | platform = ststm32@15.4.1 3 | ; platform = ststm32@17.6.0 4 | framework = mbed 5 | board = core2 6 | 7 | build_flags = 8 | -I$PROJECTSRC_DIR/TARGET_CORE2 9 | -D PIO_FRAMEWORK_MBED_RTOS_PRESENT 10 | -D MBED_BUILD_PROFILE_RELEASE 11 | -D MBED_APP_CONFIG_FILE=mbed_app.json 12 | 13 | platform_packages = 14 | framework-mbed @ ~6.51506.0 15 | ; toolchain-gccarmnoneeabi @ 1.100301.220327 16 | ; toolchain-gccarmnoneeabi @ 1.120301.0 17 | 18 | extra_scripts = 19 | post:gen_hex.py 20 | pre:ignore_packages.py 21 | 22 | lib_deps = 23 | ; https://github.com/micro-ROS/micro_ros_platformio 24 | https://github.com/husarion/micro_ros_platformio 25 | lib_compat_mode = off 26 | 27 | board_microros_distro = humble 28 | board_microros_transport = custom 29 | board_microros_user_meta = colcon.meta 30 | 31 | upload_protocol = stlink 32 | debug_tool = stlink -------------------------------------------------------------------------------- /include/battery.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | constexpr int MEASUREMENT_SERIES = 10; 10 | constexpr float BATTERY_VOLTAGE_LOW = 10.8; 11 | 12 | enum 13 | { 14 | BATTERY_LOW = 1, 15 | BATTERY_OK = 0 16 | }; 17 | 18 | struct BatteryData 19 | { 20 | float voltage; 21 | float threshold; 22 | uint8_t status; 23 | }; 24 | 25 | static BatteryData battery_data = { 0.0, BATTERY_VOLTAGE_LOW, BATTERY_OK }; 26 | static DigitalOut battery_led(LED1, 1); 27 | static Ticker battery_led_flipper; 28 | static AnalogIn battery_adc(BAT_MEAS); 29 | 30 | extern sensor_msgs__msg__BatteryState battery_msg; 31 | static rtos::Thread read_battery_voltage_and_show_status_thead; 32 | 33 | void init_battery(); 34 | float read_battery_voltage_and_show_status(); 35 | void read_battery_voltage_and_show_status_task(); 36 | void fill_battery_msg(sensor_msgs__msg__BatteryState *msg); -------------------------------------------------------------------------------- /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "name": "PlatformIO in Devcontainer", 3 | "dockerFile": "Dockerfile", 4 | 5 | // Set *default* container specific settings.json values on container create. 6 | "settings": { 7 | "terminal.integrated.shell.linux": "/bin/bash" 8 | }, 9 | 10 | // Add the IDs of extensions you want installed when the container is created. 11 | "extensions": ["ms-vscode.cpptools", "platformio.platformio-ide", "eamodio.gitlens"], 12 | 13 | "forwardPorts": [ 14 | // PIO Home is blank if its port isn't exosed from within the container. 15 | 8008 16 | ], 17 | 18 | "runArgs": [ 19 | // --privileged grants access to /dev on host. Arduino will most likely be /dev/ttysN 20 | "--privileged", 21 | ], 22 | 23 | "mounts": [ 24 | // Mount the host's device to this container's. 25 | "source=/dev,target=/dev,type=bind,consistency=consistent" 26 | ], 27 | "workspaceMount": "src=${localWorkspaceFolder}/,dst=/home/runner/rosbot_ros2_firmware/,type=bind", 28 | "workspaceFolder": "/home/runner/rosbot_ros2_firmware/", 29 | "remoteUser": "runner" 30 | } -------------------------------------------------------------------------------- /lib/RosbotDrive/internal/rosbot-regulator/RosbotRegulator.h: -------------------------------------------------------------------------------- 1 | /** @file RosbotRegulator.h 2 | * Regulator abstract class. 3 | */ 4 | #ifndef __ROSBOT_REGULATOR_H__ 5 | #define __ROSBOT_REGULATOR_H__ 6 | 7 | struct RosbotRegulator_params 8 | { 9 | float kp; 10 | float ki; 11 | float kd; 12 | float out_min; 13 | float out_max; 14 | float a_max; 15 | float speed_max; 16 | unsigned int dt_ms; 17 | }; 18 | 19 | class RosbotRegulator 20 | { 21 | public: 22 | RosbotRegulator(const RosbotRegulator_params ¶ms) 23 | : _params(params) 24 | {} 25 | 26 | virtual ~RosbotRegulator(){}; 27 | 28 | virtual void updateParams(const RosbotRegulator_params ¶ms)=0; 29 | 30 | virtual void getParams(RosbotRegulator_params ¶ms)=0; 31 | 32 | virtual float updateState(float setpoint, float feedback)=0; 33 | 34 | virtual void reset()=0; 35 | 36 | virtual float getPidout()=0; 37 | 38 | virtual float getError()=0; 39 | 40 | protected: 41 | RosbotRegulator_params _params; 42 | }; 43 | 44 | #endif /* __ROSBOT_REGULATOR_H__ */ -------------------------------------------------------------------------------- /custom_targets.json: -------------------------------------------------------------------------------- 1 | { 2 | "CORE2": { 3 | "inherits": ["FAMILY_STM32"], 4 | "core": "Cortex-M4F", 5 | "extra_labels_add": ["STM32F4", "STM32F407", "STM32F407xG", "STM32F407ZG"], 6 | "supported_toolchains": ["GCC_ARM"], 7 | "config": { 8 | "clock_source": { 9 | "help": "Mask value : USE_PLL_HSE_EXTC (need HW patch) | USE_PLL_HSE_XTAL | USE_PLL_HSI", 10 | "value": "USE_PLL_HSE_XTAL", 11 | "macro_name": "CLOCK_SOURCE" 12 | } 13 | }, 14 | "macros_add": [ 15 | "USB_STM_HAL", 16 | "INITIAL_SP=0x20020000UL", 17 | "ENCODER_1=TIM2", 18 | "ENCODER_2=TIM8", 19 | "ENCODER_3=TIM3", 20 | "ENCODER_4=TIM4", 21 | "UPPER_RESISTOR=5.6e4", 22 | "LOWER_RESISTOR=1.0e4", 23 | "VIN_MEAS_CORRECTION=0.986" 24 | ], 25 | "overrides": {"lse_available": 0}, 26 | "device_has_add": ["ANALOGOUT", "TRNG", "FLASH", "MPU", "SERIAL_ASYNCH", "CAN"], 27 | "release_versions": ["5"], 28 | "device_name": "STM32F407ZG", 29 | "bootloader_supported": true 30 | } 31 | } -------------------------------------------------------------------------------- /src/ranges.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void init_ranges() 4 | { 5 | MultiDistanceSensor& distance_sensors = MultiDistanceSensor::getInstance(); 6 | if (distance_sensors.init() > 0) 7 | { 8 | uint8_t* data = distance_sensor_commands.alloc(); 9 | *data = 1; 10 | distance_sensor_commands.put(data); 11 | } 12 | 13 | for (auto i = 0u; i < RANGES_COUNT; ++i) 14 | { 15 | fill_range_msg(&range_msgs[i], i); 16 | } 17 | } 18 | 19 | void fill_range_msg(sensor_msgs__msg__Range* msg, uint8_t id) 20 | { 21 | msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, range_frame_names[id]); 22 | 23 | if (rmw_uros_epoch_synchronized()) 24 | { 25 | msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); 26 | msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); 27 | } 28 | 29 | msg->radiation_type = sensor_msgs__msg__Range__INFRARED; 30 | msg->field_of_view = 0.26; 31 | msg->min_range = 0.01; 32 | msg->max_range = 0.90; 33 | } 34 | 35 | void fill_range_msg_with_measurements(sensor_msgs__msg__Range* msg, float range) 36 | { 37 | msg->range = range; 38 | if (msg->range > msg->max_range || msg->range < msg->min_range) 39 | { 40 | msg->range = NAN; 41 | } 42 | } -------------------------------------------------------------------------------- /.devcontainer/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM python:3.9-slim AS stm32flash_builder 2 | 3 | 4 | ARG USERNAME=runner 5 | ENV USER=runner 6 | ARG USER_UID=1001 7 | ARG USER_GID=$USER_UID 8 | 9 | # Create the user 10 | RUN groupadd --gid $USER_GID $USERNAME \ 11 | && useradd --uid $USER_UID --gid $USER_GID -m $USERNAME \ 12 | # 13 | # [Optional] Add sudo support. Omit if you don't need to install software after connecting. 14 | && apt-get update \ 15 | && apt-get install -y sudo \ 16 | && echo $USERNAME ALL=\(root\) NOPASSWD:ALL > /etc/sudoers.d/$USERNAME \ 17 | && chmod 0440 /etc/sudoers.d/$USERNAME 18 | 19 | RUN sudo apt-get update && \ 20 | sudo apt-get install --no-install-recommends -y \ 21 | build-essential sudo git cmake gdb clang clang-format cppcheck bash-completion python3-pip curl gcc-arm-none-eabi && \ 22 | python3 -c "$(curl -fsSL https://raw.githubusercontent.com/platformio/platformio/master/scripts/get-platformio.py)" && \ 23 | ln -s /home/runner/.platformio/penv/bin/platformio /bin/pio && \ 24 | pip install -U colcon-common-extensions lark 25 | 26 | # Build core2-flasher 27 | RUN git clone https://github.com/husarion/flasher && \ 28 | cd flasher && git submodule update --init --recursive && \ 29 | mkdir build && cd build && cmake .. && make && \ 30 | ln -s /flasher/build/core2-flasher /bin/core2-flasher 31 | 32 | # Build stm32 flasher 33 | RUN git clone https://github.com/husarion/stm32loader && \ 34 | cd stm32loader && sudo python setup.py install 35 | 36 | SHELL ["/bin/bash", "-c"] 37 | -------------------------------------------------------------------------------- /src/microros_transport/mbed_serial_transport.cpp: -------------------------------------------------------------------------------- 1 | #include "microros_transport/mbed_serial_transport.hpp" 2 | 3 | #include 4 | #include 5 | 6 | extern "C" { 7 | 8 | static bool serial_transport_open(struct uxrCustomTransport *transport) { 9 | mbed::UARTSerial *serial = (mbed::UARTSerial *)transport->args; 10 | serial->set_flow_control(mbed::SerialBase::Disabled); 11 | serial->set_blocking(false); 12 | return true; 13 | } 14 | 15 | static bool serial_transport_close(struct uxrCustomTransport *transport) { 16 | return true; 17 | } 18 | 19 | static size_t serial_transport_write(struct uxrCustomTransport *transport, 20 | const uint8_t *buf, size_t len, 21 | uint8_t *err) { 22 | mbed::UARTSerial *serial = (mbed::UARTSerial *)transport->args; 23 | return serial->write(buf, len); 24 | } 25 | 26 | static size_t serial_transport_read(struct uxrCustomTransport *transport, 27 | uint8_t *buf, size_t len, int timeout, 28 | uint8_t *err) { 29 | mbed::UARTSerial *serial = (mbed::UARTSerial *)transport->args; 30 | return serial->read(buf, len); 31 | } 32 | } 33 | 34 | void set_microros_serial_transports(mbed::UARTSerial *serial) { 35 | rmw_uros_set_custom_transport(true, serial, serial_transport_open, 36 | serial_transport_close, serial_transport_write, 37 | serial_transport_read); 38 | } -------------------------------------------------------------------------------- /lib/MultiDistanceSensor/MultiDistanceSensor.h: -------------------------------------------------------------------------------- 1 | #ifndef __MULTI_DISTANCE_SENSOR_H__ 2 | #define __MULTI_DISTANCE_SENSOR_H__ 3 | 4 | #include 5 | 6 | #define NUM_DISTANCE_SENSORS 4 7 | #define DISTANCE_SENSORS_DEFAULT_I2C_FREQ 100000U 8 | 9 | enum SensorSelector : uint8_t 10 | { 11 | SENSOR_FR = 0, 12 | SENSOR_FL = 1, 13 | SENSOR_RR = 2, 14 | SENSOR_RL = 3 15 | }; 16 | 17 | struct SensorsMeasurement 18 | { 19 | float range[4]; 20 | uint32_t timestamp; 21 | uint8_t status; 22 | }; 23 | 24 | extern Mail distance_sensor_mail_box; 25 | extern Mail distance_sensor_commands; 26 | 27 | class MultiDistanceSensor : NonCopyable 28 | { 29 | 30 | public: 31 | enum : int 32 | { 33 | ERR_NONE = 0, 34 | ERR_BUSSY = 1, 35 | ERR_I2C_FAILURE = 2, 36 | ERR_NOT_READY = 3, 37 | ERR_NOT_INIT = 4 38 | }; 39 | static MultiDistanceSensor & getInstance(); 40 | int init(); 41 | 42 | private: 43 | static MultiDistanceSensor * _instance; 44 | MultiDistanceSensor(); 45 | ~MultiDistanceSensor(); 46 | void runMeasurement(); 47 | void start(); 48 | void stop(); 49 | int restart(); 50 | void sensors_loop(); 51 | void processOut(); 52 | 53 | I2C * _i2c; 54 | DigitalInOut * _xshout[NUM_DISTANCE_SENSORS]; 55 | VL53L0X * _sensor[NUM_DISTANCE_SENSORS]; 56 | bool _is_active[NUM_DISTANCE_SENSORS]; 57 | bool _initialized; 58 | bool _sensors_enabled; 59 | int _last_sensor_index; 60 | SensorsMeasurement _m; 61 | Thread _distance_sensor_thread; 62 | }; 63 | 64 | #endif -------------------------------------------------------------------------------- /include/servos.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | static std::map servo_voltage_configuration{ 8 | {5.0, 0}, 9 | {6.0, 1}, 10 | {7.4, 2}, 11 | {8.6, 3} 12 | }; 13 | 14 | enum Servos{ 15 | servo0, 16 | servo1, 17 | servo2, 18 | servo3, 19 | servo4, 20 | servo5, 21 | SERVOS_COUNT 22 | }; 23 | 24 | extern std_msgs__msg__UInt32MultiArray servos_command_msg; 25 | 26 | void init_servos(); 27 | void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray *msg); 28 | void servos_command_callback(const void* message); 29 | 30 | namespace rosbot_sensors { 31 | 32 | class ServoManger : NonCopyable 33 | { 34 | public: 35 | enum : int 36 | { 37 | SERVO_OUTPUT_1 = 0, 38 | SERVO_OUTPUT_2 = 1, 39 | SERVO_OUTPUT_3 = 2, 40 | SERVO_OUTPUT_4 = 3, 41 | SERVO_OUTPUT_5 = 4, 42 | SERVO_OUTPUT_6 = 5, 43 | VOLTAGE_5V = 0, 44 | VOLTAGE_6V = 1, 45 | VOLTAGE_7_4V = 2, 46 | VOLTAGE_8_6V = 3, 47 | }; 48 | 49 | ServoManger(); 50 | 51 | void enablePower(bool en = true); 52 | int getEnabledOutputs(); 53 | PwmOut* getOutput(int output); 54 | bool setWidth(int output, int width_us); 55 | bool setPeriod(int output, int period_us); 56 | void enableOutput(int output, bool en = true); 57 | void setPowerMode(int mode); 58 | 59 | private: 60 | PwmOut* _servo[6]; 61 | int _voltage_mode; 62 | int _enabled_outputs; 63 | DigitalOut _servo_sel1; 64 | DigitalOut _servo_sel2; 65 | DigitalOut _servo_power; 66 | }; 67 | 68 | } // namespace rosbot_sensors 69 | 70 | extern rosbot_sensors::ServoManger servo_manager; -------------------------------------------------------------------------------- /src/battery.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | sensor_msgs__msg__BatteryState battery_msg; 4 | 5 | void init_battery() 6 | { 7 | read_battery_voltage_and_show_status_thead.start(read_battery_voltage_and_show_status_task); 8 | fill_battery_msg(&battery_msg); 9 | } 10 | 11 | void read_battery_voltage_and_show_status_task() 12 | { 13 | while (true) 14 | { 15 | battery_msg.voltage = read_battery_voltage_and_show_status(); 16 | ThisThread::sleep_for(100); 17 | } 18 | } 19 | 20 | static void toggle_battery_led() 21 | { 22 | battery_led = !battery_led; 23 | } 24 | 25 | float read_battery_voltage_and_show_status() 26 | { 27 | static int index = 0; 28 | battery_data.voltage = 29 | 3.3f * VIN_MEAS_CORRECTION * (UPPER_RESISTOR + LOWER_RESISTOR) / LOWER_RESISTOR * battery_adc.read(); 30 | if (battery_data.threshold > battery_data.voltage && index < MEASUREMENT_SERIES) // low level 31 | index++; 32 | else if (battery_data.threshold < battery_data.voltage && index > 0) 33 | index--; 34 | 35 | if (battery_data.status == BATTERY_OK && index == MEASUREMENT_SERIES) 36 | { 37 | battery_data.status = BATTERY_LOW; 38 | battery_led = 0; 39 | battery_led_flipper.attach(callback(toggle_battery_led), 0.4); 40 | } 41 | else if (battery_data.status == BATTERY_LOW && index == 0) 42 | { 43 | battery_data.status = BATTERY_OK; 44 | battery_led_flipper.detach(); 45 | battery_led = 1; 46 | } 47 | return battery_data.voltage; 48 | } 49 | 50 | void fill_battery_msg(sensor_msgs__msg__BatteryState *msg) { 51 | msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "battery"); 52 | 53 | if (rmw_uros_epoch_synchronized()) { 54 | msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); 55 | msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); 56 | } 57 | 58 | msg->power_supply_technology = sensor_msgs__msg__BatteryState__POWER_SUPPLY_TECHNOLOGY_LION; 59 | } -------------------------------------------------------------------------------- /include/memory_debug_message_info.hpp: -------------------------------------------------------------------------------- 1 | #if defined(MEMORY_DEBUG_INFO) 2 | #define MAX_THREAD_INFO 10 3 | 4 | mbed_stats_heap_t heap_info; 5 | mbed_stats_stack_t stack_info[MAX_THREAD_INFO]; 6 | 7 | int print_debug_info() { 8 | debug("\nThis message is from debug function"); 9 | debug_if(1, "\nThis message is from debug_if function"); 10 | debug_if(0, "\nSOMETHING WRONG!!! This message from debug_if function shouldn't show on bash"); 11 | 12 | printf("\nMemoryStats:"); 13 | mbed_stats_heap_get(&heap_info); 14 | printf("\n\tBytes allocated currently: %d", heap_info.current_size); 15 | printf("\n\tMax bytes allocated at a given time: %d", heap_info.max_size); 16 | printf("\n\tCumulative sum of bytes ever allocated: %d", heap_info.total_size); 17 | printf("\n\tCurrent number of bytes allocated for the heap: %d", heap_info.reserved_size); 18 | printf("\n\tCurrent number of allocations: %d", heap_info.alloc_cnt); 19 | printf("\n\tNumber of failed allocations: %d", heap_info.alloc_fail_cnt); 20 | 21 | mbed_stats_stack_get(&stack_info[0]); 22 | printf("\nCumulative Stack Info:"); 23 | printf("\n\tMaximum number of bytes used on the stack: %d", stack_info[0].max_size); 24 | printf("\n\tCurrent number of bytes allocated for the stack: %d", stack_info[0].reserved_size); 25 | printf("\n\tNumber of stacks stats accumulated in the structure: %d", stack_info[0].stack_cnt); 26 | 27 | mbed_stats_stack_get_each(stack_info, MAX_THREAD_INFO); 28 | printf("\nThread Stack Info:"); 29 | for (int i = 0; i < MAX_THREAD_INFO; i++) { 30 | if (stack_info[i].thread_id != 0) { 31 | printf("\n\tThread: %d", i); 32 | printf("\n\t\tThread Id: 0x%08X", stack_info[i].thread_id); 33 | printf("\n\t\tMaximum number of bytes used on the stack: %d", stack_info[i].max_size); 34 | printf("\n\t\tCurrent number of bytes allocated for the stack: %d", stack_info[i].reserved_size); 35 | printf("\n\t\tNumber of stacks stats accumulated in the structure: %d", stack_info[i].stack_cnt); 36 | } 37 | } 38 | 39 | printf("\nDone...\n\n"); 40 | } 41 | #endif /* MEMORY_DEBUG_INFO */ 42 | -------------------------------------------------------------------------------- /src/imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void init_imu(mbed::I2C* i2c_ptr) 4 | { 5 | ImuDriver::Type type = ImuDriver::getType(i2c_ptr, 2); 6 | sprintf(imu_description_string, "Detected sensor: %s\r\n", imu_sensor_type_string[type]); 7 | 8 | if (type != ImuDriver::UNKNOWN) 9 | { 10 | imu_driver_ptr = new ImuDriver(i2c_ptr, type); 11 | imu_driver_ptr->init(); 12 | imu_driver_ptr->start(); 13 | } 14 | 15 | fill_imu_msg(&imu_msg); 16 | } 17 | 18 | void fill_imu_msg(sensor_msgs__msg__Imu* msg) 19 | { 20 | msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "imu"); 21 | 22 | if (rmw_uros_epoch_synchronized()) 23 | { 24 | msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); 25 | msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); 26 | } 27 | msg->orientation.x = 0; 28 | msg->orientation.y = 0; 29 | msg->orientation.z = 0; 30 | msg->orientation.w = 1; 31 | msg->angular_velocity.x = 0; 32 | msg->angular_velocity.y = 0; 33 | msg->angular_velocity.z = 0; 34 | msg->linear_acceleration.x = 0; 35 | msg->linear_acceleration.y = 0; 36 | msg->linear_acceleration.z = 0; 37 | for (auto i = 0u; i < 9u; ++i) 38 | { 39 | msg->angular_velocity_covariance[i] = msg->linear_acceleration_covariance[i] = msg->orientation_covariance[i] = 0.0; 40 | } 41 | msg->orientation_covariance[9] = 0.0; 42 | msg->orientation_covariance[10] = 0.0; 43 | msg->orientation_covariance[11] = 0.0; 44 | } 45 | 46 | void fill_imu_msg_with_measurements(sensor_msgs__msg__Imu* msg, ImuDriver::ImuMeasurement* measurements) 47 | { 48 | msg->orientation.y = measurements->orientation[1]; 49 | msg->orientation.z = measurements->orientation[2]; 50 | msg->orientation.x = measurements->orientation[0]; 51 | msg->orientation.w = measurements->orientation[3]; 52 | 53 | msg->angular_velocity.x = measurements->angular_velocity[0]; 54 | msg->angular_velocity.y = measurements->angular_velocity[1]; 55 | msg->angular_velocity.z = measurements->angular_velocity[2]; 56 | 57 | msg->linear_acceleration.x = measurements->linear_acceleration[0]; 58 | msg->linear_acceleration.y = measurements->linear_acceleration[1]; 59 | msg->linear_acceleration.z = measurements->linear_acceleration[2]; 60 | } -------------------------------------------------------------------------------- /.github/workflows/firmware_release.yml: -------------------------------------------------------------------------------- 1 | name: Firmware release 2 | on: 3 | pull_request: 4 | branches: main 5 | types: [closed] 6 | 7 | jobs: 8 | get-bump: 9 | name: Get version bump 10 | runs-on: ubuntu-latest 11 | outputs: 12 | bump: ${{ steps.get-version-bump.outputs.bump }} 13 | steps: 14 | - if: github.event.pull_request.merged == true 15 | name: Get version bump 16 | id: get-version-bump 17 | uses: husarion-ci/action-get-version-bump@v0.3.0 18 | 19 | bump-version: 20 | name: Bump version 21 | runs-on: ubuntu-latest 22 | needs: get-bump 23 | if: needs.get-bump.outputs.bump != 'none' 24 | outputs: 25 | new_version: ${{ steps.bump-semver.outputs.new_version }} 26 | steps: 27 | - uses: actions/checkout@v3 28 | with: 29 | fetch-depth: 0 30 | 31 | - uses: actions-ecosystem/action-get-latest-tag@v1 32 | id: get-latest-tag 33 | with: 34 | with_initial_version: false 35 | semver_only: true 36 | 37 | - uses: actions-ecosystem/action-bump-semver@v1 38 | id: bump-semver 39 | with: 40 | current_version: ${{ steps.get-latest-tag.outputs.tag }} 41 | level: ${{ needs.get-bump.outputs.bump }} 42 | 43 | - name: Create tag 44 | run: git tag ${{ steps.bump-semver.outputs.new_version }} 45 | 46 | - name: Publish tags 47 | run: git push --tags 48 | 49 | firmware-release: 50 | name: Firmware release 51 | needs: bump-version 52 | runs-on: ubuntu-22.04 53 | steps: 54 | - name: Checkout 55 | uses: actions/checkout@v4 56 | with: 57 | submodules: true 58 | 59 | # - name: Installing platformio 60 | # run: pip3 install -U platformio 61 | 62 | # - name: Building a firmware for STM32 63 | # run: git submodule update --init --recursive && pio run 64 | 65 | - name: Building a firmware for STM32 66 | run: | 67 | set -e 68 | docker compose build --no-cache 69 | docker compose up --exit-code-from pio 70 | 71 | - name: Release a firmware 72 | uses: softprops/action-gh-release@v1 73 | with: 74 | files: | 75 | .pio/build/core2/firmware.bin 76 | .pio/build/core2/firmware.hex 77 | CHANGELOG.md 78 | LICENSE 79 | tag_name: ${{ needs.bump-version.outputs.new_version }} -------------------------------------------------------------------------------- /include/wheels.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | // Motors setup 13 | #define MOTOR_FR MOTOR1 14 | #define MOTOR_FL MOTOR4 15 | #define MOTOR_RR MOTOR2 16 | #define MOTOR_RL MOTOR3 17 | 18 | constexpr const char* FRONT_LEFT_MOTOR_NAME = "fl_wheel_joint"; 19 | constexpr const char* FRONT_RIGHT_MOTOR_NAME = "fr_wheel_joint"; 20 | constexpr const char* REAR_LEFT_MOTOR_NAME = "rl_wheel_joint"; 21 | constexpr const char* REAR_RIGHT_MOTOR_NAME = "rr_wheel_joint"; 22 | 23 | static volatile float last_wheels_speed_calc_time = 0.0; 24 | static volatile uint32_t last_speed_command_time = 0; 25 | static volatile bool is_speed_watchdog_active = false; 26 | static volatile uint64_t speed_watchdog_interval = 1000; // ms 27 | 28 | static mbed::Timer odom_watchdog_timer; 29 | static rtos::Thread wheels_state_read_thread; 30 | 31 | constexpr uint8_t POLARITY = 0b00111100; 32 | constexpr float ROBOT_LENGTH = 0.197; 33 | constexpr uint8_t ENCODER_CPR = 48; 34 | constexpr float ROBOT_LENGTH_HALF = ROBOT_LENGTH / 2.0; 35 | constexpr float DISTANCE_FRONT_TO_REAR_WHEEL = 0.11; 36 | constexpr float WHEEL_SEPARATION_LENGTH = DISTANCE_FRONT_TO_REAR_WHEEL / 2; 37 | constexpr float ROBOT_WIDTH = 0.215; // 0.22 0.195 38 | constexpr float ROBOT_WIDTH_HALF = ROBOT_WIDTH / 2.0; 39 | constexpr float DIAMETER_MODIFICATOR = 1.106; // 1.24, 1.09, 1.164 40 | constexpr float TYRE_DEFLATION = 1.042; // theoretical distance / real distance 41 | constexpr float GEAR_RATIO = 34.014; 42 | constexpr float WHEEL_DIAMETER = 0.085; 43 | constexpr float WHEEL_RADIUS = WHEEL_DIAMETER / 2.0; 44 | 45 | static sensor_msgs__msg__JointState wheels_state_msg; 46 | extern std_msgs__msg__Float32MultiArray wheels_command_msg; 47 | 48 | enum Motors 49 | { 50 | motor_right_rear, 51 | motor_left_rear, 52 | motor_right_front, 53 | motor_left_front, 54 | MOTORS_COUNT 55 | }; 56 | 57 | enum MotorsState 58 | { 59 | motor_state_position, 60 | motor_state_velocity, 61 | motor_state_effort, 62 | MOTORS_STATE_COUNT 63 | }; 64 | 65 | void init_wheels(); 66 | void update_wheels_states(); 67 | void check_speed_watchdog(); 68 | void wheels_state_read_and_watchdog_task(); 69 | 70 | void wheels_command_callback(const void* message); 71 | 72 | void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray *msg); 73 | void fill_wheels_state_msg(sensor_msgs__msg__JointState* msg); -------------------------------------------------------------------------------- /ROS_API.md: -------------------------------------------------------------------------------- 1 | Use `micro_ros_agent` to communicate with all firmware functionalities. 2 | ```bash 3 | ros2 run micro_ros_agent micro_ros_agent serial -D $SERIAL_PORT serial -b 576000 4 | ``` 5 | - `rosbot_ros2_firmware` it is a micro-ROS node on CORE2 inside ROSbot 2R, 2 PRO, 2. It is used to publish all the sensor data such as wheels positions, IMU measurements, battery level and buttons states from firmware to ROS2 and also to subscribe command values such as motors speeds, servos periods, servos parameters and LEDs states. 6 | **Subscribes** 7 | - `/cmd_ser` (_std_msgs/msg/UInt32MultiArray[6]_) 8 | - `/led/left` (_std_msgs/msg/Bool_) 9 | - `/led/right` (_std_msgs/msg/Bool_) 10 | - `/_motors_cmd` (_std_msgs/msg/Float32MultiArray[4]_) 11 | 12 | **Publishes** 13 | - `/_motors_response` (_*sensor_msgs/msg/JointState_) 14 | - `/_imu/data_raw` (_sensor_msgs/msg/Imu_) 15 | - `/battery` (_sensor_msgs/BatteryState_) 16 | - `/range/fr` (_sensor_msgs/msg/Range_) 17 | - `/range/fl` (_sensor_msgs/msg/Range_) 18 | - `/range/rr` (_sensor_msgs/msg/Range_) 19 | - `/range/rl` (_sensor_msgs/msg/Range_) 20 | - `/button/left` (_std_msgs/msg/Bool_) 21 | - `/button/right` (_std_msgs/msg/Bool_) 22 | 23 | **Parameters** 24 | - `servo_enable_power` (_Bool_) 25 | - `servo_voltage` (_Double_): 26 | - `5.0`V 27 | - `6.0`V 28 | - `7.4`V 29 | - `8.6`V 30 | - `servo[0...5]_enable` (*Bool_) e.g. `servo2_enable` 31 | - `servo[0...5]_period` (*UInt32_) e.g. `servo2_period` 32 | 33 | #### Command line examples 34 | - **Motors driving (e.g. go forward)** 35 | ```bash 36 | ros2 topic pub /_motors_cmd std_msgs/msg/Float32MultiArray "data: [1.0, 1.0, 1.0, 1.0]" 37 | ``` 38 | 39 | - **Servos steering** 40 | ```bash 41 | # Choose power supply voltage for the servos e.g. 5.0V 42 | ros2 param set /rosbot_ros2_firmware servo_voltage 5.0 43 | 44 | # Enable power for the servos 45 | ros2 param set /rosbot_ros2_firmware servo_enable_power true 46 | 47 | # Set the control period in microseconds e.g. 20 000us for the servo5 48 | ros2 param set /rosbot_ros2_firmware servo5_period 20000 49 | 50 | # Enable PWM output for the servo e.g. for the servo5 51 | ros2 param set /rosbot_ros2_firmware servo5_enable true 52 | 53 | # Send duty cycle to the servos 54 | ros2 topic pub /cmd_ser std_msgs/msg/UInt32MultiArray "data: [0, 0, 0, 0, 0, 2000]" 55 | 56 | ``` 57 | 58 | - **LED blinking** 59 | ```bash 60 | # Turn on the left LED 61 | ros2 topic pub /led/left std_msgs/msg/Bool "data: true" 62 | 63 | # Turn off the left LED 64 | ros2 topic pub /led/left std_msgs/msg/Bool "data: false" 65 | ``` -------------------------------------------------------------------------------- /include/microros.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | constexpr const uint8_t UXR_CLIENT_DOMAIN_ID_TO_OVERRIDE_WITH_ENV = 255; 22 | 23 | constexpr const char *NODE_NAME = "rosbot_ros2_firmware"; 24 | constexpr const char *IMU_TOPIC_NAME = "_imu/data_raw"; 25 | constexpr const char *WHEELS_STATE_TOPIC_NAME = "_motors_response"; 26 | constexpr const char *WHEELS_COMMAND_TOPIC_NAME = "_motors_cmd"; 27 | constexpr const char *BATTERY_TOPIC_NAME = "battery"; 28 | constexpr const char *SERVOS_COMMAND_TOPIC_NAME = "cmd_ser"; 29 | 30 | constexpr const char *GET_CPU_ID_SERVICE_NAME = "/get_cpu_id"; 31 | 32 | enum AgentStates { 33 | WAITING_AGENT, 34 | AGENT_AVAILABLE, 35 | AGENT_CONNECTED, 36 | AGENT_DISCONNECTED 37 | }; 38 | 39 | bool microros_deinit(); 40 | 41 | #define RCCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){return false;}} 42 | #define RCSOFTCHECK(fn) { rcl_ret_t temp_rc = fn; if((temp_rc != RCL_RET_OK)){led3 = 1;}} 43 | 44 | #define EXECUTE_EVERY_N_MS(MS, X) \ 45 | do { \ 46 | static volatile int64_t init = -1; \ 47 | if (init == -1) { \ 48 | init = uxr_millis(); \ 49 | } \ 50 | if (uxr_millis() - init > MS) { \ 51 | X; \ 52 | init = uxr_millis(); \ 53 | } \ 54 | } while (0) 55 | 56 | void error_loop(); 57 | 58 | bool microros_init(); 59 | bool microros_deinit(); 60 | bool microros_spin(); 61 | 62 | bool init_imu_publisher(); 63 | bool init_wheels_state_publisher(); 64 | bool init_battery_publisher(); 65 | bool init_range_publishers(); 66 | bool init_wheels_command_subscriber(); 67 | bool init_servos_command_subscriber(); 68 | bool init_button_publishers(); 69 | bool init_led_subscribers(); 70 | bool init_param_server(); 71 | bool init_parameters(); 72 | bool init_services(); 73 | 74 | bool publish_imu_msg(sensor_msgs__msg__Imu *imu_msg); 75 | bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg); 76 | bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg); 77 | bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id); 78 | bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id); 79 | 80 | void get_cpu_id_service_callback(const void *request, void *response); -------------------------------------------------------------------------------- /src/TARGET_CORE2/PeripheralNames.h: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | ******************************************************************************* 3 | * Copyright (c) 2014, STMicroelectronics 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 3. Neither the name of STMicroelectronics nor the names of its contributors 15 | * may be used to endorse or promote products derived from this software 16 | * without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************* 29 | */ 30 | #ifndef MBED_PERIPHERALNAMES_H 31 | #define MBED_PERIPHERALNAMES_H 32 | 33 | #include "cmsis.h" 34 | 35 | #ifdef __cplusplus 36 | extern "C" { 37 | #endif 38 | 39 | typedef enum { 40 | ADC_1 = (int)ADC1_BASE, 41 | ADC_2 = (int)ADC2_BASE, 42 | ADC_3 = (int)ADC3_BASE 43 | } ADCName; 44 | 45 | typedef enum { 46 | DAC_0 = 0, 47 | DAC_1 48 | } DACName; 49 | 50 | typedef enum { 51 | UART_1 = (int)USART1_BASE, 52 | UART_2 = (int)USART2_BASE, 53 | UART_3 = (int)USART3_BASE, 54 | UART_4 = (int)UART4_BASE, 55 | UART_5 = (int)UART5_BASE, 56 | UART_6 = (int)USART6_BASE, 57 | } UARTName; 58 | 59 | typedef enum { 60 | SPI_1 = (int)SPI1_BASE, 61 | SPI_2 = (int)SPI2_BASE, 62 | SPI_3 = (int)SPI3_BASE 63 | } SPIName; 64 | 65 | typedef enum { 66 | I2C_1 = (int)I2C1_BASE, 67 | I2C_2 = (int)I2C2_BASE, 68 | I2C_3 = (int)I2C3_BASE 69 | } I2CName; 70 | 71 | typedef enum { 72 | PWM_1 = (int)TIM1_BASE, 73 | PWM_2 = (int)TIM2_BASE, 74 | PWM_3 = (int)TIM3_BASE, 75 | PWM_4 = (int)TIM4_BASE, 76 | PWM_5 = (int)TIM5_BASE, 77 | PWM_8 = (int)TIM8_BASE, 78 | PWM_9 = (int)TIM9_BASE, 79 | PWM_10 = (int)TIM10_BASE, 80 | PWM_11 = (int)TIM11_BASE, 81 | PWM_12 = (int)TIM12_BASE, 82 | PWM_13 = (int)TIM13_BASE, 83 | PWM_14 = (int)TIM14_BASE 84 | } PWMName; 85 | 86 | typedef enum { 87 | CAN_1 = (int)CAN1_BASE, 88 | CAN_2 = (int)CAN2_BASE 89 | } CANName; 90 | 91 | #ifdef __cplusplus 92 | } 93 | #endif 94 | 95 | #endif 96 | -------------------------------------------------------------------------------- /src/servos.cpp: -------------------------------------------------------------------------------- 1 | #include "servos.hpp" 2 | rosbot_sensors::ServoManger servo_manager; 3 | std_msgs__msg__UInt32MultiArray servos_command_msg; 4 | 5 | void init_servos(){ 6 | fill_servos_command_msg(&servos_command_msg); 7 | } 8 | 9 | void fill_servos_command_msg(std_msgs__msg__UInt32MultiArray* msg) 10 | { 11 | static uint32_t data[SERVOS_COUNT] = { 0, 0, 0, 0, 0, 0 }; 12 | msg->data.capacity = SERVOS_COUNT; 13 | msg->data.size = 0; 14 | msg->data.data = (uint32_t*)data; 15 | } 16 | 17 | void servos_command_callback(const void* message) 18 | { 19 | const std_msgs__msg__UInt32MultiArray* msg = (const std_msgs__msg__UInt32MultiArray*)message; 20 | if (msg != NULL and msg->data.size == SERVOS_COUNT) 21 | { 22 | for (auto i = 0u; i < SERVOS_COUNT; ++i) 23 | { 24 | servo_manager.setWidth(i, msg->data.data[i]); 25 | } 26 | } 27 | } 28 | 29 | namespace rosbot_sensors 30 | { 31 | 32 | ServoManger::ServoManger() 33 | : _servo{ nullptr, nullptr, nullptr, nullptr, nullptr, nullptr } 34 | , _voltage_mode(0) 35 | , _enabled_outputs(0) 36 | , _servo_sel1(SERVO_SEL1, 0) 37 | , _servo_sel2(SERVO_SEL2, 0) 38 | , _servo_power(SERVO_POWER_ON, 0) 39 | { 40 | } 41 | 42 | void ServoManger::enablePower(bool en) 43 | { 44 | _servo_power.write(en ? 1 : 0); 45 | } 46 | 47 | int ServoManger::getEnabledOutputs() 48 | { 49 | return _enabled_outputs; 50 | } 51 | 52 | PwmOut* ServoManger::getOutput(int output) 53 | { 54 | if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) 55 | return nullptr; 56 | 57 | return _servo[output]; 58 | } 59 | 60 | bool ServoManger::setWidth(int output, int width_us) 61 | { 62 | if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) 63 | return false; 64 | 65 | if (_servo[output] == nullptr) 66 | return false; 67 | 68 | _servo[output]->pulsewidth_us(width_us); 69 | return true; 70 | } 71 | 72 | bool ServoManger::setPeriod(int output, int period_us) 73 | { 74 | if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) 75 | return false; 76 | 77 | if (_servo[output] == nullptr) 78 | return false; 79 | 80 | _servo[output]->period_us(period_us); 81 | return true; 82 | } 83 | 84 | void ServoManger::enableOutput(int output, bool en) 85 | { 86 | if (output < SERVO_OUTPUT_1 || output > SERVO_OUTPUT_6) 87 | return; 88 | 89 | if (en && _servo[output] == nullptr) 90 | { 91 | switch (output) 92 | { 93 | case SERVO_OUTPUT_1: 94 | _servo[output] = new PwmOut(SERVO1_PWM); 95 | break; 96 | case SERVO_OUTPUT_2: 97 | _servo[output] = new PwmOut(SERVO2_PWM); 98 | break; 99 | case SERVO_OUTPUT_3: 100 | _servo[output] = new PwmOut(SERVO3_PWM); 101 | break; 102 | case SERVO_OUTPUT_4: 103 | _servo[output] = new PwmOut(SERVO4_PWM); 104 | break; 105 | case SERVO_OUTPUT_5: 106 | _servo[output] = new PwmOut(SERVO5_PWM_ALT1); 107 | break; 108 | case SERVO_OUTPUT_6: 109 | _servo[output] = new PwmOut(SERVO6_PWM_ALT1); 110 | break; 111 | } 112 | _enabled_outputs++; 113 | } 114 | else if (_servo[output] != nullptr && !en) 115 | { 116 | delete _servo[output]; 117 | _servo[output] = nullptr; 118 | _enabled_outputs--; 119 | } 120 | } 121 | 122 | void ServoManger::setPowerMode(int mode) 123 | { 124 | _servo_sel1.write(mode & 0x01L); 125 | _servo_sel2.write(mode & 0x02L); 126 | } 127 | 128 | } // namespace rosbot_sensors 129 | -------------------------------------------------------------------------------- /lib/RosbotDrive/RosbotDrive.h: -------------------------------------------------------------------------------- 1 | /** @file RosbotDrive.h 2 | * Definitions of RosbotDrive class and accompany structures. 3 | */ 4 | #ifndef __ROSBOT_DRIVE_H__ 5 | #define __ROSBOT_DRIVE_H__ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | /** 13 | * @brief Rosbot Motor Internal Number. 14 | * 15 | * MOTOR1 and MOTOR2 tags identify driver 1 motor 1 and 2 outputs. 16 | * MOTOR3 and MOTOR4 tags identify driver 2 motor 1 and 2 outputs. 17 | */ 18 | enum RosbotMotNum : uint8_t 19 | { 20 | MOTOR1 = 0, ///< Driver 1 motor 1 21 | MOTOR2 = 1, ///< Driver 1 motor 2 22 | MOTOR3 = 2, ///< Driver 2 motor 1 23 | MOTOR4 = 3 ///< Driver 2 motor 2 24 | }; 25 | 26 | enum SpeedMode 27 | { 28 | TICSKPS, 29 | MPS, 30 | DUTY_CYCLE 31 | }; 32 | 33 | enum RosbotDriveStates 34 | { 35 | UNINIT, 36 | HALT, 37 | IDLE, 38 | OPERATIONAL, 39 | FAULT 40 | }; 41 | 42 | struct RosbotWheel 43 | { 44 | float radius; 45 | float diameter_modificator; 46 | float tyre_deflation; 47 | float gear_ratio; 48 | uint32_t encoder_cpr; // conuts per revolution 49 | uint8_t polarity; // LSB -> motor, MSB -> encoder 50 | }; 51 | 52 | struct NewTargetSpeed 53 | { 54 | float speed[4]; 55 | SpeedMode mode; 56 | }; 57 | 58 | struct PidDebugData 59 | { 60 | float cspeed; 61 | float tspeed; 62 | float pidout; 63 | float error; 64 | }; 65 | 66 | struct RosbotSpeed 67 | { 68 | float lin_x; 69 | float lin_y; 70 | float ang_z; 71 | }; 72 | 73 | //TODO: documentation 74 | /** 75 | * @brief Rosbot Drive Module. 76 | * 77 | * This class represents the ROSbot drive module. 78 | */ 79 | class RosbotDrive : NonCopyable 80 | { 81 | public: 82 | static const RosbotWheel DEFAULT_WHEEL_PARAMS; /**< Default ROSbot's wheels parameters. */ 83 | 84 | static const RosbotRegulator_params DEFAULT_REGULATOR_PARAMS; /**< Default ROSbot regulator parameters. */ 85 | 86 | static RosbotDrive &getInstance(); 87 | 88 | void init(const RosbotWheel &wheel_params, const RosbotRegulator_params ¶ms); 89 | 90 | void enable(bool en = true); 91 | 92 | void enablePidReg(bool en); 93 | 94 | bool isPidEnabled(); 95 | 96 | void stop(); 97 | 98 | void setupMotorSequence(RosbotMotNum first, RosbotMotNum second, RosbotMotNum third, RosbotMotNum fourth); 99 | 100 | float getSpeed(RosbotMotNum mot_num); 101 | 102 | float getSpeed(RosbotMotNum mot_num, SpeedMode mode); 103 | 104 | float getDistance(RosbotMotNum mot_num); 105 | 106 | float getAngularPos(RosbotMotNum mot_num); 107 | 108 | int32_t getEncoderTicks(RosbotMotNum mot_num); 109 | 110 | void resetDistance(); 111 | 112 | void updateTargetSpeed(const NewTargetSpeed &new_speed); 113 | 114 | void updateWheelCoefficients(const RosbotWheel ¶ms); 115 | 116 | void updatePidParams(const RosbotRegulator_params ¶ms); 117 | 118 | void getPidParams(RosbotRegulator_params ¶ms); 119 | 120 | // void getPidDebugData(PidDebugData * data, RosbotMotNum mot_num); 121 | 122 | private: 123 | static RosbotDrive *_instance; 124 | 125 | RosbotDrive(); 126 | 127 | void regulatorLoop(); 128 | 129 | volatile RosbotDriveStates _state; 130 | volatile bool _regulator_output_enabled; 131 | volatile bool _regulator_loop_enabled; 132 | 133 | RosbotWheel _wheel_params; 134 | 135 | volatile float _tspeed_mps[4]; 136 | volatile float _cspeed_mps[4]; 137 | volatile int32_t _cdistance[4]; 138 | uint8_t _motor_sequence[4]; 139 | 140 | int _regulator_interval_ms; 141 | 142 | float _wheel_coefficient1; 143 | float _wheel_coefficient2; 144 | 145 | DRV8848 *_mot_driver[2]; 146 | DRV8848::DRVMotor *_mot[4]; 147 | Encoder *_encoder[4]; 148 | RosbotRegulator *_regulator[4]; 149 | 150 | Mutex rosbot_drive_mutex; 151 | }; 152 | 153 | #endif /* __ROSBOT_DRIVE_H__ */ -------------------------------------------------------------------------------- /src/wheels.cpp: -------------------------------------------------------------------------------- 1 | #include "wheels.hpp" 2 | 3 | std_msgs__msg__Float32MultiArray wheels_command_msg; 4 | 5 | void init_wheels() 6 | { 7 | odom_watchdog_timer.start(); 8 | last_wheels_speed_calc_time = odom_watchdog_timer.read(); 9 | RosbotWheel custom_wheel_params = { .radius = WHEEL_RADIUS, 10 | .diameter_modificator = DIAMETER_MODIFICATOR, 11 | .tyre_deflation = TYRE_DEFLATION, 12 | .gear_ratio = GEAR_RATIO, 13 | .encoder_cpr = ENCODER_CPR, 14 | .polarity = POLARITY }; 15 | 16 | RosbotDrive& drive = RosbotDrive::getInstance(); 17 | drive.setupMotorSequence(MOTOR_FR, MOTOR_FL, MOTOR_RR, MOTOR_RL); 18 | drive.init(custom_wheel_params, RosbotDrive::DEFAULT_REGULATOR_PARAMS); 19 | drive.enable(true); 20 | drive.enablePidReg(true); 21 | 22 | fill_wheels_state_msg(&wheels_state_msg); 23 | fill_wheels_command_msg(&wheels_command_msg); 24 | wheels_state_read_thread.start(wheels_state_read_and_watchdog_task); 25 | } 26 | 27 | void update_wheels_states() 28 | { 29 | RosbotDrive& drive = RosbotDrive::getInstance(); 30 | 31 | float current_position[MOTORS_COUNT]; 32 | current_position[motor_left_front] = drive.getAngularPos(MOTOR_FL); 33 | current_position[motor_right_front] = drive.getAngularPos(MOTOR_FR); 34 | current_position[motor_left_rear] = drive.getAngularPos(MOTOR_RL); 35 | current_position[motor_right_rear] = drive.getAngularPos(MOTOR_RR); 36 | 37 | const float current_time = odom_watchdog_timer.read(); 38 | const float dt = current_time - last_wheels_speed_calc_time; 39 | last_wheels_speed_calc_time = current_time; 40 | 41 | for (auto i = 0u; i < MOTORS_COUNT; ++i) 42 | { 43 | wheels_state_msg.velocity.data[i] = (current_position[i] - wheels_state_msg.position.data[i]) / dt; 44 | wheels_state_msg.position.data[i] = current_position[i]; 45 | } 46 | } 47 | 48 | void wheels_command_callback(const void* message) 49 | { 50 | const std_msgs__msg__Float32MultiArray* msg = (const std_msgs__msg__Float32MultiArray*)message; 51 | if (msg != NULL and msg->data.size == MOTORS_COUNT) 52 | { 53 | RosbotDrive& drive = RosbotDrive::getInstance(); 54 | NewTargetSpeed new_speed = { 55 | .speed = { 56 | msg->data.data[motor_right_front] * WHEEL_RADIUS, 57 | msg->data.data[motor_right_rear] * WHEEL_RADIUS, 58 | msg->data.data[motor_left_rear] * WHEEL_RADIUS, 59 | msg->data.data[motor_left_front] * WHEEL_RADIUS, 60 | }, 61 | .mode = MPS 62 | }; 63 | 64 | for (auto i = 0u; i < MOTORS_COUNT; ++i) 65 | { 66 | if (isnan(new_speed.speed[i])) 67 | { 68 | float zero_speeds[MOTORS_COUNT] = { 0, 0, 0, 0 }; 69 | memcpy(new_speed.speed, zero_speeds, sizeof(new_speed.speed)); 70 | break; 71 | } 72 | } 73 | 74 | drive.updateTargetSpeed(new_speed); 75 | last_speed_command_time = odom_watchdog_timer.read_ms(); 76 | is_speed_watchdog_active = false; 77 | } 78 | } 79 | 80 | void check_speed_watchdog() 81 | { 82 | if (!is_speed_watchdog_active && (odom_watchdog_timer.read_ms() - last_speed_command_time) > speed_watchdog_interval) 83 | { 84 | RosbotDrive& drive = RosbotDrive::getInstance(); 85 | NewTargetSpeed new_speed = { .speed = { 0.0, 0.0, 0.0, 0.0 }, .mode = MPS }; 86 | drive.updateTargetSpeed(new_speed); 87 | is_speed_watchdog_active = true; 88 | } 89 | } 90 | 91 | void wheels_state_read_and_watchdog_task() 92 | { 93 | while (true) 94 | { 95 | check_speed_watchdog(); 96 | update_wheels_states(); 97 | ThisThread::sleep_for(10); 98 | } 99 | } 100 | 101 | void fill_wheels_state_msg(sensor_msgs__msg__JointState* msg) 102 | { 103 | static double msg_data_tab[MOTORS_STATE_COUNT][MOTORS_COUNT]; 104 | static rosidl_runtime_c__String msg_name_tab[MOTORS_COUNT]; 105 | msg->header.frame_id = micro_ros_string_utilities_set(msg->header.frame_id, "wheels_state"); 106 | 107 | msg->position.data = msg_data_tab[motor_state_position]; 108 | msg->position.capacity = msg->position.size = MOTORS_COUNT; 109 | msg->velocity.data = msg_data_tab[motor_state_velocity]; 110 | msg->velocity.capacity = msg->velocity.size = MOTORS_COUNT; 111 | msg->effort.data = msg_data_tab[motor_state_effort]; 112 | msg->effort.capacity = msg->effort.size = MOTORS_COUNT; 113 | 114 | msg_name_tab->capacity = msg_name_tab->size = MOTORS_COUNT; 115 | msg_name_tab[motor_left_front].data = (char*)FRONT_LEFT_MOTOR_NAME; 116 | msg_name_tab[motor_right_front].data = (char*)FRONT_RIGHT_MOTOR_NAME; 117 | msg_name_tab[motor_left_rear].data = (char*)REAR_LEFT_MOTOR_NAME; 118 | msg_name_tab[motor_right_rear].data = (char*)REAR_RIGHT_MOTOR_NAME; 119 | for (uint8_t i = 0; i < MOTORS_COUNT; i++) 120 | { 121 | msg_name_tab[i].capacity = msg_name_tab[i].size = strlen(msg_name_tab[i].data); 122 | } 123 | msg->name.capacity = msg->name.size = MOTORS_COUNT; 124 | msg->name.data = msg_name_tab; 125 | 126 | if (rmw_uros_epoch_synchronized()) 127 | { 128 | msg->header.stamp.sec = (int32_t)(rmw_uros_epoch_nanos() / 1000000000); 129 | msg->header.stamp.nanosec = (uint32_t)(rmw_uros_epoch_nanos() % 1000000000); 130 | } 131 | } 132 | 133 | void fill_wheels_command_msg(std_msgs__msg__Float32MultiArray* msg) 134 | { 135 | static float data[MOTORS_COUNT] = { 0, 0, 0, 0 }; 136 | msg->data.capacity = MOTORS_COUNT; 137 | msg->data.size = 0; 138 | msg->data.data = (float*)data; 139 | } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rosbot_ros2_firmware 2 | Micro-ROS powered firmware for STM32F4 microcontroller in ROSbot 2. Embedded, low-level firmware for STM32 microcontroller inside ROSbot. Developed using [Mbed Os](https://os.mbed.com/mbed-os/). 3 | 4 | ``` 5 | ______ _____ _____ _ _ __ 6 | | ___ \| _ |/ ___|| | | | / _| 7 | | |_/ /| | | |\ `--. | |__ ___ | |_ | |_ __ __ 8 | | / | | | | `--. \| '_ \ / _ \ | __| | _|\ \ /\ / / 9 | | |\ \ \ \_/ //\__/ /| |_) || (_) || |_ | | \ V V / 10 | \_| \_| \___/ \____/ |_.__/ \___/ \__| |_| \_/\_/ 11 | ``` 12 | **Firmware version:** 13 | 14 | [![rosbot_ros2_firmware](https://img.shields.io/github/v/release/husarion/rosbot_ros2_firmware)](https://github.com/husarion/rosbot_ros2_firmware/releases/latest) 15 | 16 | ## Prerequisites 17 | You need to install following tools: 18 | * [Visual Studio Code IDE](https://code.visualstudio.com/) 19 | 20 | ### Required Visual Studio Code extensions 21 | * [Dev Containers](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) (`ms-vscode-remote.remote-containers`) 22 | 23 | ## Clone the repository 24 | ```bash 25 | git clone https://github.com/husarion/rosbot_ros2_firmware 26 | ``` 27 | 28 | ## Loading dependencies 29 | Unfortunately, PlatformIO has a [bug](https://github.com/platformio/platform-ststm32/issues/491) that impacts libraries configuration. We use git submodules instead. 30 | 31 | > **Git submodules** 32 | > https://git-scm.com/book/en/v2/Git-Tools-Submodules 33 | 34 | To import dependencies, in project's root directory run command: 35 | 36 | ```bash 37 | git submodule update --init --recursive 38 | ``` 39 | 40 | ## Open workspace in Devcontainer 41 | Click the left bottom green button in VS Code and select the `Reopen in Container` option. 42 | 43 | ## Speed up build process 44 | https://docs.platformio.org/en/latest/frameworks/mbed.html#ignoring-particular-components 45 | 46 | In directory `~/.platformio/packages/framework-mbed/features` inside the devcontainer create file called `.mbedignore` with the following content: 47 | 48 | ``` 49 | cellular/* 50 | cryptocell/* 51 | deprecated_warnings/* 52 | lorawan/* 53 | lwipstack/* 54 | nanostack/* 55 | netsocket/* 56 | nfc/* 57 | unsupported/* 58 | ``` 59 | 60 | ## Build firmware 61 | Use `PlatformIO: Build` task or type 62 | ```bash 63 | pio run 64 | ``` 65 | 66 | ## Uploading firmware 67 | 68 | ### Uploading firmware using ST-Link 69 | Use `PlatformIO: Upload` task or type 70 | ```bash 71 | pio run --target upload 72 | ``` 73 | 74 | ### Uploading firmware using `core2-flasher` 75 | You will find `firmware.elf` in `./pio/build/core2`. 76 | 77 | 78 | To flash firmware using `core2-flasher` run: 79 | ``` 80 | core2-flasher .pio/build/core2/firmware.hex 81 | ``` 82 | 83 | ### Uploading firmware using `stm32loader` 84 | https://github.com/husarion/stm32loader 85 | 86 | This tool allows you to upload firmware using RPi connector. 87 | 88 | If you have the bootloader the first two sectors are write protected. Before uploading new firmware you must unlock them (this will erase the bootloader): 89 | 90 | ```bash 91 | $ sudo stm32loader -c -u -W 92 | ``` 93 | 94 | To upload new firmware run: 95 | ```bash 96 | $ sudo stm32loader -c -e -v -w firmware.bin 97 | ``` 98 | 99 | where `` : 100 | * `tinker` for Asus Tinker Board 101 | * `upboard` for Upboard 102 | * `rpi` for Raspberry Pi 103 | 104 | You will find `firmware.bin` in `./pio/build/core2`. 105 | 106 | ## Micro-ROS interface 107 | To start [Micro-ROS](https://micro.ros.org/) communication run: 108 | 109 | ```bash 110 | docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:humble serial -D serial -b 576000 111 | ``` 112 | 113 | ``: 114 | - `/dev/ttyS1` for Asus Tinker Board, 115 | - `/dev/ttyAMA0` for Raspberry Pi 116 | - `/dev/ttyS4` for UpBoard 117 | 118 | The baudrate should be adjusted for SBC you use. The default value for this firmware is `576000`. 119 | 120 | ## ROS2 communication 121 | Look [ROS_API.md](./ROS_API.md) 122 | 123 | ## Development 124 | The easiest way to change the firmware is to work inside a devcontainer. 125 | In VSCode use option `Reopen in Container` from extention: 126 | - [Dev Containers](https://marketplace.visualstudio.com/items?itemName=ms-vscode-remote.remote-containers) (ms-vscode-remote.remote-containers) 127 | 128 | ## Creating a release 129 | 130 | Release is created automatically after PR with bump command is merged to the master branch. Available bump commands are: `bump::major`, `bump::minor` and `bump::patch` (simply add them to the description of your PR). 131 | 132 | ## Versioning 133 | 134 | The project uses [SemVer](http://semver.org/) for versioning. For the versions available, see the [tags on this repository](https://github.com/husarion/rosbot-firmware-new/tags). 135 | 136 | ## Changelog 137 | 138 | See [CHANGELOG.md](CHANGELOG.md). 139 | 140 | ## Starting with Mbed OS 141 | 142 | Documentation: 143 | * [MBED OS Documentation](https://os.mbed.com/docs/v5.14/) 144 | * [MBED OS API Doxygen](https://os.mbed.com/docs/v5.14/mbed-os-api-doxy/modules.html) 145 | 146 | 147 | ## Important dev links (mbed + platformio) 148 | * https://github.com/platformio/platform-ststm32/tree/develop/examples/mbed-legacy-examples/mbed-rtos 149 | * https://github.com/platformio/platform-ststm32/blob/develop/boards/olimex_e407.json 150 | * https://github.com/platformio/platform-ststm32/blob/develop/boards/black_f407zg.json 151 | * https://github.com/ARMmbed/mbed-os/tree/mbed-os-5.15.6/targets/TARGET_STM/TARGET_STM32F4/TARGET_STM32F407xG 152 | * https://docs.platformio.org/en/latest/frameworks/mbed.html 153 | * https://docs.platformio.org/en/latest/platforms/creating_board.html -------------------------------------------------------------------------------- /lib/RosbotDrive/internal/rosbot-regulator/RosbotRegulatorCMSIS.h: -------------------------------------------------------------------------------- 1 | /** @file RosbotRegulatorCMSIS.h 2 | * Implementation of RosbotRegulator. 3 | */ 4 | #ifndef __ROSBOT_REGULATOR_CMSIS_H__ 5 | #define __ROSBOT_REGULATOR_CMSIS_H__ 6 | 7 | #include 8 | #include "RosbotRegulator.h" 9 | #include 10 | 11 | /***************************CMSIS-DSP-PID***************************/ 12 | /* 13 | * Copyright (C) 2010-2017 ARM Limited or its affiliates. All rights reserved. 14 | * 15 | * SPDX-License-Identifier: Apache-2.0 16 | * 17 | * Licensed under the Apache License, Version 2.0 (the License); you may 18 | * not use this file except in compliance with the License. 19 | * You may obtain a copy of the License at 20 | * 21 | * www.apache.org/licenses/LICENSE-2.0 22 | * 23 | * Unless required by applicable law or agreed to in writing, software 24 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 25 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 26 | * See the License for the specific language governing permissions and 27 | * limitations under the License. 28 | */ 29 | #if 0 30 | /** 31 | * @brief Process function for the floating-point PID Control. 32 | * @param[in,out] S is an instance of the floating-point PID Control structure 33 | * @param[in] in input sample to process 34 | * @return out processed output sample. 35 | */ 36 | static __INLINE float32_t arm_pid_f32( 37 | arm_pid_instance_f32 *S, 38 | float32_t in) 39 | { 40 | float32_t out; 41 | 42 | /* y[n] = y[n-1] + A0 * x[n] + A1 * x[n-1] + A2 * x[n-2] */ 43 | out = (S->A0 * in) + 44 | (S->A1 * S->state[0]) + (S->A2 * S->state[1]) + (S->state[2]); 45 | 46 | /* Update state */ 47 | S->state[1] = S->state[0]; 48 | S->state[0] = in; 49 | S->state[2] = out; 50 | 51 | /* return to application */ 52 | return (out); 53 | } 54 | #endif 55 | 56 | void arm_pid_init_f32( 57 | arm_pid_instance_f32 * S, 58 | int32_t resetStateFlag) 59 | { 60 | 61 | /* Derived coefficient A0 */ 62 | S->A0 = S->Kp + S->Ki + S->Kd; 63 | 64 | /* Derived coefficient A1 */ 65 | S->A1 = (-S->Kp) - ((float32_t) 2.0 * S->Kd); 66 | 67 | /* Derived coefficient A2 */ 68 | S->A2 = S->Kd; 69 | 70 | /* Check whether state needs reset or not */ 71 | if(resetStateFlag) 72 | { 73 | /* Clear the state buffer. The size will be always 3 samples */ 74 | memset(S->state, 0, 3u * sizeof(float32_t)); 75 | } 76 | 77 | } 78 | 79 | /** 80 | * @brief Reset function for the floating-point PID Control. 81 | * @param[in] *S Instance pointer of PID control data structure. 82 | * @return none. 83 | * \par Description: 84 | * The function resets the state buffer to zeros. 85 | */ 86 | void arm_pid_reset_f32( 87 | arm_pid_instance_f32 * S) 88 | { 89 | 90 | /* Clear the state buffer. The size will be always 3 samples */ 91 | memset(S->state, 0, 3u * sizeof(float32_t)); 92 | } 93 | /***************************CMSIS-DSP-PID***************************/ 94 | 95 | #define MAX_ACCELERATION 2e-4 96 | 97 | class RosbotRegulatorCMSIS : public RosbotRegulator 98 | { 99 | public: 100 | RosbotRegulatorCMSIS(const RosbotRegulator_params ¶ms) 101 | :RosbotRegulator(params) 102 | , _error(0.0f) 103 | , _pidout(0.0f) 104 | , _vsetpoint(0.0f) 105 | , _speed_step(0.0f) 106 | { 107 | _state.Kp = _params.kp; 108 | _state.Ki = _params.ki; 109 | _state.Kd = _params.kd; 110 | MBED_ASSERT(_params.a_max <= MAX_ACCELERATION); 111 | if(_params.a_max > MAX_ACCELERATION) 112 | { 113 | _speed_step = MAX_ACCELERATION * 1000.0f / _params.dt_ms; 114 | } 115 | else 116 | _speed_step = _params.a_max * 1000.0f / _params.dt_ms; 117 | arm_pid_init_f32(&_state,1); 118 | } 119 | 120 | ~RosbotRegulatorCMSIS(){} 121 | 122 | void updateParams(const RosbotRegulator_params ¶ms) 123 | { 124 | _params = params; 125 | _state.Kp = _params.kp; 126 | _state.Ki = _params.ki; 127 | _state.Kd = _params.kd; 128 | MBED_ASSERT(_params.a_max <= MAX_ACCELERATION); 129 | if(_params.a_max > MAX_ACCELERATION) 130 | { 131 | _speed_step = MAX_ACCELERATION * 1000.0f / _params.dt_ms; 132 | } 133 | else 134 | _speed_step = _params.a_max * 1000.0f / _params.dt_ms; 135 | _speed_step = _params.a_max * 1000.0f / _params.dt_ms; 136 | _vsetpoint = 0; 137 | arm_pid_init_f32(&_state,1); 138 | } 139 | 140 | void getParams(RosbotRegulator_params ¶ms) 141 | { 142 | params = _params; 143 | } 144 | 145 | float updateState(float setpoint, float feedback) 146 | { 147 | 148 | if (fabs(feedback) <= _speed_step && setpoint == 0) 149 | { 150 | arm_pid_reset_f32(&_state); 151 | _pidout =_vsetpoint = 0; 152 | _error = setpoint - feedback; 153 | return 0.0f; 154 | } 155 | 156 | // target speed limit and acceleration limit 157 | float csetpoint = _vsetpoint + copysign(_speed_step, setpoint - _vsetpoint); 158 | if (csetpoint > _params.speed_max) 159 | _vsetpoint = _params.speed_max; 160 | else if (csetpoint < -_params.speed_max) 161 | _vsetpoint = -_params.speed_max; 162 | else if (fabs(csetpoint) <= _speed_step && setpoint == 0) 163 | _vsetpoint = 0; 164 | else 165 | _vsetpoint = csetpoint; 166 | 167 | 168 | _error = _vsetpoint - feedback; 169 | 170 | _pidout = arm_pid_f32(&_state, _error); 171 | 172 | _pidout = (_pidout > _params.out_max ? _params.out_max : (_pidout < _params.out_min ? _params.out_min : _pidout)); 173 | return _pidout; 174 | } 175 | 176 | float getPidout() 177 | { 178 | return _pidout; 179 | } 180 | 181 | float getError() 182 | { 183 | return _error; 184 | } 185 | 186 | void reset() 187 | { 188 | arm_pid_reset_f32(&_state); 189 | _vsetpoint = 0; 190 | } 191 | 192 | private: 193 | arm_pid_instance_f32 _state; 194 | float _error; 195 | float _pidout; 196 | float _vsetpoint; 197 | float _speed_step; 198 | }; 199 | 200 | #endif /* __ROSBOT_REGULATOR_CMSIS_H__ */ -------------------------------------------------------------------------------- /src/main.cpp: -------------------------------------------------------------------------------- 1 | /** @file main.cpp 2 | * ROSbot firmware. 3 | * 4 | * @author Husarion 5 | * @copyright MIT 6 | */ 7 | 8 | #include 9 | 10 | #define MAIN_LOOP_INTERVAL_MS 10 11 | #define IMU_I2C_FREQUENCY 100000L 12 | #define IMU_I2C_SCL SENS2_PIN3 13 | #define IMU_I2C_SDA SENS2_PIN4 14 | 15 | volatile uint8_t err_msg; 16 | static volatile uint32_t spin_count; 17 | 18 | static mbed::InterruptIn button1(BUTTON1); 19 | static mbed::InterruptIn button2(BUTTON2); 20 | 21 | void button1_fall_callback() 22 | { 23 | buttons_publish_flag[0] = true; 24 | } 25 | 26 | void button2_fall_callback() 27 | { 28 | buttons_publish_flag[1] = true; 29 | } 30 | 31 | void button1_rise_callback() 32 | { 33 | buttons_publish_flag[0] = false; 34 | } 35 | 36 | void button2_rise_callback() 37 | { 38 | buttons_publish_flag[1] = false; 39 | } 40 | 41 | void range_sensors_msg_handler() 42 | { 43 | osEvent evt1 = distance_sensor_mail_box.get(0); 44 | if (evt1.status == osEventMail) 45 | { 46 | SensorsMeasurement *message = (SensorsMeasurement *)evt1.value.p; 47 | if (message->status == MultiDistanceSensor::ERR_I2C_FAILURE) 48 | { 49 | err_msg++; 50 | if (distance_sensor_commands.empty() && err_msg == 3) 51 | { 52 | uint8_t *data = distance_sensor_commands.alloc(); 53 | *data = 2; 54 | distance_sensor_commands.put(data); 55 | data = distance_sensor_commands.alloc(); 56 | *data = 1; 57 | distance_sensor_commands.put(data); 58 | err_msg = 0; 59 | } 60 | } 61 | else 62 | { 63 | err_msg = 0; 64 | for (auto i = 0u; i < RANGES_COUNT; ++i) 65 | { 66 | fill_range_msg_with_measurements(&range_msgs[i], message->range[i]); 67 | } 68 | } 69 | distance_sensor_mail_box.free(message); 70 | } 71 | } 72 | 73 | void publish_range_sensors(rcl_timer_t *timer, int64_t last_call_time) 74 | { 75 | RCLC_UNUSED(last_call_time); 76 | if (timer != NULL) 77 | { 78 | for (auto i = 0u; i < RANGES_COUNT; ++i) 79 | { 80 | fill_range_msg(&range_msgs[i], i); 81 | publish_range_msg(&range_msgs[i], i); 82 | } 83 | } 84 | } 85 | 86 | void imu_msg_handler() 87 | { 88 | osEvent evt2 = imu_sensor_mail_box.get(0); 89 | 90 | if (evt2.status == osEventMail) 91 | { 92 | ImuDriver::ImuMeasurement *message = (ImuDriver::ImuMeasurement *)evt2.value.p; 93 | fill_imu_msg(&imu_msg); 94 | fill_imu_msg_with_measurements(&imu_msg, message); 95 | publish_imu_msg(&imu_msg); 96 | imu_sensor_mail_box.free(message); 97 | } 98 | } 99 | 100 | void battery_msg_handler() 101 | { 102 | if (spin_count % 40 == 0) 103 | { 104 | fill_battery_msg(&battery_msg); 105 | publish_battery_msg(&battery_msg); 106 | } 107 | } 108 | 109 | void button_msgs_handler() 110 | { 111 | for (auto i = 0u; i < BUTTONS_COUNT; ++i) 112 | { 113 | if (buttons_publish_flag[i] != button_msgs[i].data) 114 | { 115 | button_msgs[i].data = buttons_publish_flag[i]; 116 | publish_button_msg(&button_msgs[i], i); 117 | } 118 | } 119 | } 120 | 121 | void wheels_state_msg_handler() 122 | { 123 | if (spin_count % 5 == 0) 124 | { 125 | fill_wheels_state_msg(&wheels_state_msg); 126 | publish_wheels_state_msg(&wheels_state_msg); 127 | } 128 | } 129 | 130 | void timer_callback(rcl_timer_t *timer, int64_t last_call_time) 131 | { 132 | RCLC_UNUSED(last_call_time); 133 | if (timer != NULL) 134 | { 135 | imu_msg_handler(); 136 | wheels_state_msg_handler(); 137 | button_msgs_handler(); 138 | range_sensors_msg_handler(); 139 | battery_msg_handler(); 140 | spin_count++; 141 | } 142 | } 143 | 144 | bool on_parameter_changed(const Parameter *old_param, const Parameter *new_param, void *context) 145 | { 146 | (void)context; 147 | if (old_param == NULL && new_param == NULL) 148 | { 149 | return false; 150 | } 151 | if (old_param != NULL and new_param != NULL) 152 | { 153 | std::map::iterator it; 154 | switch (old_param->value.type) 155 | { 156 | case RCLC_PARAMETER_DOUBLE: 157 | it = servo_voltage_configuration.find(new_param->value.double_value); 158 | if (it == servo_voltage_configuration.end()) 159 | { 160 | return false; 161 | } 162 | servo_manager.setPowerMode(it->second); 163 | break; 164 | case RCLC_PARAMETER_BOOL: 165 | if (not strcmp(new_param->name.data, "servo_enable_power")) 166 | { 167 | servo_manager.enablePower(new_param->value.bool_value); 168 | } 169 | else if (isdigit(new_param->name.data[5])) 170 | { 171 | servo_manager.enableOutput(new_param->name.data[5] - '0', new_param->value.bool_value); 172 | } 173 | else 174 | { 175 | return false; 176 | } 177 | break; 178 | case RCLC_PARAMETER_INT: 179 | if (isdigit(new_param->name.data[5])) 180 | { 181 | servo_manager.setPeriod(new_param->name.data[5] - '0', new_param->value.integer_value); 182 | } 183 | else 184 | { 185 | return false; 186 | } 187 | break; 188 | default: 189 | break; 190 | } 191 | } 192 | return true; 193 | } 194 | 195 | int main() 196 | { 197 | ThisThread::sleep_for(100); 198 | sens_power = 1; // sensors power on 199 | ThisThread::sleep_for(100); 200 | 201 | init_battery(); 202 | init_wheels(); 203 | init_button_and_attach_to_callbacks(&button1, button1_rise_callback, button1_fall_callback); 204 | init_button_and_attach_to_callbacks(&button2, button2_rise_callback, button2_fall_callback); 205 | 206 | I2C *i2c_ptr = new I2C(IMU_I2C_SDA, IMU_I2C_SCL); 207 | i2c_ptr->frequency(IMU_I2C_FREQUENCY); 208 | init_imu(i2c_ptr); 209 | init_servos(); 210 | init_ranges(); 211 | 212 | // if button1 or button2 are pressed, connect through USB 213 | if (button1.read() == true && button2.read() == true) 214 | { 215 | led2=0; 216 | led3=1; 217 | set_microros_serial_transports(µros_serial_rpi); 218 | } 219 | else 220 | { 221 | led2=1; 222 | led3=0; 223 | set_microros_serial_transports(µros_serial_ftdi); 224 | } 225 | 226 | uint32_t cnt=0; 227 | while (not rmw_uros_ping_agent(100, 1) == RMW_RET_OK) 228 | { 229 | ThisThread::sleep_for(100); 230 | cnt++; 231 | if (cnt == 30) { 232 | NVIC_SystemReset(); 233 | } 234 | } 235 | 236 | if (not microros_init()) 237 | { 238 | microros_deinit(); 239 | ThisThread::sleep_for(2000); 240 | NVIC_SystemReset(); 241 | } 242 | 243 | led2=0; 244 | led3=0; 245 | 246 | AgentStates state = AGENT_CONNECTED; 247 | while (state == AGENT_CONNECTED) 248 | { 249 | EXECUTE_EVERY_N_MS(2000, 250 | state = (RMW_RET_OK == rmw_uros_ping_agent(200, 5)) ? AGENT_CONNECTED : AGENT_DISCONNECTED;); 251 | microros_spin(); 252 | } 253 | 254 | for (int i = 0; i < 10; ++i) 255 | { 256 | ThisThread::sleep_for(200); 257 | } 258 | microros_deinit(); 259 | NVIC_SystemReset(); 260 | } -------------------------------------------------------------------------------- /lib/MultiDistanceSensor/MultiDistanceSensor.cpp: -------------------------------------------------------------------------------- 1 | #include "MultiDistanceSensor.h" 2 | 3 | static const uint8_t DEFAULT_HW_ADDRESS = 0x29; 4 | 5 | static const uint8_t SENSOR_HW_ADDRESS[]={ 6 | DEFAULT_HW_ADDRESS + 1, 7 | DEFAULT_HW_ADDRESS + 2, 8 | DEFAULT_HW_ADDRESS + 3, 9 | DEFAULT_HW_ADDRESS + 4, 10 | }; 11 | 12 | #if defined(TARGET_CORE2) 13 | 14 | #define SENSOR_FR_XSHOUT_PIN SENS6_PIN1 15 | #define SENSOR_FL_XSHOUT_PIN SENS6_PIN2 16 | #define SENSOR_RR_XSHOUT_PIN SENS6_PIN4 17 | #define SENSOR_RL_XSHOUT_PIN SENS6_PIN3 18 | #define SENSORS_SDA_PIN SENS1_PIN4 19 | #define SENSORS_SCL_PIN SENS1_PIN3 20 | 21 | static DigitalInOut xshout[]={ 22 | DigitalInOut(SENSOR_FR_XSHOUT_PIN, PIN_OUTPUT, OpenDrainNoPull, 0), 23 | DigitalInOut(SENSOR_FL_XSHOUT_PIN, PIN_OUTPUT, OpenDrainNoPull, 0), 24 | DigitalInOut(SENSOR_RR_XSHOUT_PIN, PIN_OUTPUT, OpenDrainNoPull, 0), 25 | DigitalInOut(SENSOR_RL_XSHOUT_PIN, PIN_OUTPUT, OpenDrainNoPull, 0)}; 26 | 27 | #else 28 | #error "Your target is not supported!" 29 | #endif /* TARGET_CORE2 */ 30 | 31 | MultiDistanceSensor * MultiDistanceSensor::_instance = nullptr; 32 | 33 | Mail distance_sensor_mail_box; 34 | Mail distance_sensor_commands; 35 | 36 | MultiDistanceSensor::MultiDistanceSensor() 37 | :_i2c(nullptr) 38 | ,_xshout{nullptr, nullptr, nullptr, nullptr} 39 | ,_is_active{false,false,false,false} 40 | ,_initialized(false) 41 | ,_sensors_enabled(false) 42 | ,_last_sensor_index(-1) 43 | {} 44 | 45 | MultiDistanceSensor & MultiDistanceSensor::getInstance() 46 | { 47 | if(_instance==NULL) 48 | { 49 | static MultiDistanceSensor instance; 50 | _instance = &instance; 51 | } 52 | return *_instance; 53 | } 54 | 55 | MultiDistanceSensor::~MultiDistanceSensor() 56 | { 57 | stop(); 58 | for(int i=0;i<4;i++) delete _sensor[i]; 59 | } 60 | 61 | int MultiDistanceSensor::restart() 62 | { 63 | int result=0; 64 | 65 | for(int i=0;i<4;i++) 66 | { 67 | _xshout[i]->write(0); 68 | _sensor[i]->setDefaultAddress(); 69 | _is_active[i] = false; 70 | } 71 | 72 | ThisThread::sleep_for(10); 73 | 74 | _i2c->frequency(DISTANCE_SENSORS_DEFAULT_I2C_FREQ); 75 | 76 | for(int i=0;i<4;i++) 77 | { 78 | _sensor[i]->setTimeout(500); 79 | _xshout[i]->write(1); 80 | ThisThread::sleep_for(10); 81 | if(_sensor[i]->init()) 82 | { 83 | _sensor[i]->setAddress(SENSOR_HW_ADDRESS[i]); 84 | _sensor[i]->setMeasurementTimingBudget(80); 85 | _is_active[i]=true; 86 | _last_sensor_index = i; 87 | result++; 88 | } 89 | else 90 | { 91 | _xshout[i]->write(0); 92 | } 93 | } 94 | 95 | return result; 96 | } 97 | 98 | void MultiDistanceSensor::stop() 99 | { 100 | for(int i=0;istopContinuous(); 105 | } 106 | } 107 | _sensors_enabled = false; 108 | } 109 | 110 | void MultiDistanceSensor::start() 111 | { 112 | for(int i=0;isetTimeout(100); 117 | _sensor[i]->startContinuous(100); 118 | } 119 | } 120 | _sensors_enabled = true; 121 | } 122 | 123 | int MultiDistanceSensor::init() 124 | { 125 | if(_initialized) 126 | return 0; 127 | 128 | _i2c = new I2C(SENSORS_SDA_PIN,SENSORS_SCL_PIN); 129 | 130 | for(int i=0;i 0) 139 | _distance_sensor_thread.start(callback(this,&MultiDistanceSensor::sensors_loop)); 140 | return result; 141 | } 142 | 143 | void MultiDistanceSensor::sensors_loop() 144 | { 145 | while (1) 146 | { 147 | osEvent evt = distance_sensor_commands.get(0); 148 | 149 | if(evt.status == osEventMail) 150 | { 151 | uint8_t * command = (uint8_t *)evt.value.p; 152 | switch(*command) 153 | { 154 | case 0: 155 | stop(); 156 | break; 157 | case 1: 158 | start(); 159 | break; 160 | case 2: 161 | _i2c->abort_transfer(); 162 | restart(); 163 | break; 164 | default: 165 | break; 166 | } 167 | distance_sensor_commands.free(command); 168 | } 169 | 170 | if (_sensors_enabled) runMeasurement(); 171 | 172 | ThisThread::sleep_for(10); 173 | } 174 | } 175 | 176 | void MultiDistanceSensor::processOut() 177 | { 178 | // static uint32_t spin_cnt = 0; 179 | 180 | // if(_m.status == ERR_NOT_INIT && spin_cnt++ % 50 != 0) 181 | // return; 182 | 183 | if(_m.status != ERR_NOT_INIT && !distance_sensor_mail_box.full()) 184 | { 185 | SensorsMeasurement * msg = distance_sensor_mail_box.alloc(); 186 | if (msg == nullptr) 187 | return; 188 | 189 | memcpy(&msg->range,&_m.range,sizeof(_m.range)); 190 | msg->timestamp = _m.timestamp; 191 | msg->status = _m.status; 192 | distance_sensor_mail_box.put(msg); 193 | } 194 | } 195 | 196 | void MultiDistanceSensor::runMeasurement() 197 | { 198 | if(_last_sensor_index == -1) 199 | { 200 | _m.status = ERR_NOT_INIT; 201 | return processOut(); 202 | } 203 | 204 | bool is_measurement_ready = (_sensor[_last_sensor_index]->readReg(VL53L0X::RESULT_INTERRUPT_STATUS) & 0x07); 205 | 206 | if (_sensor[_last_sensor_index]->last_status != 0) 207 | { 208 | _xshout[_last_sensor_index]->write(0); 209 | _is_active[_last_sensor_index] = false; 210 | 211 | _m.status = ERR_I2C_FAILURE; 212 | _m.timestamp = Kernel::get_ms_count(); 213 | for(int i=0; ireadRangeContinuousMillimeters(false); 231 | if(_sensor[i]->last_status == 0) 232 | { 233 | _m.range[i] = (float) range / 1000.0; 234 | } 235 | else 236 | { 237 | _m.range[i] = -1.0; 238 | _is_active[i] = false; 239 | _xshout[i]->write(0); 240 | _m.status = ERR_I2C_FAILURE; 241 | } 242 | } 243 | } 244 | 245 | return processOut(); 246 | } 247 | } -------------------------------------------------------------------------------- /lib/RosbotDrive/RosbotDrive.cpp: -------------------------------------------------------------------------------- 1 | #include "RosbotDrive.h" 2 | #include "RosbotRegulatorCMSIS.h" 3 | #define PWM_DEFAULT_FREQ_HZ 18000UL /**< Default frequency for motors' pwms.*/ 4 | 5 | #define FOR(x) for(int i=0;igetDCMotor((MotNum)i); 106 | _mot[i+2]=_mot_driver[1]->getDCMotor((MotNum)i); 107 | } 108 | 109 | // Use CMSIS PID regulator 110 | FOR(4) _regulator[i] = new RosbotRegulatorCMSIS(reg_params); //TODO change to static 111 | 112 | _regulator_interval_ms = reg_params.dt_ms; 113 | 114 | _wheel_coefficient1 = 2 * M_PI * wheel_params.radius / (wheel_params.gear_ratio * wheel_params.encoder_cpr * wheel_params.tyre_deflation); 115 | _wheel_coefficient2 = 2 * M_PI / (wheel_params.gear_ratio * wheel_params.encoder_cpr); 116 | 117 | FOR(4) 118 | { 119 | _mot[i]->setPolarity(wheel_params.polarity>>i & 1); 120 | _mot[i]->init(PWM_DEFAULT_FREQ_HZ); 121 | _mot[i]->setDriveMode(true); 122 | _encoder[i]->setPolarity(wheel_params.polarity>>(i+4) & 1); 123 | _encoder[i]->init(); 124 | } 125 | 126 | _state=HALT; 127 | 128 | FOR(2) _mot_driver[i]->enable(true); 129 | 130 | initialized = true; 131 | 132 | regulator_thread.start(callback(this,&RosbotDrive::regulatorLoop)); 133 | } 134 | 135 | void RosbotDrive::enable(bool en) 136 | { 137 | if(_state == UNINIT) 138 | return; 139 | 140 | switch(_state) 141 | { 142 | case HALT: 143 | if(en) 144 | _state=OPERATIONAL; 145 | else 146 | _state=IDLE; 147 | 148 | FOR(2) _mot_driver[i]->enable(en); 149 | 150 | break; 151 | case IDLE: 152 | if(en) 153 | { 154 | _state=OPERATIONAL; 155 | FOR(2) _mot_driver[i]->enable(en); 156 | } 157 | break; 158 | case OPERATIONAL: 159 | if(!en) 160 | { 161 | _state=IDLE; 162 | FOR(4) 163 | { 164 | _mot[i]->setPower(0); 165 | _tspeed_mps[i]=0; 166 | _regulator[i]->reset(); 167 | } 168 | FOR(2) _mot_driver[i]->enable(en); 169 | } 170 | break; 171 | default: 172 | break; 173 | } 174 | } 175 | 176 | void RosbotDrive::regulatorLoop() 177 | { 178 | uint64_t sleepTime; 179 | int32_t distance; 180 | float factor1; 181 | int mot_num; 182 | while (1) 183 | { 184 | sleepTime = Kernel::get_ms_count() + _regulator_interval_ms; 185 | if (_regulator_loop_enabled) //TODO: change to mutex with fixed held time 186 | { 187 | 188 | factor1 = 1000.0 * _wheel_coefficient1 / _regulator_interval_ms; 189 | FOR(4) 190 | { 191 | distance = _encoder[i]->getCount(); 192 | _cspeed_mps[i] = (float)(distance - _cdistance[i]) * factor1; 193 | _cdistance[i] = distance; 194 | 195 | } 196 | if ((_state == OPERATIONAL) && _regulator_output_enabled) 197 | { 198 | FOR(4) 199 | { 200 | mot_num = _motor_sequence[i]; 201 | _mot[mot_num]->setPower(_regulator[mot_num]->updateState(_tspeed_mps[mot_num],_cspeed_mps[mot_num])); 202 | } 203 | } 204 | } 205 | ThisThread::sleep_until(sleepTime); 206 | } 207 | } 208 | 209 | void RosbotDrive::updateTargetSpeed(const NewTargetSpeed & new_speed) 210 | { 211 | if(_state != OPERATIONAL) 212 | return; 213 | switch(new_speed.mode) 214 | { 215 | case DUTY_CYCLE: 216 | if(!_regulator_output_enabled) 217 | FOR(4) {_mot[i]->setPower(new_speed.speed[i]);} 218 | break; 219 | case MPS: 220 | if(_regulator_output_enabled) 221 | FOR(4) {_tspeed_mps[i]=new_speed.speed[i];} 222 | break; 223 | default: 224 | return; 225 | } 226 | } 227 | 228 | float RosbotDrive::getDistance(RosbotMotNum mot_num) 229 | { 230 | return (float)_wheel_coefficient1 * _encoder[mot_num]->getCount(); 231 | } 232 | 233 | float RosbotDrive::getAngularPos(RosbotMotNum mot_num) 234 | { 235 | return (float)_wheel_coefficient2 * _encoder[mot_num]->getCount(); 236 | } 237 | 238 | int32_t RosbotDrive::getEncoderTicks(RosbotMotNum mot_num) 239 | { 240 | return _encoder[mot_num]->getCount(); 241 | } 242 | 243 | float RosbotDrive::getSpeed(RosbotMotNum mot_num) 244 | { 245 | return _cspeed_mps[mot_num]; 246 | } 247 | 248 | void RosbotDrive::updateWheelCoefficients(const RosbotWheel & params) 249 | { 250 | _regulator_loop_enabled = false; 251 | _wheel_coefficient1 = 2 * M_PI * params.radius / (params.gear_ratio * params.encoder_cpr * params.tyre_deflation); 252 | _wheel_coefficient2 = 2 * M_PI / (params.gear_ratio * params.encoder_cpr); 253 | _regulator_loop_enabled = true; 254 | } 255 | 256 | void RosbotDrive::updatePidParams(const RosbotRegulator_params & params) 257 | { 258 | _regulator_loop_enabled = false; 259 | FOR(4) _regulator[i]->updateParams(params); 260 | _regulator_loop_enabled = true; 261 | } 262 | 263 | void RosbotDrive::getPidParams(RosbotRegulator_params & params) 264 | { 265 | _regulator[0]->getParams(params); 266 | } 267 | 268 | void RosbotDrive::stop() 269 | { 270 | switch(_state) 271 | { 272 | case IDLE: 273 | _state=HALT; 274 | FOR(2) _mot_driver[i]->enable(true); 275 | break; 276 | case OPERATIONAL: 277 | _state=HALT; 278 | FOR(4) 279 | { 280 | _tspeed_mps[i]=0; 281 | _mot[i]->setPower(0); 282 | } 283 | break; 284 | default: 285 | break; 286 | } 287 | } 288 | 289 | void RosbotDrive::enablePidReg(bool en) 290 | { 291 | _regulator_output_enabled = en; 292 | } 293 | 294 | bool RosbotDrive::isPidEnabled() 295 | { 296 | return _regulator_output_enabled; 297 | } 298 | 299 | // void RosbotDrive::getPidDebugData(PidDebugData_t * data, RosbotMotNum mot_num) 300 | // { 301 | // // CriticalSectionLock lock; 302 | // data->cspeed=_cspeed_mps[mot_num]; 303 | // data->tspeed=_tspeed_mps[mot_num]; 304 | // data->error=_error[mot_num]; 305 | // data->pidout=_pidout[mot_num]; 306 | // } 307 | 308 | float RosbotDrive::getSpeed(RosbotMotNum mot_num, SpeedMode mode) 309 | { 310 | switch(mode) 311 | { 312 | case TICSKPS: 313 | return _cspeed_mps[mot_num]/_wheel_coefficient1; 314 | case MPS: 315 | return _cspeed_mps[mot_num]; 316 | case DUTY_CYCLE: 317 | return _mot[mot_num]->getDutyCycle(); 318 | default: 319 | return 0.0; 320 | } 321 | } 322 | 323 | void RosbotDrive::resetDistance() 324 | { 325 | bool tmp = _regulator_output_enabled; 326 | _regulator_loop_enabled = false; 327 | _regulator_output_enabled = false; 328 | FOR(4) 329 | { 330 | if(_tspeed_mps[i] > 0.0f ) _mot[i]->setPower(0); 331 | _encoder[i]->resetCount(); 332 | _regulator[i]->reset(); 333 | _tspeed_mps[i]=0; 334 | _cspeed_mps[i]=0; 335 | _cdistance[i]=0; 336 | } 337 | _regulator_loop_enabled = true; 338 | _regulator_output_enabled = tmp; 339 | } 340 | 341 | void RosbotDrive::setupMotorSequence(RosbotMotNum first, RosbotMotNum second, RosbotMotNum third, RosbotMotNum fourth) 342 | { 343 | bool tmp = _regulator_output_enabled; 344 | _regulator_output_enabled = false; 345 | _motor_sequence[0] = first; 346 | _motor_sequence[1] = second; 347 | _motor_sequence[2] = third; 348 | _motor_sequence[3] = fourth; 349 | _regulator_output_enabled = tmp; 350 | } -------------------------------------------------------------------------------- /src/TARGET_CORE2/system_clock.c: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | * Copyright (c) 2006-2017 ARM Limited 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | */ 16 | 17 | /** 18 | * This file configures the system clock as follows: 19 | *----------------------------------------------------------------------------- 20 | * System clock source | 1- USE_PLL_HSE_EXTC (external 8 MHz clock) 21 | * | 2- USE_PLL_HSE_XTAL (external 8 MHz xtal) 22 | * | 3- USE_PLL_HSI (internal 16 MHz) 23 | *----------------------------------------------------------------------------- 24 | * SYSCLK(MHz) | 168 25 | * AHBCLK (MHz) | 168 26 | * APB1CLK (MHz) | 42 27 | * APB2CLK (MHz) | 84 28 | * USB capable | YES 29 | *----------------------------------------------------------------------------- 30 | **/ 31 | 32 | #include "stm32f4xx.h" 33 | #include "nvic_addr.h" 34 | #include "mbed_error.h" 35 | 36 | /*!< Uncomment the following line if you need to relocate your vector Table in 37 | Internal SRAM. */ 38 | /* #define VECT_TAB_SRAM */ 39 | #define VECT_TAB_OFFSET 0x00 /*!< Vector Table base offset field. 40 | This value must be a multiple of 0x200. */ 41 | 42 | 43 | // clock source is selected with CLOCK_SOURCE in json config 44 | #define USE_PLL_HSE_EXTC 0x8 // Use external clock (ST Link MCO) 45 | #define USE_PLL_HSE_XTAL 0x4 // Use external xtal (X3 on board - not provided by default) 46 | #define USE_PLL_HSI 0x2 // Use HSI internal clock 47 | 48 | 49 | #if ( ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) ) 50 | uint8_t SetSysClock_PLL_HSE(uint8_t bypass); 51 | #endif /* ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) */ 52 | 53 | #if ((CLOCK_SOURCE) & USE_PLL_HSI) 54 | uint8_t SetSysClock_PLL_HSI(void); 55 | #endif /* ((CLOCK_SOURCE) & USE_PLL_HSI) */ 56 | 57 | /** 58 | * @brief Setup the microcontroller system 59 | * Initialize the FPU setting, vector table location and External memory 60 | * configuration. 61 | * @param None 62 | * @retval None 63 | */ 64 | void SystemInit(void) 65 | { 66 | /* FPU settings ------------------------------------------------------------*/ 67 | #if (__FPU_PRESENT == 1) && (__FPU_USED == 1) 68 | SCB->CPACR |= ((3UL << 10 * 2) | (3UL << 11 * 2)); /* set CP10 and CP11 Full Access */ 69 | #endif 70 | /* Reset the RCC clock configuration to the default reset state ------------*/ 71 | /* Set HSION bit */ 72 | RCC->CR |= (uint32_t)0x00000001; 73 | 74 | /* Reset CFGR register */ 75 | RCC->CFGR = 0x00000000; 76 | 77 | /* Reset HSEON, CSSON and PLLON bits */ 78 | RCC->CR &= (uint32_t)0xFEF6FFFF; 79 | 80 | /* Reset PLLCFGR register */ 81 | RCC->PLLCFGR = 0x24003010; 82 | 83 | /* Reset HSEBYP bit */ 84 | RCC->CR &= (uint32_t)0xFFFBFFFF; 85 | 86 | /* Disable all interrupts */ 87 | RCC->CIR = 0x00000000; 88 | 89 | #if defined (DATA_IN_ExtSRAM) || defined (DATA_IN_ExtSDRAM) 90 | SystemInit_ExtMemCtl(); 91 | #endif /* DATA_IN_ExtSRAM || DATA_IN_ExtSDRAM */ 92 | 93 | /* Configure the Vector Table location add offset address ------------------*/ 94 | #ifdef VECT_TAB_SRAM 95 | SCB->VTOR = SRAM_BASE | VECT_TAB_OFFSET; /* Vector Table Relocation in Internal SRAM */ 96 | #else 97 | SCB->VTOR = NVIC_FLASH_VECTOR_ADDRESS; /* Vector Table Relocation in Internal FLASH */ 98 | #endif 99 | 100 | } 101 | 102 | /** 103 | * @brief Configures the System clock source, PLL Multiplier and Divider factors, 104 | * AHB/APBx prescalers and Flash settings 105 | * @note This function should be called only once the RCC clock configuration 106 | * is reset to the default reset state (done in SystemInit() function). 107 | * @param None 108 | * @retval None 109 | */ 110 | void SetSysClock(void) 111 | { 112 | #if ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) 113 | /* 1- Try to start with HSE and external clock */ 114 | if (SetSysClock_PLL_HSE(1) == 0) 115 | #endif 116 | { 117 | #if ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) 118 | /* 2- If fail try to start with HSE and external xtal */ 119 | if (SetSysClock_PLL_HSE(0) == 0) 120 | #endif 121 | { 122 | #if ((CLOCK_SOURCE) & USE_PLL_HSI) 123 | /* 3- If fail start with HSI clock */ 124 | if (SetSysClock_PLL_HSI() == 0) 125 | #endif 126 | { 127 | { 128 | error("SetSysClock failed\n"); 129 | } 130 | } 131 | } 132 | } 133 | 134 | /* Output clock on MCO2 pin(PC9) for debugging purpose */ 135 | //HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_SYSCLK, RCC_MCODIV_1); // 84 MHz 136 | } 137 | 138 | #if ( ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) ) 139 | /******************************************************************************/ 140 | /* PLL (clocked by HSE) used as System clock source */ 141 | /******************************************************************************/ 142 | uint8_t SetSysClock_PLL_HSE(uint8_t bypass) 143 | { 144 | RCC_OscInitTypeDef RCC_OscInitStruct = {0}; 145 | RCC_ClkInitTypeDef RCC_ClkInitStruct = {0}; 146 | 147 | /* The voltage scaling allows optimizing the power consumption when the device is 148 | clocked below the maximum system frequency, to update the voltage scaling value 149 | regarding system frequency refer to product datasheet. */ 150 | __HAL_RCC_PWR_CLK_ENABLE(); 151 | __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1); 152 | 153 | /* Enable HSE oscillator and activate PLL with HSE as source */ 154 | RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE; 155 | if (bypass == 0) { 156 | RCC_OscInitStruct.HSEState = RCC_HSE_ON; /* External 8 MHz xtal on OSC_IN/OSC_OUT */ 157 | } else { 158 | RCC_OscInitStruct.HSEState = RCC_HSE_BYPASS; /* External 8 MHz clock on OSC_IN */ 159 | } 160 | // RCC_OscInitStruct.HSIState = RCC_HSI_OFF; 161 | RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; 162 | RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE; 163 | RCC_OscInitStruct.PLL.PLLM = 8; // VCO input clock = 1 MHz (8 MHz / 8) 164 | RCC_OscInitStruct.PLL.PLLN = 336; // VCO output clock = 336 MHz (1 MHz * 336) 165 | RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // PLLCLK = 168 MHz (336 MHz / 2) 166 | RCC_OscInitStruct.PLL.PLLQ = 7; // USB clock = 48 MHz (336 MHz / 7) --> OK for USB 167 | if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { 168 | return 0; // FAIL 169 | } 170 | 171 | /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */ 172 | RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); 173 | RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // 168 MHz 174 | RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // 168 MHz 175 | RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // 42 MHz 176 | RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // 84 MHz (SPI1 clock...) 177 | if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { 178 | return 0; // FAIL 179 | } 180 | 181 | /* Output clock on MCO1 pin(PA8) for debugging purpose */ 182 | /* 183 | if (bypass == 0) 184 | HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSE, RCC_MCODIV_2); // 4 MHz 185 | else 186 | HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSE, RCC_MCODIV_1); // 8 MHz 187 | */ 188 | 189 | return 1; // OK 190 | } 191 | #endif /* ((CLOCK_SOURCE) & USE_PLL_HSE_XTAL) || ((CLOCK_SOURCE) & USE_PLL_HSE_EXTC) */ 192 | 193 | #if ((CLOCK_SOURCE) & USE_PLL_HSI) 194 | /******************************************************************************/ 195 | /* PLL (clocked by HSI) used as System clock source */ 196 | /******************************************************************************/ 197 | uint8_t SetSysClock_PLL_HSI(void) 198 | { 199 | RCC_OscInitTypeDef RCC_OscInitStruct; 200 | RCC_ClkInitTypeDef RCC_ClkInitStruct; 201 | 202 | /* The voltage scaling allows optimizing the power consumption when the device is 203 | clocked below the maximum system frequency, to update the voltage scaling value 204 | regarding system frequency refer to product datasheet. */ 205 | __HAL_RCC_PWR_CLK_ENABLE(); 206 | __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2); 207 | 208 | /* Enable HSI oscillator and activate PLL with HSI as source */ 209 | RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI | RCC_OSCILLATORTYPE_HSE; 210 | RCC_OscInitStruct.HSIState = RCC_HSI_ON; 211 | RCC_OscInitStruct.HSEState = RCC_HSE_OFF; 212 | RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT; 213 | RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON; 214 | RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI; 215 | RCC_OscInitStruct.PLL.PLLM = 16; // VCO input clock = 1 MHz (16 MHz / 16) 216 | RCC_OscInitStruct.PLL.PLLN = 336; // VCO output clock = 336 MHz (1 MHz * 336) 217 | RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2; // PLLCLK = 168 MHz (336 MHz / 2) 218 | RCC_OscInitStruct.PLL.PLLQ = 7; // USB clock = 48 MHz (336 MHz / 7) --> freq is ok but not precise enough 219 | if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) { 220 | return 0; // FAIL 221 | } 222 | 223 | /* Select PLL as system clock source and configure the HCLK, PCLK1 and PCLK2 clocks dividers */ 224 | RCC_ClkInitStruct.ClockType = (RCC_CLOCKTYPE_SYSCLK | RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2); 225 | RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK; // 168 MHz 226 | RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1; // 168 MHz 227 | RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4; // 42 MHz 228 | RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV2; // 84 MHz 229 | if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_5) != HAL_OK) { 230 | return 0; // FAIL 231 | } 232 | 233 | /* Output clock on MCO1 pin(PA8) for debugging purpose */ 234 | //HAL_RCC_MCOConfig(RCC_MCO1, RCC_MCO1SOURCE_HSI, RCC_MCODIV_1); // 16 MHz 235 | 236 | return 1; // OK 237 | } 238 | #endif /* ((CLOCK_SOURCE) & USE_PLL_HSI) */ 239 | -------------------------------------------------------------------------------- /src/microros.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | rclc_executor_t executor; 5 | rclc_support_t support; 6 | rcl_allocator_t rcl_allocator; 7 | rcl_node_t node; 8 | rcl_timer_t timer; 9 | rcl_timer_t range_timer; 10 | 11 | 12 | rcl_publisher_t imu_pub; 13 | rcl_publisher_t wheels_state_pub; 14 | rcl_publisher_t battery_pub; 15 | rcl_publisher_t range_pubs[RANGES_COUNT]; 16 | rcl_publisher_t buttons_pubs[BUTTONS_COUNT]; 17 | 18 | rcl_subscription_t wheels_command_sub; 19 | rcl_subscription_t servos_command_sub; 20 | rcl_subscription_t led_subs[LED_COUNT]; 21 | 22 | rcl_service_t get_cpu_id_service; 23 | 24 | rclc_parameter_server_t param_server; 25 | 26 | std_msgs__msg__Bool led_msg; 27 | 28 | std_srvs__srv__Trigger_Request get_cpu_id_service_request; 29 | std_srvs__srv__Trigger_Response get_cpu_id_service_response; 30 | 31 | const char *range_pub_names[] = {"range/fr", "range/fl", "range/rr", "range/rl"}; 32 | const char *buttons_pub_names[] = {"button/left", "button/right"}; 33 | const char *led_subs_names[] = {"led/left", "led/right"}; 34 | 35 | extern void timer_callback(rcl_timer_t *timer, int64_t last_call_time); 36 | 37 | extern void publish_range_sensors(rcl_timer_t *timer, int64_t last_call_time); 38 | extern bool on_parameter_changed(const Parameter * old_param, const Parameter * new_param, void * context); 39 | 40 | bool microros_init() { 41 | rcl_allocator = rcl_get_default_allocator(); 42 | 43 | rcl_init_options_t init_options = rcl_get_zero_initialized_init_options(); 44 | RCCHECK(rcl_init_options_init(&init_options, rcl_allocator)); 45 | RCCHECK(rcl_init_options_set_domain_id(&init_options, UXR_CLIENT_DOMAIN_ID_TO_OVERRIDE_WITH_ENV)); 46 | RCCHECK(rclc_support_init_with_options(&support, 0, NULL, &init_options, &rcl_allocator)); 47 | 48 | RCCHECK(rclc_node_init_default(&node, NODE_NAME, "", &support)); 49 | 50 | if (not init_wheels_command_subscriber() or 51 | not init_wheels_state_publisher() or 52 | not init_servos_command_subscriber() or 53 | not init_imu_publisher() or 54 | not init_battery_publisher() or 55 | not init_range_publishers() or 56 | not init_button_publishers() or 57 | not init_led_subscribers() or 58 | not init_param_server() or 59 | not init_parameters() or 60 | not init_services()) { 61 | return false; 62 | } 63 | 64 | RCCHECK(rclc_timer_init_default(&timer, &support, RCL_MS_TO_NS(10), 65 | timer_callback)); 66 | RCCHECK(rclc_timer_init_default(&range_timer, &support, RCL_MS_TO_NS( 200 ), 67 | publish_range_sensors)); 68 | 69 | RCCHECK(rclc_executor_init(&executor, &support.context, 12, &rcl_allocator)); 70 | RCCHECK(rclc_executor_add_timer(&executor, &timer)); 71 | RCCHECK(rclc_executor_add_timer(&executor, &range_timer)); 72 | 73 | RCCHECK(rclc_executor_add_subscription(&executor, &wheels_command_sub, &wheels_command_msg, 74 | &wheels_command_callback, ON_NEW_DATA)); 75 | RCCHECK(rclc_executor_add_subscription(&executor, &servos_command_sub, &servos_command_msg, 76 | &servos_command_callback, ON_NEW_DATA)); 77 | RCCHECK(rclc_executor_add_subscription(&executor, &led_subs[0], &led_msg, 78 | &led1_callback, ON_NEW_DATA)); 79 | RCCHECK(rclc_executor_add_subscription(&executor, &led_subs[1], &led_msg, 80 | &led2_callback, ON_NEW_DATA)); 81 | RCCHECK(rclc_executor_add_parameter_server(&executor, ¶m_server, on_parameter_changed)); 82 | RCCHECK(rclc_executor_add_service(&executor, &get_cpu_id_service, &get_cpu_id_service_request, &get_cpu_id_service_response, get_cpu_id_service_callback)); 83 | 84 | RCCHECK(rclc_executor_prepare(&executor)); 85 | RCCHECK(rmw_uros_sync_session(1000)); 86 | return true; 87 | } 88 | 89 | bool microros_deinit() { 90 | rmw_context_t *rmw_context = rcl_context_get_rmw_context(&support.context); 91 | (void)rmw_uros_set_context_entity_destroy_session_timeout(rmw_context, 0); 92 | 93 | RCCHECK(rcl_subscription_fini(&wheels_command_sub, &node)); 94 | RCCHECK(rcl_subscription_fini(&servos_command_sub, &node)); 95 | for (auto i = 0u; i < LED_COUNT; ++i) { 96 | RCCHECK(rcl_subscription_fini(&led_subs[i], &node)); 97 | } 98 | RCCHECK(rcl_publisher_fini(&wheels_state_pub, &node)); 99 | RCCHECK(rcl_publisher_fini(&imu_pub, &node)); 100 | RCCHECK(rcl_publisher_fini(&battery_pub, &node)); 101 | for (auto i = 0u; i < RANGES_COUNT; ++i) { 102 | RCCHECK(rcl_publisher_fini(&range_pubs[i], &node)); 103 | } 104 | for (auto i = 0u; i < BUTTONS_COUNT; ++i) { 105 | RCCHECK(rcl_publisher_fini(&buttons_pubs[i], &node)); 106 | } 107 | RCCHECK(rclc_parameter_server_fini(¶m_server, &node)); 108 | 109 | RCCHECK(rclc_executor_fini(&executor)); 110 | RCCHECK(rcl_node_fini(&node)); 111 | return true; 112 | } 113 | 114 | bool microros_spin() { 115 | RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100))); 116 | return true; 117 | } 118 | 119 | bool init_imu_publisher() { 120 | RCCHECK(rclc_publisher_init_best_effort( 121 | &imu_pub, 122 | &node, 123 | ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Imu), 124 | IMU_TOPIC_NAME)); 125 | return true; 126 | } 127 | 128 | bool init_wheels_state_publisher() { 129 | RCCHECK(rclc_publisher_init_best_effort( 130 | &wheels_state_pub, 131 | &node, 132 | ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, JointState), 133 | WHEELS_STATE_TOPIC_NAME)); 134 | return true; 135 | } 136 | 137 | bool init_battery_publisher() { 138 | RCCHECK(rclc_publisher_init_default( 139 | &battery_pub, 140 | &node, 141 | ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, BatteryState), 142 | BATTERY_TOPIC_NAME)); 143 | return true; 144 | } 145 | 146 | bool init_range_publishers() { 147 | for (auto i = 0u; i < RANGES_COUNT; ++i) { 148 | RCCHECK(rclc_publisher_init_best_effort( 149 | &range_pubs[i], 150 | &node, 151 | ROSIDL_GET_MSG_TYPE_SUPPORT(sensor_msgs, msg, Range), 152 | range_pub_names[i])); 153 | } 154 | return true; 155 | } 156 | 157 | bool init_button_publishers() { 158 | for (auto i = 0u; i < BUTTONS_COUNT; ++i) { 159 | RCCHECK(rclc_publisher_init_default( 160 | &buttons_pubs[i], 161 | &node, 162 | ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), 163 | buttons_pub_names[i])); 164 | } 165 | return true; 166 | } 167 | 168 | bool init_wheels_command_subscriber() { 169 | RCCHECK(rclc_subscription_init_best_effort( 170 | &wheels_command_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Float32MultiArray), 171 | WHEELS_COMMAND_TOPIC_NAME)); 172 | return true; 173 | } 174 | 175 | bool init_servos_command_subscriber(){ 176 | RCCHECK(rclc_subscription_init_best_effort( 177 | &servos_command_sub, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, UInt32MultiArray), 178 | SERVOS_COMMAND_TOPIC_NAME)); 179 | return true; 180 | } 181 | 182 | bool init_led_subscribers(){ 183 | for(auto i = 0u; i < LED_COUNT; ++i){ 184 | RCCHECK(rclc_subscription_init_best_effort( 185 | &led_subs[i], &node, ROSIDL_GET_MSG_TYPE_SUPPORT(std_msgs, msg, Bool), 186 | led_subs_names[i])); 187 | } 188 | return true; 189 | } 190 | 191 | bool init_param_server(){ 192 | const rclc_parameter_options_t options = { 193 | .notify_changed_over_dds = true, 194 | .max_params = 14 195 | }; 196 | RCCHECK(rclc_parameter_server_init_with_option(¶m_server, &node, &options)); 197 | return true; 198 | } 199 | 200 | bool init_parameters(){ 201 | RCCHECK(rclc_add_parameter(¶m_server, "servo_voltage", RCLC_PARAMETER_DOUBLE)); 202 | RCCHECK(rclc_parameter_set_double(¶m_server, "servo_voltage", 5.0)); 203 | 204 | RCCHECK(rclc_add_parameter(¶m_server, "servo_enable_power", RCLC_PARAMETER_BOOL)); 205 | RCCHECK(rclc_parameter_set_bool(¶m_server, "servo_enable_power", false)); 206 | constexpr int BUFF = 20; 207 | char param_name[BUFF]; 208 | for(auto i = 0u; i < SERVOS_COUNT; ++i){ 209 | snprintf(param_name, BUFF, "servo%d_enable", i); 210 | RCCHECK(rclc_add_parameter(¶m_server, param_name, RCLC_PARAMETER_BOOL)); 211 | RCCHECK(rclc_parameter_set_bool(¶m_server, param_name, false)); 212 | 213 | snprintf(param_name, BUFF, "servo%d_period", i); 214 | RCCHECK(rclc_add_parameter(¶m_server, param_name, RCLC_PARAMETER_INT)); 215 | RCCHECK(rclc_parameter_set_int(¶m_server, param_name, 0)); 216 | } 217 | return true; 218 | } 219 | 220 | bool init_services() { 221 | std_srvs__srv__Trigger_Request__init(&get_cpu_id_service_request); 222 | std_srvs__srv__Trigger_Response__init(&get_cpu_id_service_response); 223 | 224 | RCCHECK(rclc_service_init_default( 225 | &get_cpu_id_service, 226 | &node, 227 | ROSIDL_GET_SRV_TYPE_SUPPORT(std_srvs, srv, Trigger), 228 | GET_CPU_ID_SERVICE_NAME 229 | )); 230 | 231 | return true; 232 | } 233 | 234 | bool publish_wheels_state_msg(sensor_msgs__msg__JointState *msg) { 235 | RCCHECK(rcl_publish(&wheels_state_pub, msg, NULL)); 236 | return true; 237 | } 238 | 239 | bool publish_imu_msg(sensor_msgs__msg__Imu *msg) { 240 | RCCHECK(rcl_publish(&imu_pub, msg, NULL)); 241 | return true; 242 | } 243 | 244 | bool publish_battery_msg(sensor_msgs__msg__BatteryState *msg) { 245 | RCCHECK(rcl_publish(&battery_pub, msg, NULL)); 246 | return true; 247 | } 248 | 249 | bool publish_range_msg(sensor_msgs__msg__Range *msg, uint8_t id) { 250 | RCCHECK(rcl_publish(&range_pubs[id], msg, NULL)); 251 | return true; 252 | } 253 | 254 | bool publish_button_msg(std_msgs__msg__Bool *msg, uint8_t id) { 255 | RCCHECK(rcl_publish(&buttons_pubs[id], msg, NULL)); 256 | return true; 257 | } 258 | 259 | void get_cpu_id_service_callback(const void *req, void *res) { 260 | (void)req; // Unused parameter 261 | 262 | const uint32_t ADDRESS = 0x1FFF7A10; 263 | const uint8_t NUM_BYTES = 12; 264 | uint8_t buffer[NUM_BYTES]; 265 | memcpy(buffer, (void *)ADDRESS, NUM_BYTES); 266 | 267 | // Prepare the CPU ID in hexadecimal format 268 | char cpu_id_buffer[NUM_BYTES * 2 + 1] = {0}; 269 | char *hex_ptr = cpu_id_buffer; 270 | for (uint8_t i = 0; i < NUM_BYTES; ++i) { 271 | snprintf(hex_ptr, 3, "%02X", buffer[i]); 272 | hex_ptr += 2; 273 | } 274 | 275 | // Prepare the final output buffer with "CPU ID: " prefix 276 | static char out_buffer[100]; // Ensure this is large enough 277 | snprintf(out_buffer, sizeof(out_buffer), "{\"cpu_id\": \"%s\"}", cpu_id_buffer); 278 | 279 | // Set the response 280 | std_srvs__srv__Trigger_Response *response = (std_srvs__srv__Trigger_Response *)res; 281 | response->success = true; 282 | response->message.data = out_buffer; 283 | response->message.size = strlen(out_buffer); 284 | } 285 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /src/TARGET_CORE2/PinNames.h: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | ******************************************************************************* 3 | * Copyright (c) 2018, STMicroelectronics 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 3. Neither the name of STMicroelectronics nor the names of its contributors 15 | * may be used to endorse or promote products derived from this software 16 | * without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************* 29 | */ 30 | 31 | #ifndef MBED_PINNAMES_H 32 | #define MBED_PINNAMES_H 33 | 34 | #include "cmsis.h" 35 | #include "PinNamesTypes.h" 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | typedef enum { 42 | ALT0 = 0x100, 43 | ALT1 = 0x200, 44 | ALT2 = 0x300, 45 | ALT3 = 0x400 46 | } ALTx; 47 | 48 | typedef enum { 49 | PA_0 = 0x00, 50 | PA_0_ALT0 = PA_0 | ALT0, 51 | PA_0_ALT1 = PA_0 | ALT1, 52 | PA_1 = 0x01, 53 | PA_1_ALT0 = PA_1 | ALT0, 54 | PA_1_ALT1 = PA_1 | ALT1, 55 | PA_2 = 0x02, 56 | PA_2_ALT0 = PA_2 | ALT0, 57 | PA_2_ALT1 = PA_2 | ALT1, 58 | PA_3 = 0x03, 59 | PA_3_ALT0 = PA_3 | ALT0, 60 | PA_3_ALT1 = PA_3 | ALT1, 61 | PA_4 = 0x04, 62 | PA_4_ALT0 = PA_4 | ALT0, 63 | PA_5 = 0x05, 64 | PA_5_ALT0 = PA_5 | ALT0, 65 | PA_6 = 0x06, 66 | PA_6_ALT0 = PA_6 | ALT0, 67 | PA_7 = 0x07, 68 | PA_7_ALT0 = PA_7 | ALT0, 69 | PA_7_ALT1 = PA_7 | ALT1, 70 | PA_7_ALT2 = PA_7 | ALT2, 71 | PA_8 = 0x08, 72 | PA_9 = 0x09, 73 | PA_10 = 0x0A, 74 | PA_11 = 0x0B, 75 | PA_12 = 0x0C, 76 | PA_13 = 0x0D, 77 | PA_14 = 0x0E, 78 | PA_15 = 0x0F, 79 | PA_15_ALT0 = PA_15 | ALT0, 80 | 81 | PB_0 = 0x10, 82 | PB_0_ALT0 = PB_0 | ALT0, 83 | PB_0_ALT1 = PB_0 | ALT1, 84 | PB_1 = 0x11, 85 | PB_1_ALT0 = PB_1 | ALT0, 86 | PB_1_ALT1 = PB_1 | ALT1, 87 | PB_2 = 0x12, 88 | PB_3 = 0x13, 89 | PB_3_ALT0 = PB_3 | ALT0, 90 | PB_4 = 0x14, 91 | PB_4_ALT0 = PB_4 | ALT0, 92 | PB_4_ALT1 = PB_4 | ALT1, 93 | PB_5 = 0x15, 94 | PB_5_ALT0 = PB_5 | ALT0, 95 | PB_5_ALT1 = PB_5 | ALT1, 96 | PB_6 = 0x16, 97 | PB_7 = 0x17, 98 | PB_8 = 0x18, 99 | PB_8_ALT0 = PB_8 | ALT0, 100 | PB_8_ALT1 = PB_8 | ALT1, 101 | PB_9 = 0x19, 102 | PB_9_ALT0 = PB_9 | ALT0, 103 | PB_9_ALT1 = PB_9 | ALT1, 104 | PB_10 = 0x1A, 105 | PB_11 = 0x1B, 106 | PB_12 = 0x1C, 107 | PB_13 = 0x1D, 108 | PB_14 = 0x1E, 109 | PB_14_ALT0 = PB_14 | ALT0, 110 | PB_14_ALT1 = PB_14 | ALT1, 111 | PB_15 = 0x1F, 112 | PB_15_ALT0 = PB_15 | ALT0, 113 | PB_15_ALT1 = PB_15 | ALT1, 114 | 115 | PC_0 = 0x20, 116 | PC_0_ALT0 = PC_0 | ALT0, 117 | PC_0_ALT1 = PC_0 | ALT1, 118 | PC_1 = 0x21, 119 | PC_1_ALT0 = PC_1 | ALT0, 120 | PC_1_ALT1 = PC_1 | ALT1, 121 | PC_2 = 0x22, 122 | PC_2_ALT0 = PC_2 | ALT0, 123 | PC_2_ALT1 = PC_2 | ALT1, 124 | PC_3 = 0x23, 125 | PC_3_ALT0 = PC_3 | ALT0, 126 | PC_3_ALT1 = PC_3 | ALT1, 127 | PC_4 = 0x24, 128 | PC_4_ALT0 = PC_4 | ALT0, 129 | PC_5 = 0x25, 130 | PC_5_ALT0 = PC_5 | ALT0, 131 | PC_6 = 0x26, 132 | PC_6_ALT0 = PC_6 | ALT0, 133 | PC_7 = 0x27, 134 | PC_7_ALT0 = PC_7 | ALT0, 135 | PC_8 = 0x28, 136 | PC_8_ALT0 = PC_8 | ALT0, 137 | PC_9 = 0x29, 138 | PC_9_ALT0 = PC_9 | ALT0, 139 | PC_10 = 0x2A, 140 | PC_10_ALT0 = PC_10 | ALT0, 141 | PC_11 = 0x2B, 142 | PC_11_ALT0 = PC_11 | ALT0, 143 | PC_12 = 0x2C, 144 | PC_13 = 0x2D, 145 | PC_14 = 0x2E, 146 | PC_15 = 0x2F, 147 | 148 | PD_0 = 0x30, 149 | PD_1 = 0x31, 150 | PD_2 = 0x32, 151 | PD_3 = 0x33, 152 | PD_4 = 0x34, 153 | PD_5 = 0x35, 154 | PD_6 = 0x36, 155 | PD_7 = 0x37, 156 | PD_8 = 0x38, 157 | PD_9 = 0x39, 158 | PD_10 = 0x3A, 159 | PD_11 = 0x3B, 160 | PD_12 = 0x3C, 161 | PD_13 = 0x3D, 162 | PD_14 = 0x3E, 163 | PD_15 = 0x3F, 164 | 165 | PE_0 = 0x40, 166 | PE_1 = 0x41, 167 | PE_2 = 0x42, 168 | PE_3 = 0x43, 169 | PE_4 = 0x44, 170 | PE_5 = 0x45, 171 | PE_6 = 0x46, 172 | PE_7 = 0x47, 173 | PE_8 = 0x48, 174 | PE_9 = 0x49, 175 | PE_10 = 0x4A, 176 | PE_11 = 0x4B, 177 | PE_12 = 0x4C, 178 | PE_13 = 0x4D, 179 | PE_14 = 0x4E, 180 | PE_15 = 0x4F, 181 | 182 | PF_0 = 0x50, 183 | PF_1 = 0x51, 184 | PF_2 = 0x52, 185 | PF_3 = 0x53, 186 | PF_4 = 0x54, 187 | PF_5 = 0x55, 188 | PF_6 = 0x56, 189 | PF_7 = 0x57, 190 | PF_8 = 0x58, 191 | PF_9 = 0x59, 192 | PF_10 = 0x5A, 193 | PF_11 = 0x5B, 194 | PF_12 = 0x5C, 195 | PF_13 = 0x5D, 196 | PF_14 = 0x5E, 197 | PF_15 = 0x5F, 198 | 199 | PG_0 = 0x60, 200 | PG_1 = 0x61, 201 | PG_2 = 0x62, 202 | PG_3 = 0x63, 203 | PG_4 = 0x64, 204 | PG_5 = 0x65, 205 | PG_6 = 0x66, 206 | PG_7 = 0x67, 207 | PG_8 = 0x68, 208 | PG_9 = 0x69, 209 | PG_10 = 0x6A, 210 | PG_11 = 0x6B, 211 | PG_12 = 0x6C, 212 | PG_13 = 0x6D, 213 | PG_14 = 0x6E, 214 | PG_15 = 0x6F, 215 | 216 | PH_0 = 0x70, 217 | PH_1 = 0x71, 218 | PH_2 = 0x72, 219 | PH_3 = 0x73, 220 | PH_4 = 0x74, 221 | PH_5 = 0x75, 222 | PH_6 = 0x76, 223 | PH_7 = 0x77, 224 | PH_8 = 0x78, 225 | PH_9 = 0x79, 226 | PH_10 = 0x7A, 227 | PH_11 = 0x7B, 228 | PH_12 = 0x7C, 229 | PH_13 = 0x7D, 230 | PH_14 = 0x7E, 231 | PH_15 = 0x7F, 232 | 233 | PI_0 = 0x80, 234 | PI_1 = 0x81, 235 | PI_2 = 0x82, 236 | PI_3 = 0x83, 237 | PI_4 = 0x84, 238 | PI_5 = 0x85, 239 | PI_6 = 0x86, 240 | PI_7 = 0x87, 241 | PI_8 = 0x88, 242 | PI_9 = 0x89, 243 | PI_10 = 0x8A, 244 | PI_11 = 0x8B, 245 | PI_12 = 0x8C, 246 | PI_13 = 0x8D, 247 | PI_14 = 0x8E, 248 | PI_15 = 0x8F, 249 | 250 | // ADC internal channels 251 | ADC_TEMP = 0xF0, 252 | ADC_VREF = 0xF1, 253 | ADC_VBAT = 0xF2, 254 | 255 | 256 | // STDIO for console print 257 | #ifdef MBED_CONF_TARGET_STDIO_UART_TX 258 | STDIO_UART_TX = MBED_CONF_TARGET_STDIO_UART_TX, 259 | #else 260 | STDIO_UART_TX = PB_10, 261 | #endif 262 | #ifdef MBED_CONF_TARGET_STDIO_UART_RX 263 | STDIO_UART_RX = MBED_CONF_TARGET_STDIO_UART_RX, 264 | #else 265 | STDIO_UART_RX = PB_11, 266 | #endif 267 | 268 | // Generic signals namings 269 | LED1 = PE_2, 270 | LED2 = PE_3, 271 | LED3 = PE_4, 272 | // Standardized button names 273 | BUTTON1 = PG_12, 274 | BUTTON2 = PG_13, 275 | SERIAL_TX = STDIO_UART_TX, /* USART 1*/ 276 | SERIAL_RX = STDIO_UART_RX, 277 | USBTX = STDIO_UART_TX, /* USART 1*/ 278 | USBRX = STDIO_UART_RX, 279 | I2C_SCL = PB_8, /* I2C1 */ 280 | I2C_SDA = PB_9, 281 | SPI_MOSI = PA_7, 282 | SPI_MISO = PA_6, 283 | SPI_SCK = PA_5, 284 | SPI_CS = PB_6, 285 | PWM_OUT = PB_3, 286 | 287 | 288 | /**** USB pins ****/ 289 | USB_OTG_FS_DM = PA_11, 290 | USB_OTG_FS_DP = PA_12, 291 | USB_OTG_FS_ID = PA_10, 292 | USB_OTG_FS_SOF = PA_8, 293 | USB_OTG_FS_VBUS = PA_9, 294 | USB_OTG_HS_DM = PB_14, 295 | USB_OTG_HS_DP = PB_15, 296 | USB_OTG_HS_ID = PB_12, 297 | USB_OTG_HS_SOF = PA_4, 298 | USB_OTG_HS_ULPI_CK = PA_5, 299 | USB_OTG_HS_ULPI_D0 = PA_3, 300 | USB_OTG_HS_ULPI_D1 = PB_0, 301 | USB_OTG_HS_ULPI_D2 = PB_1, 302 | USB_OTG_HS_ULPI_D3 = PB_10, 303 | USB_OTG_HS_ULPI_D4 = PB_11, 304 | USB_OTG_HS_ULPI_D5 = PB_12, 305 | USB_OTG_HS_ULPI_D6 = PB_13, 306 | USB_OTG_HS_ULPI_D7 = PB_5, 307 | USB_OTG_HS_ULPI_DIR = PC_2, 308 | USB_OTG_HS_ULPI_NXT = PC_3, 309 | USB_OTG_HS_ULPI_STP = PC_0, 310 | USB_OTG_HS_VBUS = PB_13, 311 | 312 | /**** ETHERNET pins ****/ 313 | ETH_COL = PA_3, 314 | ETH_CRS = PA_0, 315 | ETH_CRS_DV = PA_7, 316 | ETH_MDC = PC_1, 317 | ETH_MDIO = PA_2, 318 | ETH_PPS_OUT = PB_5, 319 | ETH_REF_CLK = PA_1, 320 | ETH_RXD0 = PC_4, 321 | ETH_RXD1 = PC_5, 322 | ETH_RXD2 = PB_0, 323 | ETH_RXD3 = PB_1, 324 | ETH_RX_CLK = PA_1, 325 | ETH_RX_DV = PA_7, 326 | ETH_RX_ER = PB_10, 327 | ETH_TXD0 = PB_12, 328 | ETH_TXD1 = PB_13, 329 | ETH_TXD2 = PC_2, 330 | ETH_TXD3 = PE_2, 331 | ETH_TXD3_ALT0 = PB_8, 332 | ETH_TX_CLK = PC_3, 333 | ETH_TX_EN = PB_11, 334 | 335 | /**** OSCILLATOR pins ****/ 336 | RCC_OSC32_IN = PC_14, 337 | RCC_OSC32_OUT = PC_15, 338 | RCC_OSC_IN = PH_0, 339 | RCC_OSC_OUT = PH_1, 340 | 341 | /**** DEBUG pins ****/ 342 | SYS_JTCK_SWCLK = PA_14, 343 | SYS_JTDI = PA_15, 344 | SYS_JTDO_SWO = PB_3, 345 | SYS_JTMS_SWDIO = PA_13, 346 | SYS_JTRST = PB_4, 347 | SYS_TRACECLK = PE_2, 348 | SYS_TRACED0 = PE_3, 349 | SYS_TRACED1 = PE_4, 350 | SYS_TRACED2 = PE_5, 351 | SYS_TRACED3 = PE_6, 352 | SYS_WKUP = PA_0, 353 | 354 | /**** SENSORS PINS ****/ 355 | SENS1_PIN1 = PA_2, 356 | SENS1_PIN1_ALT0 = PA_2_ALT0, 357 | SENS1_PIN1_ALT1 = PA_2_ALT1, 358 | SENS1_PIN2 = PG_3, 359 | SENS1_PIN3 = PB_8, 360 | SENS1_PIN3_ALT0 = PB_8_ALT0, 361 | SENS1_PIN3_ALT1 = PB_8_ALT1, 362 | SENS1_PIN4 = PB_9, 363 | SENS1_PIN4_ALT0 = PB_9_ALT0, 364 | SENS1_PIN4_ALT1 = PB_9_ALT1, 365 | 366 | SENS2_PIN1 = PA_6, 367 | SENS2_PIN1_ALT0 = PA_6_ALT0, 368 | SENS2_PIN2 = PC_8, 369 | SENS2_PIN2_ALT0 = PC_8_ALT0, 370 | SENS2_PIN3 = PA_8, 371 | SENS2_PIN4 = PC_9, 372 | SENS2_PIN4_ALT0 = PC_9_ALT0, 373 | 374 | SENS3_PIN1 = PC_4, 375 | SENS3_PIN1_ALT0 = PC_4_ALT0, 376 | SENS3_PIN2 = PG_2, 377 | SENS3_PIN3 = PC_12, // UART_5_TX 378 | SENS3_PIN3_UART_TX = PC_12, // UART_5_TX 379 | SENS3_PIN4 = PD_2, // UART_5_RX 380 | SENS3_PIN4_UART_RX = PD_2, // UART_5_RX 381 | 382 | SENS4_PIN1 = PC_5, 383 | SENS4_PIN1_ALT0 = PC_5_ALT0, 384 | SENS4_PIN2 = PD_15, 385 | SENS4_PIN3 = PG_14, // USART_6_TX 386 | SENS4_PIN3_UART_TX = PG_14, // USART_6_TX 387 | SENS4_PIN4 = PG_9, // USART_6_RX 388 | SENS4_PIN4_UART_RX = PG_9, // USART_6_RX 389 | 390 | SENS5_PIN1 = PB_0, 391 | SENS5_PIN1_ALT0 = PB_0_ALT0, 392 | SENS5_PIN1_ALT1 = PB_0_ALT1, 393 | SENS5_PIN2 = PD_14, 394 | SENS5_PIN3 = PD_13, 395 | SENS5_PIN4 = PD_12, 396 | 397 | SENS6_PIN1 = PB_1, 398 | SENS6_PIN2 = PD_8, 399 | SENS6_PIN3 = PD_10, 400 | SENS6_PIN4 = PD_9, 401 | 402 | SENS_POWER_ON = PG_4, // SENSOR 5V TOGGLE 403 | 404 | /**** EXT PORT PINS ****/ 405 | EXT_PIN1 = PF_3, 406 | EXT_PIN2 = PF_10, 407 | EXT_PIN3 = PF_4, 408 | EXT_PIN4 = PF_5, 409 | EXT_PIN5 = PC_0, 410 | EXT_PIN5_ALT0 = PC_0_ALT0, 411 | EXT_PIN5_ALT1 = PC_0_ALT1, 412 | EXT_PIN6 = PD_6, // RX 413 | EXT_PIN7 = PD_5, // TX 414 | EXT_PIN8 = PB_13, // SCK SPI 415 | EXT_PIN9 = PC_2, // MISO SPI 416 | EXT_PIN9_ALT0 = PC_2_ALT0, 417 | EXT_PIN9_ALT1 = PC_2_ALT1, 418 | EXT_PIN10 = PC_3, // MOSI SPI 419 | EXT_PIN10_ALT0 = PC_3_ALT0, 420 | EXT_PIN10_ATL1 = PC_3_ALT1, 421 | EXT_PIN11 = PF_0, // SDA I2C 422 | EXT_PIN12 = PF_1, // SCL I2C 423 | 424 | /**** RPI PORT PINS ****/ 425 | RPI_SERIAL_TX = PA_9, 426 | RPI_SERIAL_RX = PA_10, 427 | RPI_CONSOLE = PG_5, 428 | RPI_STATUS = PG_6, 429 | RPI_BTN = PG_7, 430 | 431 | /**** CAN ****/ 432 | CAN_TX = PD_1, 433 | CAN_RX = PD_0, 434 | CAN_EN = PA_15, 435 | CAN_EN_ALT0 = PA_15_ALT0, 436 | 437 | /**** USB-B/USB-micro FTDI PINS ****/ 438 | FT_SERIAL_TX = PB_10, 439 | FT_SERIAL_RX = PB_11, 440 | FT_CBUS3 = PD_11, 441 | 442 | /**** HOST USB PINS ****/ 443 | USB_DM = PA_11, 444 | USB_DP = PA_12, 445 | USB_CHARGE = PE_8, 446 | USB_CHARGE_IND = PE_7, 447 | 448 | /**** SD READER PINS****/ 449 | SD_MOSI = PB_5_ALT0, 450 | SD_MISO = PC_11, 451 | SD_CLK = PC_10, 452 | SD_CS = PB_3_ALT0, 453 | SD_IN = PG_15, 454 | SD_POWER_ON = PD_7, 455 | 456 | /**** SERVO PINS ****/ 457 | SERVO1_PWM = PE_9, 458 | SERVO2_PWM = PE_11, 459 | SERVO3_PWM = PE_13, 460 | SERVO4_PWM = PE_14, 461 | SERVO5_PWM = PB_15, 462 | SERVO5_PWM_ALT0 = PB_15_ALT0, 463 | SERVO5_PWM_ALT1 = PB_15_ALT1, 464 | SERVO6_PWM = PB_14, 465 | SERVO6_PWM_ALT0 = PB_14_ALT0, 466 | SERVO6_PWM_ALT1 = PB_14_ALT1, 467 | SERVO_ADC = PA_3, 468 | SERVO_ADC_ALT0 = PA_3_ALT0, 469 | SERVO_ADC_ALT1 = PA_3_ALT1, 470 | SERVO_SEL1 = PE_10, 471 | SERVO_SEL2 = PE_12, 472 | SERVO_POWER_ON = PE_15, 473 | 474 | /**** BAT AND BOOT PINS****/ 475 | BAT_MEAS = PA_5, // VIN SUPPLY MEASURMENT 476 | BAT_MEAS_ALT0 = PA_5_ALT0, // UPPER_RESISTOR: 56kOhm 477 | // LOWER_RESISTOR: 10kOhm 478 | // Volts = ((2 * ADC_READ))/2520 * (UPPER_RESISTOR + LOWER_RESISTOR) / LOWER_RESISTOR; 479 | BOOT1 = PB_2, 480 | 481 | /**** MOTOR PINS ****/ 482 | MOT12_SLEEP = PC_13, 483 | MOT12_FAULT = PE_0, 484 | MOT1_PWM = PF_6, 485 | MOT1A_IN = PG_10, 486 | MOT1B_IN = PG_11, 487 | MOT1A_ENC = PA_0, 488 | MOT1A_ENC_ALT0 = PA_0_ALT0, 489 | MOT1A_ENC_ALT1 = PA_0_ALT1, 490 | MOT1B_ENC = PA_1, 491 | MOT2_PWM = PF_7, 492 | MOT2A_IN = PD_3, 493 | MOT2B_IN = PD_4, 494 | MOT2A_ENC = PC_6, 495 | MOT2A_ENC_ALT0 = PC_6_ALT0, 496 | MOT2B_ENC = PC_7, 497 | MOT2B_ENC_ALT0 = PC_7_ALT0, 498 | MOT34_SLEEP = PC_14, 499 | MOT34_FAULT = PE_1, 500 | MOT3_PWM = PF_8, 501 | MOT3A_IN = PC_15, 502 | MOT3B_IN = PF_2, 503 | MOT3A_ENC = PB_4, 504 | MOT3A_ENC_ALT0 = PB_4_ALT0, 505 | MOT3A_ENC_ALT1 = PB_4_ALT1, 506 | MOT3B_ENC = PA_7, 507 | MOT3B_ENC_ALT0 = PA_7_ALT0, 508 | MOT3B_ENC_ALT1 = PA_7_ALT1, 509 | MOT3B_ENC_ALT2 = PA_7_ALT2, 510 | MOT4_PWM = PF_9, 511 | MOT4A_IN = PE_5, 512 | MOT4B_IN = PE_6, 513 | MOT4A_ENC = PB_6, 514 | MOT4B_ENC = PB_7, 515 | 516 | // Not connected 517 | NC = (int)0xFFFFFFFF 518 | } PinName; 519 | 520 | #ifdef __cplusplus 521 | } 522 | #endif 523 | 524 | #endif 525 | -------------------------------------------------------------------------------- /src/TARGET_CORE2/PeripheralPins.c: -------------------------------------------------------------------------------- 1 | /* mbed Microcontroller Library 2 | ******************************************************************************* 3 | * Copyright (c) 2018, STMicroelectronics 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright notice, 10 | * this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright notice, 12 | * this list of conditions and the following disclaimer in the documentation 13 | * and/or other materials provided with the distribution. 14 | * 3. Neither the name of STMicroelectronics nor the names of its contributors 15 | * may be used to endorse or promote products derived from this software 16 | * without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | ******************************************************************************* 29 | */ 30 | 31 | #include "PeripheralPins.h" 32 | #include "mbed_toolchain.h" 33 | 34 | //============================================================================== 35 | // Notes 36 | // 37 | // - The pins mentioned Px_y_ALTz are alternative possibilities which use other 38 | // HW peripheral instances. You can use them the same way as any other "normal" 39 | // pin (i.e. PwmOut pwm(PA_7_ALT0);). These pins are not displayed on the board 40 | // pinout image on mbed.org. 41 | // 42 | // - The pins which are connected to other components present on the board have 43 | // the comment "Connected to xxx". The pin function may not work properly in this 44 | // case. These pins may not be displayed on the board pinout image on mbed.org. 45 | // Please read the board reference manual and schematic for more information. 46 | // 47 | // - Warning: pins connected to the default STDIO_UART_TX and STDIO_UART_RX pins are commented 48 | // See https://os.mbed.com/teams/ST/wiki/STDIO for more information. 49 | // 50 | //============================================================================== 51 | 52 | 53 | //*** ADC *** (24 pins) 54 | 55 | MBED_WEAK const PinMap PinMap_ADC[] = { 56 | {PA_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC1_IN0 57 | {PA_0_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC2_IN0 58 | {PA_0_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 0, 0)}, // ADC3_IN0 59 | {PA_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC1_IN1 60 | {PA_1_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC2_IN1 61 | {PA_1_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // ADC3_IN1 62 | {PA_2, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC1_IN2 63 | {PA_2_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC2_IN2 64 | {PA_2_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // ADC3_IN2 65 | {PA_3, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC1_IN3 66 | {PA_3_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC2_IN3 67 | {PA_3_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 3, 0)}, // ADC3_IN3 68 | {PA_4, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC1_IN4 69 | {PA_4_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC2_IN4 70 | {PA_5, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC1_IN5 71 | {PA_5_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC2_IN5 72 | {PA_6, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC1_IN6 73 | {PA_6_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC2_IN6 74 | {PA_7, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC1_IN7 75 | {PA_7_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC2_IN7 76 | {PB_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC1_IN8 77 | {PB_0_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC2_IN8 78 | {PB_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC1_IN9 79 | {PB_1_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC2_IN9 80 | {PC_0, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC1_IN10 81 | {PC_0_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC2_IN10 82 | {PC_0_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 10, 0)}, // ADC3_IN10 83 | {PC_1, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC1_IN11 84 | {PC_1_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC2_IN11 85 | {PC_1_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 11, 0)}, // ADC3_IN11 86 | {PC_2, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC1_IN12 87 | {PC_2_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC2_IN12 88 | {PC_2_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 12, 0)}, // ADC3_IN12 89 | {PC_3, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC1_IN13 90 | {PC_3_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC2_IN13 91 | {PC_3_ALT1, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 13, 0)}, // ADC3_IN13 92 | {PC_4, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC1_IN14 93 | {PC_4_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC2_IN14 94 | {PC_5, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC1_IN15 95 | {PC_5_ALT0, ADC_2, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC2_IN15 96 | {PF_3, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 9, 0)}, // ADC3_IN9 97 | {PF_4, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 14, 0)}, // ADC3_IN14 98 | {PF_5, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 15, 0)}, // ADC3_IN15 99 | {PF_6, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 4, 0)}, // ADC3_IN4 100 | {PF_7, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 5, 0)}, // ADC3_IN5 101 | {PF_8, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 6, 0)}, // ADC3_IN6 102 | {PF_9, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 7, 0)}, // ADC3_IN7 103 | {PF_10, ADC_3, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 8, 0)}, // ADC3_IN8 104 | 105 | {NC, NC, 0} 106 | }; 107 | 108 | MBED_WEAK const PinMap PinMap_ADC_Internal[] = { 109 | {ADC_TEMP, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 16, 0)}, 110 | {ADC_VREF, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 17, 0)}, 111 | {ADC_VBAT, ADC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 18, 0)}, 112 | {NC, NC, 0} 113 | }; 114 | 115 | //*** DAC *** 116 | 117 | MBED_WEAK const PinMap PinMap_DAC[] = { 118 | {PA_4, DAC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 1, 0)}, // DAC_OUT1 // Connected to I2S3_WS [CS43L22_LRCK] 119 | {PA_5, DAC_1, STM_PIN_DATA_EXT(STM_MODE_ANALOG, GPIO_NOPULL, 0, 2, 0)}, // DAC_OUT2 // Connected to SPI1_SCK [LIS302DL_SCL/SPC] 120 | {NC, NC, 0} 121 | }; 122 | 123 | //*** I2C *** 124 | 125 | MBED_WEAK const PinMap PinMap_I2C_SDA[] = { 126 | {PB_7, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, 127 | {PB_9, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, // Connected to Audio_SDA [CS43L22_SDA] 128 | {PB_11, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, 129 | {PC_9, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, 130 | {PF_0, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, 131 | {NC, NC, 0} 132 | }; 133 | 134 | MBED_WEAK const PinMap PinMap_I2C_SCL[] = { 135 | {PA_8, I2C_3, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C3)}, 136 | {PB_6, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, // Connected to Audio_SCL [CS43L22_SCL] 137 | {PB_8, I2C_1, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C1)}, 138 | {PB_10, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, // Connected to CLK_IN [MP45DT02_CLK] 139 | {PF_1, I2C_2, STM_PIN_DATA(STM_MODE_AF_OD, GPIO_NOPULL, GPIO_AF4_I2C2)}, // Connected to CLK_IN [MP45DT02_CLK] 140 | {NC, NC, 0} 141 | }; 142 | 143 | //*** PWM *** 144 | 145 | // TIM5 cannot be used because already used by the us_ticker 146 | MBED_WEAK const PinMap PinMap_PWM[] = { 147 | {PA_0, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 // Connected to B1 [Blue PushButton] 148 | // {PA_0, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 1, 0)}, // TIM5_CH1 // Connected to B1 [Blue PushButton] 149 | {PA_1, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 150 | // {PA_1, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 2, 0)}, // TIM5_CH2 151 | {PA_2, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 152 | // {PA_2, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 3, 0)}, // TIM5_CH3 153 | {PA_2_ALT0, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 1, 0)}, // TIM9_CH1 154 | {PA_3, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 155 | // {PA_3, PWM_5, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM5, 4, 0)}, // TIM5_CH4 156 | {PA_3_ALT0, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 2, 0)}, // TIM9_CH2 157 | {PA_5, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 // Connected to SPI1_SCK [LIS302DL_SCL/SPC] 158 | {PA_5_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N // Connected to SPI1_SCK [LIS302DL_SCL/SPC] 159 | {PA_6, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 // Connected to SPI1_MISO [LIS302DL_SDO] 160 | {PA_6_ALT0, PWM_13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 // Connected to SPI1_MISO [LIS302DL_SDO] 161 | {PA_7, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N // Connected to SPI1_MOSI [LIS302DL_SDA/SDI/SDO] 162 | {PA_7_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 // Connected to SPI1_MOSI [LIS302DL_SDA/SDI/SDO] 163 | {PA_7_ALT1, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 1)}, // TIM8_CH1N // Connected to SPI1_MOSI [LIS302DL_SDA/SDI/SDO] 164 | {PA_7_ALT2, PWM_14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 // Connected to SPI1_MOSI [LIS302DL_SDA/SDI/SDO] 165 | {PA_8, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 166 | {PA_9, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 // Connected to VBUS_FS 167 | {PA_10, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 // Connected to OTG_FS_ID 168 | {PA_11, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 // Connected to OTG_FS_DM 169 | {PA_15, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 1, 0)}, // TIM2_CH1 170 | {PB_0, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N 171 | {PB_0_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 172 | {PB_0_ALT1, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N 173 | {PB_1, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N 174 | {PB_1_ALT0, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 175 | {PB_1_ALT1, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N 176 | {PB_3, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 2, 0)}, // TIM2_CH2 // Connected to SWO 177 | {PB_4, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 178 | {PB_5, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 179 | {PB_6, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 // Connected to Audio_SCL [CS43L22_SCL] 180 | {PB_7, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 181 | {PB_8, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 182 | {PB_8_ALT0, PWM_10, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM10, 1, 0)}, // TIM10_CH1 183 | {PB_9, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 // Connected to Audio_SDA [CS43L22_SDA] 184 | {PB_9_ALT0, PWM_11, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM11, 1, 0)}, // TIM11_CH1 // Connected to Audio_SDA [CS43L22_SDA] 185 | {PB_10, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 3, 0)}, // TIM2_CH3 // Connected to CLK_IN [MP45DT02_CLK] 186 | {PB_11, PWM_2, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM2, 4, 0)}, // TIM2_CH4 187 | {PB_13, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N 188 | {PB_14, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N 189 | {PB_14_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 1)}, // TIM8_CH2N 190 | {PB_14_ALT1, PWM_12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM12, 1, 0)}, // TIM12_CH1 191 | {PB_15, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N 192 | {PB_15_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 1)}, // TIM8_CH3N 193 | {PB_15_ALT1, PWM_12, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM12, 2, 0)}, // TIM12_CH2 194 | {PC_6, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 1, 0)}, // TIM3_CH1 195 | {PC_6_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 1, 0)}, // TIM8_CH1 196 | {PC_7, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 2, 0)}, // TIM3_CH2 // Connected to I2S3_MCK [CS43L22_MCLK] 197 | {PC_7_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 2, 0)}, // TIM8_CH2 // Connected to I2S3_MCK [CS43L22_MCLK] 198 | {PC_8, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 3, 0)}, // TIM3_CH3 199 | {PC_8_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 3, 0)}, // TIM8_CH3 200 | {PC_9, PWM_3, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM3, 4, 0)}, // TIM3_CH4 201 | {PC_9_ALT0, PWM_8, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM8, 4, 0)}, // TIM8_CH4 202 | {PD_12, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 1, 0)}, // TIM4_CH1 // Connected to LD4 [Green Led] 203 | {PD_13, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 2, 0)}, // TIM4_CH2 // Connected to LD3 [Orange Led] 204 | {PD_14, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 3, 0)}, // TIM4_CH3 // Connected to LD5 [Red Led] 205 | {PD_15, PWM_4, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF2_TIM4, 4, 0)}, // TIM4_CH4 // Connected to LD6 [Blue Led] 206 | {PE_5, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 1, 0)}, // TIM9_CH1 207 | {PE_6, PWM_9, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM9, 2, 0)}, // TIM9_CH2 208 | {PE_8, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 1)}, // TIM1_CH1N 209 | {PE_9, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 1, 0)}, // TIM1_CH1 210 | {PE_10, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 1)}, // TIM1_CH2N 211 | {PE_11, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 2, 0)}, // TIM1_CH2 212 | {PE_12, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 1)}, // TIM1_CH3N 213 | {PE_13, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 3, 0)}, // TIM1_CH3 214 | {PE_14, PWM_1, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF1_TIM1, 4, 0)}, // TIM1_CH4 215 | {PF_6, PWM_10, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM10, 1, 0)}, // TIM10_CH1 216 | {PF_7, PWM_11, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF3_TIM11, 1, 0)}, // TIM11_CH1 217 | {PF_8, PWM_13, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM13, 1, 0)}, // TIM13_CH1 218 | {PF_9, PWM_14, STM_PIN_DATA_EXT(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF9_TIM14, 1, 0)}, // TIM14_CH1 219 | {NC, NC, 0} 220 | }; 221 | 222 | //*** SERIAL *** 223 | 224 | MBED_WEAK const PinMap PinMap_UART_TX[] = { 225 | {PA_0, UART_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, // Connected to B1 [Blue PushButton] 226 | {PA_2, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, 227 | {PA_9, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to VBUS_FS 228 | {PB_6, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to Audio_SCL [CS43L22_SCL] 229 | {PB_10, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, // Connected to CLK_IN [MP45DT02_CLK] 230 | {PC_6, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 231 | {PC_10, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, // Connected to I2S3_SCK [CS43L22_SCLK] 232 | {PC_10_ALT0, UART_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, // Connected to I2S3_SCK [CS43L22_SCLK] 233 | {PC_12, UART_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, // Connected to I2S3_SD [CS43L22_SDIN] 234 | {PD_5, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, // Connected to OTG_FS_OverCurrent 235 | {PD_8, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 236 | {PG_14, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 237 | {NC, NC, 0} 238 | }; 239 | 240 | MBED_WEAK const PinMap PinMap_UART_RX[] = { 241 | {PA_1, UART_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, 242 | {PA_3, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, 243 | {PA_10, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to OTG_FS_ID 244 | {PB_7, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, 245 | {PB_11, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 246 | {PC_7, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, // Connected to I2S3_MCK [CS43L22_MCLK] 247 | {PC_11, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 248 | {PC_11_ALT0, UART_4, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART4)}, 249 | {PD_2, UART_5, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_UART5)}, 250 | {PD_6, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, 251 | {PD_9, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 252 | {PG_9, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 253 | {NC, NC, 0} 254 | }; 255 | 256 | MBED_WEAK const PinMap PinMap_UART_RTS[] = { 257 | {PA_1, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, 258 | {PA_12, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to OTG_FS_DP 259 | {PB_14, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 260 | {PD_4, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, // Connected to Audio_RST [CS43L22_RESET] 261 | {PD_12, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, // Connected to LD4 [Green Led] 262 | {PG_8, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 263 | {PG_12, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 264 | {NC, NC, 0} 265 | }; 266 | 267 | MBED_WEAK const PinMap PinMap_UART_CTS[] = { 268 | {PA_0, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, // Connected to B1 [Blue PushButton] 269 | {PA_11, UART_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART1)}, // Connected to OTG_FS_DM 270 | {PB_13, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 271 | {PD_3, UART_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART2)}, 272 | {PD_11, UART_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF7_USART3)}, 273 | {PG_13, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 274 | {PG_15, UART_6, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_PULLUP, GPIO_AF8_USART6)}, 275 | {NC, NC, 0} 276 | }; 277 | 278 | //*** SPI *** 279 | 280 | MBED_WEAK const PinMap PinMap_SPI_MOSI[] = { 281 | {PA_7, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to SPI1_MOSI [LIS302DL_SDA/SDI/SDO] 282 | {PB_5, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, 283 | {PB_5_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, 284 | {PB_15, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, 285 | {PC_3, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, // Connected to PDM_OUT [MP45DT02_DOUT] 286 | {PC_12, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, // Connected to I2S3_SD [CS43L22_SDIN] 287 | {NC, NC, 0} 288 | }; 289 | 290 | MBED_WEAK const PinMap PinMap_SPI_MISO[] = { 291 | {PA_6, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to SPI1_MISO [LIS302DL_SDO] 292 | {PB_4, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, 293 | {PB_4_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, 294 | {PB_14, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, 295 | {PC_2, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, 296 | {PC_11, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, 297 | {NC, NC, 0} 298 | }; 299 | 300 | MBED_WEAK const PinMap PinMap_SPI_SCLK[] = { 301 | {PA_5, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to SPI1_SCK [LIS302DL_SCL/SPC] 302 | {PB_3, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to SWO 303 | {PB_3_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, // Connected to SWO 304 | {PB_10, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, // Connected to CLK_IN [MP45DT02_CLK] 305 | {PB_13, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, 306 | {PC_10, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, // Connected to I2S3_SCK [CS43L22_SCLK] 307 | {NC, NC, 0} 308 | }; 309 | 310 | MBED_WEAK const PinMap PinMap_SPI_SSEL[] = { 311 | {PA_4, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, // Connected to I2S3_WS [CS43L22_LRCK] 312 | {PA_4_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, // Connected to I2S3_WS [CS43L22_LRCK] 313 | {PA_15, SPI_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI1)}, 314 | {PA_15_ALT0, SPI_3, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF6_SPI3)}, 315 | {PB_9, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, // Connected to Audio_SDA [CS43L22_SDA] 316 | {PB_12, SPI_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF5_SPI2)}, 317 | {NC, NC, 0} 318 | }; 319 | 320 | //*** CAN *** 321 | 322 | MBED_WEAK const PinMap PinMap_CAN_RD[] = { 323 | {PA_11, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, // Connected to OTG_FS_DM 324 | {PB_5, CAN_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN2)}, 325 | {PB_8, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, 326 | {PB_12, CAN_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN2)}, 327 | {PD_0, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, 328 | {NC, NC, 0} 329 | }; 330 | 331 | MBED_WEAK const PinMap PinMap_CAN_TD[] = { 332 | {PA_12, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, // Connected to OTG_FS_DP 333 | {PB_6, CAN_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN2)}, // Connected to Audio_SCL [CS43L22_SCL] 334 | {PB_9, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, // Connected to Audio_SDA [CS43L22_SDA] 335 | {PB_13, CAN_2, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN2)}, 336 | {PD_1, CAN_1, STM_PIN_DATA(STM_MODE_AF_PP, GPIO_NOPULL, GPIO_AF9_CAN1)}, 337 | {NC, NC, 0} 338 | }; 339 | --------------------------------------------------------------------------------