├── .cproject ├── .gitignore ├── .project ├── .settings └── language.settings.xml ├── .travis.yml ├── LICENSE ├── README.md ├── app ├── app_jcesc_1_0.ld ├── app_miniquad_1_0.ld ├── app_px4esc_1_6.ld ├── app_s2740vc_1_0.ld ├── include │ ├── can.h │ ├── cmsis_stm32f302.h │ ├── cmsis_stm32f446.h │ ├── configuration.h │ ├── controller.h │ ├── core_cm4.h │ ├── core_cm4_simd.h │ ├── core_cmFunc.h │ ├── core_cmInstr.h │ ├── drv8305.h │ ├── esc_assert.h │ ├── estimator.h │ ├── fixed.h │ ├── hal.h │ ├── park.h │ ├── perf.h │ ├── shared.h │ ├── svm.h │ └── uavcan.h └── src │ ├── _cxx.cpp │ ├── app_vectors.c │ ├── can.cpp │ ├── configuration.cpp │ ├── controller.cpp │ ├── estimator.cpp │ ├── hal.cpp │ ├── hal_jcesc_1_0.cpp │ ├── hal_miniquad_1_0.cpp │ ├── hal_px4esc_1_6.cpp │ ├── hal_s2740vc_1_0.cpp │ ├── m4f_float_spec.cpp │ ├── main.cpp │ ├── perf.cpp │ └── shared.cpp ├── arch ├── include │ ├── irq.h │ ├── irq_base.h │ ├── irq_cmnvector.h │ ├── irq_lazyfpu.h │ ├── stm32_chip.h │ ├── stm32f10xxx_irq.h │ ├── stm32f20xxx_irq.h │ ├── stm32f30xxx_irq.h │ ├── stm32f37xxx_irq.h │ ├── stm32f40xxx_irq.h │ ├── stm32f42xxx_irq.h │ ├── stm32f44xxx_irq.h │ ├── stm32l15xxx_irq.h │ └── types.h └── src │ ├── chip.h │ ├── chip │ ├── stm32_adc.h │ ├── stm32_bkp.h │ ├── stm32_can.h │ ├── stm32_dac.h │ ├── stm32_dbgmcu.h │ ├── stm32_dma2d.h │ ├── stm32_eth.h │ ├── stm32_exti.h │ ├── stm32_flash.h │ ├── stm32_i2c.h │ ├── stm32_lcd.h │ ├── stm32_ltdc.h │ ├── stm32_memorymap.h │ ├── stm32_otgfs.h │ ├── stm32_otghs.h │ ├── stm32_pwr.h │ ├── stm32_rng.h │ ├── stm32_rtc.h │ ├── stm32_rtcc.h │ ├── stm32_sdio.h │ ├── stm32_spi.h │ ├── stm32_tim.h │ ├── stm32_usbdev.h │ ├── stm32_wdg.h │ ├── stm32f100_pinmap.h │ ├── stm32f102_pinmap.h │ ├── stm32f103c_pinmap.h │ ├── stm32f103r_pinmap.h │ ├── stm32f103v_pinmap.h │ ├── stm32f103z_pinmap.h │ ├── stm32f105v_pinmap.h │ ├── stm32f107v_pinmap.h │ ├── stm32f10xxx_dma.h │ ├── stm32f10xxx_gpio.h │ ├── stm32f10xxx_memorymap.h │ ├── stm32f10xxx_rcc.h │ ├── stm32f10xxx_uart.h │ ├── stm32f10xxx_vectors.h │ ├── stm32f20xxx_dma.h │ ├── stm32f20xxx_gpio.h │ ├── stm32f20xxx_memorymap.h │ ├── stm32f20xxx_pinmap.h │ ├── stm32f20xxx_rcc.h │ ├── stm32f20xxx_syscfg.h │ ├── stm32f20xxx_uart.h │ ├── stm32f20xxx_vectors.h │ ├── stm32f30xxx_adc.h │ ├── stm32f30xxx_gpio.h │ ├── stm32f30xxx_i2c.h │ ├── stm32f30xxx_memorymap.h │ ├── stm32f30xxx_pinmap.h │ ├── stm32f30xxx_rcc.h │ ├── stm32f30xxx_syscfg.h │ ├── stm32f30xxx_uart.h │ ├── stm32f30xxx_vectors.h │ ├── stm32f37xxx_memorymap.h │ ├── stm32f37xxx_pinmap.h │ ├── stm32f37xxx_rcc.h │ ├── stm32f37xxx_syscfg.h │ ├── stm32f37xxx_vectors.h │ ├── stm32f40xxx_dma.h │ ├── stm32f40xxx_gpio.h │ ├── stm32f40xxx_memorymap.h │ ├── stm32f40xxx_pinmap.h │ ├── stm32f40xxx_rcc.h │ ├── stm32f40xxx_syscfg.h │ ├── stm32f40xxx_uart.h │ ├── stm32f40xxx_vectors.h │ ├── stm32f42xxx_dma.h │ ├── stm32f42xxx_pinmap.h │ ├── stm32f42xxx_rcc.h │ ├── stm32f42xxx_syscfg.h │ ├── stm32f42xxx_vectors.h │ ├── stm32f44xxx_dma.h │ ├── stm32f44xxx_pinmap.h │ ├── stm32f44xxx_rcc.h │ ├── stm32f44xxx_syscfg.h │ ├── stm32f44xxx_vectors.h │ ├── stm32l15xxx_aes.h │ ├── stm32l15xxx_gpio.h │ ├── stm32l15xxx_memorymap.h │ ├── stm32l15xxx_pinmap.h │ ├── stm32l15xxx_rcc.h │ ├── stm32l15xxx_syscfg.h │ ├── stm32l15xxx_uart.h │ └── stm32l15xxx_vectors.h │ ├── dwt.h │ ├── etm.h │ ├── exc_return.h │ ├── irq_attach.c │ ├── irq_dispatch.c │ ├── irq_initialize.c │ ├── irq_unexpectedisr.c │ ├── libc.c │ ├── mpu.h │ ├── nvic.h │ ├── psr.h │ ├── stm32.h │ ├── stm32_dbgmcu.h │ ├── stm32_flash.c │ ├── stm32_flash.h │ ├── stm32_gpio.c │ ├── stm32_gpio.h │ ├── stm32_irq.c │ ├── stm32_rcc.c │ ├── stm32_rcc.h │ ├── stm32_start.c │ ├── stm32_syscfg.h │ ├── stm32_timerisr.c │ ├── stm32_vectors.S │ ├── stm32f10xxx_rcc.c │ ├── stm32f20xxx_rcc.c │ ├── stm32f30xxx_rcc.c │ ├── stm32f37xxx_rcc.c │ ├── stm32f40xxx_rcc.c │ ├── stm32f42xxx_rcc.c │ ├── stm32f44xxx_rcc.c │ ├── stm32l15xxx_rcc.c │ ├── up_arch.h │ ├── up_assert.c │ ├── up_cxxinitialize.c │ ├── up_doirq.c │ ├── up_exit.c │ ├── up_fpu.S │ ├── up_hardfault.c │ ├── up_idle.c │ ├── up_internal.h │ ├── up_modifyreg16.c │ ├── up_modifyreg32.c │ ├── up_modifyreg8.c │ ├── up_progmem.h │ └── up_systemreset.c ├── bootloader ├── bootloader.ld ├── include │ ├── bitmanip.h │ ├── can.h │ ├── crc.h │ ├── flash.h │ ├── macros.h │ ├── random.h │ ├── shared.h │ ├── timer.h │ ├── uavcan.h │ ├── uavcan_can_id_defs.h │ └── uavcan_dsdl_defs.h └── src │ ├── can.c │ ├── crc.c │ ├── flash.c │ ├── main.c │ ├── random.c │ ├── shared.c │ ├── timer.c │ └── uavcan.c ├── config_jcesc_1_0.h ├── config_miniquad_1_0.h ├── config_px4esc_1_6.h ├── config_s2740vc_1_0.h ├── doc └── S2740VC.html ├── dsdl ├── readme.txt └── thiemar │ └── equipment │ └── esc │ └── 20034.Status.uavcan ├── jcesc.launch ├── libuavcan ├── include │ ├── dsdlc_generated │ │ ├── thiemar │ │ │ └── equipment │ │ │ │ └── esc │ │ │ │ └── Status.hpp │ │ └── uavcan │ │ │ ├── CoarseOrientation.hpp │ │ │ ├── Timestamp.hpp │ │ │ ├── equipment │ │ │ ├── actuator │ │ │ │ ├── ArrayCommand.hpp │ │ │ │ ├── Command.hpp │ │ │ │ └── Status.hpp │ │ │ ├── ahrs │ │ │ │ ├── MagneticFieldStrength.hpp │ │ │ │ └── Solution.hpp │ │ │ ├── air_data │ │ │ │ ├── AngleOfAttack.hpp │ │ │ │ ├── IndicatedAirspeed.hpp │ │ │ │ ├── Sideslip.hpp │ │ │ │ ├── StaticPressure.hpp │ │ │ │ ├── StaticTemperature.hpp │ │ │ │ └── TrueAirspeed.hpp │ │ │ ├── camera_gimbal │ │ │ │ ├── AngularCommand.hpp │ │ │ │ ├── GEOPOICommand.hpp │ │ │ │ ├── Mode.hpp │ │ │ │ └── Status.hpp │ │ │ ├── esc │ │ │ │ ├── RPMCommand.hpp │ │ │ │ ├── RawCommand.hpp │ │ │ │ └── Status.hpp │ │ │ ├── gnss │ │ │ │ ├── Auxiliary.hpp │ │ │ │ ├── Fix.hpp │ │ │ │ └── RTCMStream.hpp │ │ │ ├── hardpoint │ │ │ │ ├── Command.hpp │ │ │ │ └── Status.hpp │ │ │ ├── indication │ │ │ │ ├── BeepCommand.hpp │ │ │ │ ├── LightsCommand.hpp │ │ │ │ ├── RGB565.hpp │ │ │ │ └── SingleLightCommand.hpp │ │ │ ├── power │ │ │ │ ├── BatteryInfo.hpp │ │ │ │ ├── CircuitStatus.hpp │ │ │ │ └── PrimaryPowerSupplyStatus.hpp │ │ │ ├── range_sensor │ │ │ │ └── Measurement.hpp │ │ │ └── safety │ │ │ │ └── ArmingStatus.hpp │ │ │ └── protocol │ │ │ ├── AccessCommandShell.hpp │ │ │ ├── CANIfaceStats.hpp │ │ │ ├── DataTypeKind.hpp │ │ │ ├── GetDataTypeInfo.hpp │ │ │ ├── GetNodeInfo.hpp │ │ │ ├── GetTransportStats.hpp │ │ │ ├── GlobalTimeSync.hpp │ │ │ ├── HardwareVersion.hpp │ │ │ ├── NodeStatus.hpp │ │ │ ├── Panic.hpp │ │ │ ├── RestartNode.hpp │ │ │ ├── SoftwareVersion.hpp │ │ │ ├── debug │ │ │ ├── KeyValue.hpp │ │ │ ├── LogLevel.hpp │ │ │ └── LogMessage.hpp │ │ │ ├── dynamic_node_id │ │ │ ├── Allocation.hpp │ │ │ └── server │ │ │ │ ├── AppendEntries.hpp │ │ │ │ ├── Discovery.hpp │ │ │ │ ├── Entry.hpp │ │ │ │ └── RequestVote.hpp │ │ │ ├── enumeration │ │ │ ├── Begin.hpp │ │ │ └── Indication.hpp │ │ │ ├── file │ │ │ ├── BeginFirmwareUpdate.hpp │ │ │ ├── Delete.hpp │ │ │ ├── EntryType.hpp │ │ │ ├── Error.hpp │ │ │ ├── GetDirectoryEntryInfo.hpp │ │ │ ├── GetInfo.hpp │ │ │ ├── Path.hpp │ │ │ ├── Read.hpp │ │ │ └── Write.hpp │ │ │ └── param │ │ │ ├── Empty.hpp │ │ │ ├── ExecuteOpcode.hpp │ │ │ ├── GetSet.hpp │ │ │ ├── NumericValue.hpp │ │ │ └── Value.hpp │ └── uavcan │ │ ├── build_config.hpp │ │ ├── data_type.hpp │ │ ├── debug.hpp │ │ ├── driver │ │ ├── can.hpp │ │ └── system_clock.hpp │ │ ├── dynamic_memory.hpp │ │ ├── error.hpp │ │ ├── helpers │ │ ├── heap_based_pool_allocator.hpp │ │ └── ostream.hpp │ │ ├── marshal │ │ ├── array.hpp │ │ ├── bit_stream.hpp │ │ ├── char_array_formatter.hpp │ │ ├── float_spec.hpp │ │ ├── integer_spec.hpp │ │ ├── scalar_codec.hpp │ │ ├── type_util.hpp │ │ └── types.hpp │ │ ├── node │ │ ├── abstract_node.hpp │ │ ├── generic_publisher.hpp │ │ ├── generic_subscriber.hpp │ │ ├── global_data_type_registry.hpp │ │ ├── node.hpp │ │ ├── publisher.hpp │ │ ├── scheduler.hpp │ │ ├── service_client.hpp │ │ ├── service_server.hpp │ │ ├── sub_node.hpp │ │ ├── subscriber.hpp │ │ └── timer.hpp │ │ ├── protocol │ │ ├── README.md │ │ ├── data_type_info_provider.hpp │ │ ├── dynamic_node_id_client.hpp │ │ ├── dynamic_node_id_server │ │ │ ├── allocation_request_manager.hpp │ │ │ ├── centralized.hpp │ │ │ ├── centralized │ │ │ │ ├── server.hpp │ │ │ │ └── storage.hpp │ │ │ ├── distributed.hpp │ │ │ ├── distributed │ │ │ │ ├── cluster_manager.hpp │ │ │ │ ├── log.hpp │ │ │ │ ├── persistent_state.hpp │ │ │ │ ├── raft_core.hpp │ │ │ │ ├── server.hpp │ │ │ │ └── types.hpp │ │ │ ├── event.hpp │ │ │ ├── node_discoverer.hpp │ │ │ ├── node_id_selector.hpp │ │ │ ├── storage_backend.hpp │ │ │ ├── storage_marshaller.hpp │ │ │ └── types.hpp │ │ ├── file_server.hpp │ │ ├── firmware_update_trigger.hpp │ │ ├── global_time_sync_master.hpp │ │ ├── global_time_sync_slave.hpp │ │ ├── logger.hpp │ │ ├── node_info_retriever.hpp │ │ ├── node_status_monitor.hpp │ │ ├── node_status_provider.hpp │ │ ├── panic_broadcaster.hpp │ │ ├── panic_listener.hpp │ │ ├── param_server.hpp │ │ ├── restart_request_server.hpp │ │ └── transport_stats_provider.hpp │ │ ├── std.hpp │ │ ├── time.hpp │ │ ├── transport │ │ ├── abstract_transfer_buffer.hpp │ │ ├── can_acceptance_filter_configurator.hpp │ │ ├── can_io.hpp │ │ ├── crc.hpp │ │ ├── dispatcher.hpp │ │ ├── frame.hpp │ │ ├── outgoing_transfer_registry.hpp │ │ ├── perf_counter.hpp │ │ ├── transfer.hpp │ │ ├── transfer_buffer.hpp │ │ ├── transfer_listener.hpp │ │ ├── transfer_receiver.hpp │ │ └── transfer_sender.hpp │ │ ├── uavcan.hpp │ │ └── util │ │ ├── bitset.hpp │ │ ├── comparison.hpp │ │ ├── lazy_constructor.hpp │ │ ├── linked_list.hpp │ │ ├── map.hpp │ │ ├── method_binder.hpp │ │ ├── multiset.hpp │ │ ├── placement_new.hpp │ │ └── templates.hpp └── src │ ├── driver │ └── uc_can.cpp │ ├── marshal │ ├── uc_bit_array_copy.cpp │ ├── uc_bit_stream.cpp │ ├── uc_float_spec.cpp │ └── uc_scalar_codec.cpp │ ├── node │ ├── uc_generic_publisher.cpp │ ├── uc_generic_subscriber.cpp │ ├── uc_global_data_type_registry.cpp │ ├── uc_scheduler.cpp │ ├── uc_service_client.cpp │ └── uc_timer.cpp │ ├── protocol │ ├── uc_dynamic_node_id_client.cpp │ └── uc_node_status_provider.cpp │ ├── transport │ ├── uc_can_acceptance_filter_configurator.cpp │ ├── uc_can_io.cpp │ ├── uc_crc.cpp │ ├── uc_dispatcher.cpp │ ├── uc_frame.cpp │ ├── uc_outgoing_transfer_registry.cpp │ ├── uc_transfer.cpp │ ├── uc_transfer_buffer.cpp │ ├── uc_transfer_listener.cpp │ ├── uc_transfer_receiver.cpp │ └── uc_transfer_sender.cpp │ ├── uc_data_type.cpp │ ├── uc_dynamic_memory.cpp │ └── uc_error.cpp ├── makefile ├── openocd_jcesc_1_0.cfg ├── openocd_miniquad_1_0.cfg ├── openocd_px4esc_1_6.cfg ├── openocd_s2740vc_1_0.cfg ├── px4esc.launch ├── tools ├── assets │ ├── bwtest.html │ ├── bwtest.js │ ├── d3.min.js │ ├── ui.html │ └── ui.js ├── bwtest.py ├── make_can_boot_descriptor.py └── ui.py ├── uavcan_ident ├── jcesc-v1-1.0.mk ├── miniquad-v1-1.0.mk ├── px4esc-v1-1.6.mk └── s2740vc-v1-1.0.mk ├── vectorcontrol.launch └── vectorcontrol2 debug.launch /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | /Debug/ 30 | /Release/ 31 | 32 | # Other junk 33 | core 34 | *.pyc 35 | .DS_Store 36 | /Debug/ 37 | /build 38 | /firmware 39 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | vectorcontrol 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | clean,full,incremental, 11 | 12 | 13 | 14 | 15 | org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder 16 | full,incremental, 17 | 18 | 19 | 20 | 21 | 22 | org.eclipse.cdt.core.cnature 23 | org.eclipse.cdt.core.ccnature 24 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 25 | org.eclipse.cdt.managedbuilder.core.ScannerConfigNature 26 | 27 | 28 | -------------------------------------------------------------------------------- /.settings/language.settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | env: 3 | matrix: 4 | - TARGET=gcc-4.8 BOARD=s2740vc_1_0 5 | - TARGET=gcc-4.8 BOARD=px4esc_1_6 6 | - TARGET=gcc-4.8 BOARD=jcesc_1_0 7 | - TARGET=gcc-4.9 BOARD=s2740vc_1_0 8 | - TARGET=gcc-4.9 BOARD=px4esc_1_6 9 | - TARGET=gcc-4.9 BOARD=jcesc_1_0 10 | addons: 11 | apt: 12 | sources: 13 | - ubuntu-toolchain-r-test 14 | packages: 15 | - build-essential 16 | - libc6-i386 17 | - libncurses5-dev 18 | before_install: 19 | - git submodule update --init --recursive 20 | - sudo add-apt-repository ppa:ubuntu-toolchain-r/test -y 21 | - sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded -y 22 | - sudo apt-get update -qq 23 | before_script: 24 | - if [ "${TARGET}" == "gcc-4.8" ]; then pushd . && cd ~ && wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 && export PATH="$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:$PATH" && popd ; fi 25 | - if [ "${TARGET}" == "gcc-4.9" ]; then sudo apt-get install gcc-arm-none-eabi=4.9.3.2015q3-1precise1 ; fi 26 | script: 27 | - arm-none-eabi-gcc --version 28 | - make -j8 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (C) 2014-2015 Thiemar Pty Ltd 2 | 3 | Permission is hereby granted, free of charge, to any person obtaining a copy 4 | of this software and associated documentation files (the "Software"), to deal 5 | in the Software without restriction, including without limitation the rights 6 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | copies of the Software, and to permit persons to whom the Software is 8 | furnished to do so, subject to the following conditions: 9 | 10 | The above copyright notice and this permission notice shall be included in 11 | all copies or substantial portions of the Software. 12 | 13 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | SOFTWARE. 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # vectorcontrol 2 | Vector control (FOC) firmware for the S2740VC ESC. 3 | -------------------------------------------------------------------------------- /app/include/can.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: Ben Dyer 5 | * Pavel Kirienko 6 | * David Sidrane 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | #pragma once 38 | 39 | typedef enum { 40 | CAN_UNKNOWN = 0, 41 | CAN_125KBAUD = 1, 42 | CAN_250KBAUD = 2, 43 | CAN_500KBAUD = 3, 44 | CAN_1MBAUD = 4 45 | } can_speed_t; 46 | 47 | 48 | typedef enum { 49 | CAN_Mode_Normal = 0, // Bits 30 and 31 00 50 | CAN_Mode_LoopBack = 1, // Bit 30: Loop Back Mode (Debug) 51 | CAN_Mode_Silent = 2, // Bit 31: Silent Mode (Debug) 52 | CAN_Mode_Silent_LoopBack = 3 // Bits 30 and 31 11 53 | } can_mode_t; 54 | 55 | 56 | int can_init(int freq); 57 | bool can_is_ready(uint8_t mailbox); 58 | void can_tx( 59 | uint8_t mailbox, 60 | uint32_t message_id, 61 | size_t length, 62 | const uint8_t *message 63 | ); 64 | bool can_rx( 65 | uint8_t fifo, 66 | uint8_t *filter_id, 67 | uint32_t *message_id, 68 | size_t *length, 69 | uint8_t *message 70 | ); 71 | void can_set_dtid_filter( 72 | uint8_t fifo, 73 | uint8_t filter_id, 74 | bool is_service, 75 | uint16_t dtid, 76 | uint8_t node_id 77 | ); 78 | -------------------------------------------------------------------------------- /app/include/configuration.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | 26 | #define FLASH_PARAM_VERSION 5u 27 | 28 | 29 | #define PARAM_NAME_MAX_LEN 92u 30 | 31 | 32 | #include 33 | #include "fixed.h" 34 | 35 | 36 | struct bootloader_app_descriptor { 37 | uint64_t signature; 38 | uint64_t image_crc; 39 | uint32_t image_size; 40 | uint32_t vcs_commit; 41 | uint8_t major_version; 42 | uint8_t minor_version; 43 | uint8_t reserved[6]; 44 | } __attribute__((packed)); 45 | 46 | 47 | /* Up to */ 48 | enum param_index_t { 49 | PARAM_UAVCAN_ESC_INDEX = 0, 50 | PARAM_UAVCAN_ESCSTATUS_INTERVAL, 51 | PARAM_THIEMAR_STATUS_INTERVAL, 52 | PARAM_THIEMAR_STATUS_ID, 53 | PARAM_MOTOR_NUM_POLES, 54 | PARAM_MOTOR_I_MAX, 55 | PARAM_MOTOR_V_MAX, 56 | PARAM_MOTOR_V_ACCEL, 57 | PARAM_MOTOR_RS, 58 | PARAM_MOTOR_LS, 59 | PARAM_MOTOR_KV, 60 | PARAM_CONTROL_BANDWIDTH, 61 | PARAM_CONTROL_GAIN, 62 | PARAM_CONTROL_DIRECTION, 63 | PARAM_CONTROL_BRAKING, 64 | NUM_PARAMS 65 | }; 66 | 67 | 68 | enum param_type_t { 69 | PARAM_TYPE_INT = 0, 70 | PARAM_TYPE_FLOAT 71 | }; 72 | 73 | 74 | struct param_t { 75 | uint8_t index; 76 | uint8_t public_type; 77 | const char* name; 78 | float default_value; 79 | float min_value; 80 | float max_value; 81 | }; 82 | 83 | 84 | class Configuration { 85 | private: 86 | float params_[NUM_PARAMS]; 87 | 88 | public: 89 | Configuration(void); 90 | 91 | void read_motor_params(struct motor_params_t& params); 92 | void read_control_params(struct control_params_t& params); 93 | 94 | bool get_param_by_name(struct param_t& out_param, const char* name); 95 | bool get_param_by_index(struct param_t& out_param, uint8_t index); 96 | 97 | bool set_param_value_by_name(const char* name, float value); 98 | bool set_param_value_by_index(uint8_t index, float value); 99 | 100 | float get_param_value_by_index(uint8_t index) { 101 | if (index >= NUM_PARAMS) { 102 | return std::numeric_limits::signaling_NaN(); 103 | } else { 104 | return params_[index]; 105 | } 106 | } 107 | 108 | void reset_params(void); 109 | void write_params(void); 110 | }; 111 | -------------------------------------------------------------------------------- /app/include/controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "fixed.h" 26 | #include "esc_assert.h" 27 | 28 | 29 | class DQCurrentController { 30 | /* Integral D and Q current errors */ 31 | float integral_vd_v_; 32 | float integral_vq_v_; 33 | 34 | /* Total current setpoint */ 35 | float i_setpoint_a_; 36 | 37 | /* PI coefficients for the current controllers. */ 38 | float kp_; 39 | float ki_; 40 | 41 | /* Feed-forward coefficients */ 42 | float ls_h_; 43 | 44 | /* 45 | Motor voltage limit (will be constrained to the lower of this and 46 | 0.95 * Vbus) 47 | */ 48 | float v_limit_v_; 49 | float accel_current_limit_a_; 50 | 51 | public: 52 | DQCurrentController(void): 53 | integral_vd_v_(0.0f), 54 | integral_vq_v_(0.0f), 55 | i_setpoint_a_(0.0f), 56 | kp_(0.0f), 57 | ki_(0.0f), 58 | ls_h_(0.0f), 59 | v_limit_v_(0.125f), 60 | accel_current_limit_a_(0.0f) 61 | {} 62 | 63 | void reset_state(void) { 64 | i_setpoint_a_ = 0.0f; 65 | integral_vd_v_ = 0.0f; 66 | integral_vq_v_ = 0.0f; 67 | } 68 | 69 | void set_setpoint(float s_current_a) { 70 | i_setpoint_a_ = s_current_a; 71 | } 72 | 73 | void set_v_limit_v(float limit) { 74 | v_limit_v_ = limit; 75 | } 76 | 77 | void set_params( 78 | const struct motor_params_t& params, 79 | const struct control_params_t& control_params, 80 | float t_s 81 | ) volatile { 82 | float bandwidth_rad_per_s; 83 | 84 | ls_h_ = params.ls_h; 85 | 86 | /* 87 | Convert control bandwidth to rad/s. Current controller bandwidth will 88 | be set to 10x this value. 89 | */ 90 | bandwidth_rad_per_s = float(2.0 * M_PI) * control_params.bandwidth_hz; 91 | 92 | /* 93 | Parameter selection described in: 94 | 95 | Harnefors, L. and Nee, H-P. (1998) 96 | "Model-Based Current Control of AC Machines Using the Internal Model 97 | Control Method", 98 | 99 | http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=658735 100 | 101 | Equation (19): 102 | 103 | Kp = wc * Ls 104 | Ti = Ls / Rs 105 | Ki = 1 / Ti 106 | 107 | Scale Ki by t_s to avoid an extra multiplication each timestep. 108 | */ 109 | kp_ = ls_h_ * bandwidth_rad_per_s * 10.0f; 110 | ki_ = t_s * params.rs_r / ls_h_; 111 | 112 | /* Set modulation limit */ 113 | v_limit_v_ = params.max_voltage_v; 114 | 115 | accel_current_limit_a_ = params.accel_voltage_v / params.rs_r; 116 | } 117 | 118 | void update( 119 | float out_v_dq_v[2], 120 | const float i_dq_a[2], 121 | float angular_velocity_frac_per_timestep, 122 | float vbus_v, 123 | float audio_v 124 | ); 125 | }; 126 | 127 | -------------------------------------------------------------------------------- /app/include/esc_assert.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #ifdef NDEBUG 26 | #define esc_assert(__e) ((void)0) 27 | #else 28 | #define esc_assert(__e) ((__e) ? (void)0 : __esc_assert_func (__FILE__, __LINE__, \ 29 | __ASSERT_FUNC, #__e)) 30 | 31 | #ifndef __ASSERT_FUNC 32 | /* Use g++'s demangled names in C++. */ 33 | #if defined __cplusplus && defined __GNUC__ 34 | #define __ASSERT_FUNC __PRETTY_FUNCTION__ 35 | 36 | /* C99 requires the use of __func__, gcc also supports it. */ 37 | #elif defined __GNUC__ || __STDC_VERSION__ >= 199901L 38 | #define __ASSERT_FUNC __func__ 39 | /* failed to detect __func__ support. */ 40 | #else 41 | #define __ASSERT_FUNC ((char *) 0) 42 | #endif 43 | #endif 44 | #endif 45 | 46 | extern void 47 | __attribute__((noreturn)) 48 | __esc_assert_func ( 49 | const char *file, 50 | int line, 51 | const char *func, 52 | const char *failedexpr 53 | ); 54 | -------------------------------------------------------------------------------- /app/include/fixed.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #if defined(CONFIG_STM32_STM32F302) 31 | /* STM32F30X-specific CMSIS defs */ 32 | #include "cmsis_stm32f302.h" 33 | #elif defined(CONFIG_STM32_STM32F446) 34 | /* STM32F44X-specific CMSIS defs */ 35 | #include "cmsis_stm32f446.h" 36 | #endif /* defined(CONFIG_STM32_STM32F302) */ 37 | 38 | #include "core_cm4.h" 39 | 40 | 41 | #ifndef M_PI 42 | #define M_PI 3.141592653589793f 43 | #endif 44 | 45 | 46 | struct motor_state_t { 47 | float angular_velocity_rad_per_s; 48 | float angle_rad; /* 0 .. 2 * pi */ 49 | float i_dq_a[2]; 50 | float v_dq_v[2]; 51 | }; 52 | 53 | 54 | struct motor_params_t { 55 | float rs_r; /* winding resistance in ohms */ 56 | float ls_h; /* winding inductance in ohms */ 57 | float phi_v_s_per_rad; /* speed constant in volt-seconds per radian */ 58 | 59 | /* Operating limits */ 60 | float max_current_a; /* RMS current limit in amps */ 61 | float max_voltage_v; /* RMS voltage limit in volts */ 62 | float accel_voltage_v; /* RMS initial open-loop voltage */ 63 | 64 | uint32_t num_poles; /* number of poles */ 65 | }; 66 | 67 | 68 | struct control_params_t { 69 | float bandwidth_hz; 70 | float gain; 71 | float braking_frac; 72 | }; 73 | 74 | 75 | inline void __attribute__((always_inline)) 76 | sin_cos( 77 | float& sinx, 78 | float& cosx, 79 | float x /* x must be in the range [-pi, pi] */ 80 | ) { 81 | const float Q = 3.1f; 82 | const float P = 3.6f; 83 | 84 | float y; 85 | 86 | x *= float(1.0 / M_PI); 87 | 88 | y = x - x * std::abs(x); 89 | sinx = y * (Q + P * std::abs(y)); 90 | 91 | /* Calculate the cosine */ 92 | x += 0.5f; 93 | if (x > 1.0f) { 94 | x -= 2.0f; 95 | } 96 | 97 | y = x - x * std::abs(x); 98 | cosx = y * (Q + P * std::abs(y)); 99 | } 100 | 101 | 102 | inline float __attribute__((always_inline)) __VSQRTF(float x) { 103 | float result; 104 | asm ("vsqrt.f32 %0, %1" : "=w" (result) : "w" (x) ); 105 | return result; 106 | } 107 | -------------------------------------------------------------------------------- /app/include/hal.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "fixed.h" 26 | 27 | 28 | enum hal_status_t { 29 | HAL_STATUS_OK = 0, 30 | HAL_STATUS_ERROR = 1 31 | }; 32 | 33 | 34 | enum hal_pwm_state_t { 35 | HAL_PWM_STATE_OFF = 0, 36 | HAL_PWM_STATE_LOW = 1, 37 | HAL_PWM_STATE_RUNNING = 2 38 | }; 39 | 40 | 41 | typedef void (*hal_callback_t)(void); 42 | /* Called with output Vab, last output Vab, last Iab, and Vbus */ 43 | typedef void (*hal_control_callback_t)(float*, const float*, 44 | const float*, float); 45 | typedef void (*hal_pwm_callback_t)(uint32_t, uint32_t); 46 | 47 | 48 | extern const uint32_t hal_core_frequency_hz; 49 | extern const uint32_t hal_pwm_frequency_hz; 50 | extern const float hal_full_scale_current_a; 51 | extern const float hal_full_scale_voltage_v; 52 | extern const float hal_control_t_s; 53 | 54 | 55 | /* Public interface */ 56 | void hal_reset(void); 57 | void hal_set_pwm_state(enum hal_pwm_state_t state); 58 | void hal_set_pwm_reverse(bool reverse); 59 | enum hal_status_t hal_transmit_can_message( 60 | uint8_t mailbox, 61 | uint32_t message_id, 62 | size_t length, 63 | const uint8_t *message 64 | ); 65 | enum hal_status_t hal_receive_can_message( 66 | uint8_t fifo, 67 | uint8_t *filter_id, 68 | uint32_t *message_id, 69 | size_t *length, 70 | uint8_t *message 71 | ); 72 | void hal_set_can_dtid_filter( 73 | uint8_t fifo, 74 | uint8_t filter_id, 75 | bool is_service, 76 | uint16_t dtid, 77 | uint8_t node_id 78 | ); 79 | uint8_t hal_get_can_node_id(void); 80 | bool hal_is_can_ready(uint8_t mailbox); 81 | void hal_disable_can_transmit(void); 82 | void hal_enable_can_transmit(void); 83 | float hal_get_temperature_degc(void); 84 | bool hal_get_fault(void); 85 | uint32_t hal_get_fault_detail(void); 86 | void hal_set_low_frequency_callback(hal_callback_t callback); 87 | void hal_set_high_frequency_callback(hal_control_callback_t callback); 88 | void hal_set_rc_pwm_callback(hal_pwm_callback_t callback); 89 | void hal_flash_protect(bool readonly); 90 | void hal_flash_erase(uint8_t *addr, size_t len); 91 | void hal_flash_write(uint8_t *addr, size_t len, const uint8_t *data); 92 | void hal_restart(void); 93 | -------------------------------------------------------------------------------- /app/include/park.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "fixed.h" 26 | 27 | inline void __attribute__((always_inline)) 28 | park_transform( 29 | float dq[2], 30 | const float alpha_beta[2], 31 | float sin_theta, 32 | float cos_theta 33 | ) { 34 | dq[0] = alpha_beta[0] * cos_theta + alpha_beta[1] * sin_theta; 35 | dq[1] = -alpha_beta[0] * sin_theta + alpha_beta[1] * cos_theta; 36 | } 37 | 38 | 39 | inline void __attribute__((always_inline)) 40 | park_transform( 41 | float dq[2], 42 | const float alpha_beta[2], 43 | float theta 44 | ) { 45 | float sin_theta, cos_theta; 46 | 47 | sin_cos(sin_theta, cos_theta, theta); 48 | park_transform(dq, alpha_beta, sin_theta, cos_theta); 49 | } 50 | 51 | 52 | inline void __attribute__((always_inline)) 53 | inverse_park_transform( 54 | float alpha_beta[2], 55 | const float dq[2], 56 | float sin_theta, 57 | float cos_theta 58 | ) { 59 | alpha_beta[0] = dq[0] * cos_theta - dq[1] * sin_theta; 60 | alpha_beta[1] = dq[0] * sin_theta + dq[1] * cos_theta; 61 | } 62 | 63 | 64 | inline void __attribute__((always_inline)) 65 | inverse_park_transform( 66 | float alpha_beta[2], 67 | const float dq[2], 68 | float theta 69 | ) { 70 | float sin_theta, cos_theta; 71 | 72 | sin_cos(sin_theta, cos_theta, theta); 73 | inverse_park_transform(alpha_beta, dq, sin_theta, cos_theta); 74 | } 75 | -------------------------------------------------------------------------------- /app/include/perf.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | 26 | extern volatile uint32_t g_perf_total; 27 | extern volatile uint32_t g_perf_last; 28 | extern volatile uint32_t g_perf_samples; 29 | 30 | 31 | #define PERF_COUNT_START uint32_t _start_cyccnt; \ 32 | { \ 33 | volatile uint32_t* DWT_CYCCNT = (uint32_t*)0xE0001004; \ 34 | asm volatile ("":::"memory"); \ 35 | _start_cyccnt = *DWT_CYCCNT; \ 36 | asm volatile ("":::"memory"); \ 37 | } 38 | 39 | 40 | #define PERF_COUNT_END { \ 41 | volatile uint32_t* DWT_CYCCNT = (uint32_t*)0xE0001004; \ 42 | asm volatile ("":::"memory"); \ 43 | g_perf_last = *DWT_CYCCNT - _start_cyccnt; \ 44 | g_perf_total += g_perf_last; \ 45 | g_perf_samples = g_perf_total < g_perf_last ? 0 : g_perf_samples + 1; \ 46 | } 47 | -------------------------------------------------------------------------------- /app/include/shared.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: Ben Dyer 5 | * Pavel Kirienko 6 | * David Sidrane 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | #pragma once 38 | 39 | /* Bootloader communication */ 40 | struct bootloader_app_shared_t { 41 | uint64_t crc; 42 | uint32_t signature; 43 | uint32_t bus_speed; 44 | uint32_t node_id; 45 | } __attribute__ ((packed)); 46 | 47 | 48 | #define BOOTLOADER_COMMON_APP_SIGNATURE 0xB0A04150u 49 | #define BOOTLOADER_COMMON_BOOTLOADER_SIGNATURE 0xB0A0424Cu 50 | 51 | 52 | bool bootloader_read(bootloader_app_shared_t *shared); 53 | void bootloader_write(const bootloader_app_shared_t *shared); 54 | -------------------------------------------------------------------------------- /app/include/svm.h: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #pragma once 24 | 25 | #include "esc_assert.h" 26 | 27 | /* sqrt(3) = sqrt3_mul / sqrt3_div */ 28 | #define SQRT3_MUL 28378 29 | #define SQRT3_Q 14 30 | 31 | 32 | inline uint8_t svm_duty_cycle_from_v_alpha_beta( 33 | uint16_t phase_oc_ticks[3], 34 | int16_t v_frac_alpha_q1p15, 35 | int16_t v_frac_beta_q1p15, 36 | uint16_t pwm_period_ticks 37 | ) { 38 | int32_t x, y, z, u_alpha, u_beta, tmpa, tmpb, tmpc; 39 | int32_t quarter_t; 40 | uint32_t double_period_t; 41 | uint8_t sector; 42 | 43 | quarter_t = pwm_period_ticks >> 2; 44 | double_period_t = pwm_period_ticks << 1u; 45 | 46 | /* See ST UM1052, pp. 30-32. */ 47 | u_alpha = v_frac_alpha_q1p15 * ((double_period_t * SQRT3_MUL) >> SQRT3_Q); 48 | u_beta = v_frac_beta_q1p15 * double_period_t; 49 | 50 | x = u_beta; 51 | y = (u_beta + u_alpha) >> 1; 52 | z = (u_beta - u_alpha) >> 1; 53 | 54 | /* Sector calculation from x, y, z */ 55 | if (y < 0) { 56 | if (z < 0) { 57 | sector = 5; 58 | } else if (x <= 0) { 59 | sector = 4; 60 | } else { 61 | sector = 3; 62 | } 63 | } else { 64 | if (z >= 0) { 65 | sector = 2; 66 | } else if (x <= 0) { 67 | sector = 6; 68 | } else { 69 | sector = 1; 70 | } 71 | } 72 | 73 | switch (sector) { 74 | case 1: 75 | case 4: 76 | tmpa = quarter_t + ((x - z) >> 18); 77 | tmpb = tmpa + (z >> 17); 78 | tmpc = tmpb - (x >> 17); 79 | break; 80 | case 2: 81 | case 5: 82 | tmpa = quarter_t + ((y - z) >> 18); 83 | tmpb = tmpa + (z >> 17); 84 | tmpc = tmpa - (y >> 17); 85 | break; 86 | case 3: 87 | case 6: 88 | /* Note different order of calculation from above two cases */ 89 | tmpa = quarter_t + ((y - x) >> 18); 90 | tmpc = tmpa - (y >> 17); 91 | tmpb = tmpc + (x >> 17); 92 | break; 93 | default: 94 | esc_assert(false); 95 | break; 96 | } 97 | 98 | phase_oc_ticks[0] = uint16_t(__USAT(tmpa, 16u)); 99 | phase_oc_ticks[1] = uint16_t(__USAT(tmpb, 16u)); 100 | phase_oc_ticks[2] = uint16_t(__USAT(tmpc, 16u)); 101 | 102 | return sector; 103 | } 104 | -------------------------------------------------------------------------------- /app/src/_cxx.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | void* __dso_handle = NULL; 24 | 25 | namespace __gnu_cxx 26 | { 27 | void 28 | __attribute__((noreturn)) 29 | __verbose_terminate_handler(); 30 | 31 | void 32 | __verbose_terminate_handler() { 33 | while (1); 34 | } 35 | } 36 | 37 | void operator delete (void *) { 38 | } 39 | 40 | extern "C" { 41 | void 42 | __attribute__((noreturn)) 43 | __cxa_pure_virtual(); 44 | 45 | void 46 | __cxa_pure_virtual() { 47 | while (1); 48 | } 49 | 50 | void _sbrk(void) { 51 | } 52 | 53 | int __aeabi_atexit( 54 | void *object, 55 | void (*destructor)(void *), 56 | void *dso_handle) 57 | { 58 | return 0; 59 | } 60 | } 61 | 62 | 63 | -------------------------------------------------------------------------------- /app/src/app_vectors.c: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #include 24 | 25 | void __attribute__((weak)) default_handler(void) { 26 | while (1u); 27 | } 28 | 29 | void __attribute__ ((weak, alias ("default_handler"))) 30 | stm32_reserved(void); 31 | void __start(void) __attribute__ ((no_instrument_function)); 32 | 33 | /* Function prototypes for each defined vector */ 34 | #define UNUSED(x) 35 | #define VECTOR(x, y) void __attribute__ ((weak, alias ("default_handler"))) \ 36 | x(void); 37 | 38 | /* Standard vectors */ 39 | VECTOR(stm32_nmi, 2) 40 | VECTOR(stm32_hardfault, 3) 41 | VECTOR(stm32_mpu, 4) 42 | VECTOR(stm32_busfault, 5) 43 | VECTOR(stm32_usagefault, 6) 44 | UNUSED(7) 45 | UNUSED(8) 46 | UNUSED(9) 47 | UNUSED(10) 48 | VECTOR(stm32_svcall, 11) 49 | VECTOR(stm32_dbgmonitor, 12) 50 | UNUSED(13) 51 | VECTOR(stm32_pendsv, 14) 52 | VECTOR(stm32_systick, 15) 53 | 54 | #if defined(CONFIG_STM32_STM32F302) 55 | /* STM32F30X-specific vectors */ 56 | #include 57 | #elif defined(CONFIG_STM32_STM32F446) 58 | /* STM32F44X-specific vectors */ 59 | #include 60 | #else 61 | #pragma error "Invalid chip" 62 | #endif /* defined(CONFIG_STM32_STM32F302) */ 63 | 64 | #undef VECTOR 65 | #undef UNUSED 66 | 67 | 68 | extern uint32_t __stack; 69 | 70 | typedef void (* const vector_entry)(void); 71 | 72 | __attribute__ ((section(".app_vectors"),used)) 73 | vector_entry app_vectors[] = { 74 | (vector_entry)&__stack, 75 | __start, 76 | 77 | #define UNUSED(x) (vector_entry)(stm32_reserved), 78 | #define VECTOR(x, y) (vector_entry)x, 79 | 80 | /* Standard vectors */ 81 | VECTOR(stm32_nmi, 2) 82 | VECTOR(stm32_hardfault, 3) 83 | VECTOR(stm32_mpu, 4) 84 | VECTOR(stm32_busfault, 5) 85 | VECTOR(stm32_usagefault, 6) 86 | VECTOR(stm32_reserved, 7) 87 | VECTOR(stm32_reserved, 8) 88 | VECTOR(stm32_reserved, 9) 89 | VECTOR(stm32_reserved, 10) 90 | VECTOR(stm32_svcall, 11) 91 | VECTOR(stm32_dbgmonitor, 12) 92 | VECTOR(stm32_reserved, 13) 93 | VECTOR(stm32_pendsv, 14) 94 | VECTOR(stm32_systick, 15) 95 | 96 | #if defined(CONFIG_STM32_STM32F302) 97 | /* STM32F30X-specific vectors */ 98 | #include 99 | #elif defined(CONFIG_STM32_STM32F446) 100 | /* STM32F44X-specific vectors */ 101 | #include 102 | #else 103 | #pragma error "Invalid chip" 104 | #endif /* defined(CONFIG_STM32_STM32F302) */ 105 | 106 | #undef VECTOR 107 | #undef UNUSED 108 | }; 109 | -------------------------------------------------------------------------------- /app/src/controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "perf.h" 28 | #include "controller.h" 29 | #include "fixed.h" 30 | 31 | 32 | void DQCurrentController::update( 33 | float out_v_dq_v[2], 34 | const float i_dq_a[2], 35 | float angular_velocity_rad_per_s, 36 | float vbus_v, 37 | float audio_v 38 | ) { 39 | /* 40 | Current controller described in: 41 | 42 | Harnefors, L. and Nee, H-P. (1998) 43 | "Model-Based Current Control of AC Machines Using the Internal Model 44 | Control Method", 45 | 46 | http://ieeexplore.ieee.org/xpls/abs_all.jsp?arnumber=658735 47 | 48 | This is an implementation of the Diagonal Internal Model Control approach 49 | (equation 20) with anti-windup during inverter saturation 50 | (equations 42-47). 51 | 52 | The code below follows the pseudocode listing in Appendix C of that paper. 53 | */ 54 | float ed_a, eq_a, vd_v, vq_v, ccd_v, ccq_v, vd_max, vq_max; 55 | 56 | /* Calculate Idq error */ 57 | ed_a = 0.0f - i_dq_a[0]; 58 | eq_a = i_setpoint_a_ - i_dq_a[1]; 59 | 60 | /* Limit error -- use conditional instructions to avoid branching */ 61 | if (eq_a > accel_current_limit_a_) { 62 | eq_a = accel_current_limit_a_; 63 | } 64 | if (eq_a < -accel_current_limit_a_) { 65 | eq_a = -accel_current_limit_a_; 66 | } 67 | 68 | /* Decouple Idq using feed-forward, and add the integral error */ 69 | ccd_v = integral_vd_v_; // - ls_h_ * angular_velocity_rad_per_s * i_dq_a[1]; 70 | ccq_v = integral_vq_v_; // + ls_h_ * angular_velocity_rad_per_s * i_dq_a[0]; 71 | 72 | /* 73 | Combine integral with proportional terms to find the desired output 74 | voltages 75 | */ 76 | vd_v = kp_ * ed_a + ccd_v; 77 | vq_v = kp_ * eq_a + ccq_v + audio_v; 78 | 79 | /* 80 | Limit the absolute value of Vdq to vbus * maximum modulation. Give Vd 81 | priority over Vq if we are limited by voltage, to ensure Id can be kept 82 | at zero. 83 | */ 84 | vd_max = std::min(v_limit_v_, vbus_v * 0.95f); 85 | if (vd_v > vd_max) { 86 | vd_v = vd_max; 87 | } 88 | if (vd_v < -vd_max) { 89 | vd_v = -vd_max; 90 | } 91 | 92 | vq_max = __VSQRTF(vd_max * vd_max - vd_v * vd_v); 93 | if (vq_v > vq_max) { 94 | vq_v = vq_max; 95 | } 96 | if (vq_v < -vq_max) { 97 | vq_v = -vq_max; 98 | } 99 | 100 | /* 101 | Anti-windup calculation for the integral term; if desired |Vdq| is greater 102 | than Vmax, reduce the amount of error accumulated. 103 | */ 104 | integral_vd_v_ += ki_ * (vd_v - ccd_v); 105 | integral_vq_v_ += ki_ * (vq_v - ccq_v); 106 | 107 | /* Output Vdq */ 108 | out_v_dq_v[0] = vd_v; 109 | out_v_dq_v[1] = vq_v; 110 | } 111 | -------------------------------------------------------------------------------- /app/src/m4f_float_spec.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #if UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 28 | namespace uavcan 29 | { 30 | 31 | /* 32 | * IEEE754Converter 33 | */ 34 | uint16_t IEEE754Converter::nativeIeeeToHalf(float value) 35 | { 36 | uint32_t result; 37 | asm ("vcvtb.f16.f32 %0, %1" : "=w" (result) : "w" (value) ); 38 | return uint16_t(result); 39 | } 40 | 41 | float IEEE754Converter::halfToNativeIeee(uint16_t value) 42 | { 43 | float result; 44 | uint32_t v = value; 45 | asm ("vcvtb.f32.f16 %0, %1" : "=w" (result) : "w" (v) ); 46 | return result; 47 | } 48 | } 49 | #endif // UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 50 | -------------------------------------------------------------------------------- /app/src/perf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Copyright (C) 2014-2015 Thiemar Pty Ltd 3 | 4 | Permission is hereby granted, free of charge, to any person obtaining a copy 5 | of this software and associated documentation files (the "Software"), to deal 6 | in the Software without restriction, including without limitation the rights 7 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 8 | copies of the Software, and to permit persons to whom the Software is 9 | furnished to do so, subject to the following conditions: 10 | 11 | The above copyright notice and this permission notice shall be included in 12 | all copies or substantial portions of the Software. 13 | 14 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 15 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 16 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 17 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 18 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 19 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 20 | SOFTWARE. 21 | */ 22 | 23 | #include 24 | #include "perf.h" 25 | 26 | volatile uint32_t g_perf_total; 27 | volatile uint32_t g_perf_last; 28 | volatile uint32_t g_perf_samples; 29 | -------------------------------------------------------------------------------- /app/src/shared.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: Ben Dyer 5 | * Pavel Kirienko 6 | * David Sidrane 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | #include 38 | #include 39 | #include "shared.h" 40 | 41 | 42 | static uint64_t bootloader_calculate_signature( 43 | const bootloader_app_shared_t *pshared 44 | ); 45 | 46 | static uint64_t bootloader_calculate_signature( 47 | const bootloader_app_shared_t *pshared 48 | ) { 49 | uavcan::DataTypeSignatureCRC crc; 50 | crc.add((uint8_t*)(&pshared->signature), sizeof(uint32_t) * 3u); 51 | return crc.get(); 52 | } 53 | 54 | 55 | /* CAN_FiRx where (i=0..27|13, x=1, 2) 56 | * STM32_CAN1_FIR(i,x) 57 | * Using i = 2 does not requier there block 58 | * to be enabled nor FINIT in CAN_FMR to be set. 59 | * todo:Validate this claim on F2, F3 60 | */ 61 | 62 | #define crc_HiLOC STM32_CAN1_FIR(2,1) 63 | #define crc_LoLOC STM32_CAN1_FIR(2,2) 64 | #define signature_LOC STM32_CAN1_FIR(3,1) 65 | #define bus_speed_LOC STM32_CAN1_FIR(3,2) 66 | #define node_id_LOC STM32_CAN1_FIR(4,1) 67 | #define CRC_H 1 68 | #define CRC_L 0 69 | 70 | 71 | bool bootloader_read(bootloader_app_shared_t *shared) { 72 | shared->signature = getreg32(signature_LOC); 73 | shared->bus_speed = getreg32(bus_speed_LOC); 74 | shared->node_id = getreg32(node_id_LOC); 75 | shared->crc = uint64_t(getreg32(crc_LoLOC)) | 76 | (uint64_t(getreg32(crc_HiLOC)) << 32u); 77 | 78 | if (shared->crc == bootloader_calculate_signature(shared)) { 79 | return true; 80 | } else { 81 | return false; 82 | } 83 | } 84 | 85 | 86 | void bootloader_write(const bootloader_app_shared_t *shared) { 87 | bootloader_app_shared_t working = *shared; 88 | 89 | working.signature = BOOTLOADER_COMMON_APP_SIGNATURE; 90 | working.crc = bootloader_calculate_signature(&working); 91 | 92 | putreg32(working.signature, signature_LOC); 93 | putreg32(working.bus_speed, bus_speed_LOC); 94 | putreg32(working.node_id, node_id_LOC); 95 | putreg32(uint32_t(working.crc), crc_LoLOC); 96 | putreg32(uint32_t(working.crc >> 32u), crc_HiLOC); 97 | } 98 | -------------------------------------------------------------------------------- /arch/include/irq_base.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * include/nuttx/irq.h 3 | * 4 | * Copyright (C) 2007-2011, 2013 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | #ifndef __INCLUDE_NUTTX_IRQ_H 37 | #define __INCLUDE_NUTTX_IRQ_H 38 | 39 | /**************************************************************************** 40 | * Included Files 41 | ****************************************************************************/ 42 | 43 | #ifndef __ASSEMBLY__ 44 | # include 45 | #endif 46 | 47 | /**************************************************************************** 48 | * Pre-processor Definitions 49 | ****************************************************************************/ 50 | /* IRQ detach is a convenience definition. Detaching an interrupt handler 51 | * is equivalent to setting a NULL interrupt handler. 52 | */ 53 | 54 | #ifndef __ASSEMBLY__ 55 | # define irq_detach(isr) irq_attach(isr, NULL) 56 | #endif 57 | 58 | /**************************************************************************** 59 | * Public Types 60 | ****************************************************************************/ 61 | 62 | /* This struct defines the way the registers are stored */ 63 | 64 | #ifndef __ASSEMBLY__ 65 | typedef int (*xcpt_t)(int irq, void *context); 66 | 67 | extern xcpt_t g_irqvector[NR_IRQS+1]; 68 | #endif 69 | 70 | /**************************************************************************** 71 | * Public Variables 72 | ****************************************************************************/ 73 | 74 | #ifndef __ASSEMBLY__ 75 | #ifdef __cplusplus 76 | #define EXTERN extern "C" 77 | extern "C" 78 | { 79 | #else 80 | #define EXTERN extern 81 | #endif 82 | 83 | /**************************************************************************** 84 | * Public Function Prototypes 85 | ****************************************************************************/ 86 | 87 | /**************************************************************************** 88 | * Name: irq_attach 89 | * 90 | * Description: 91 | * Configure the IRQ subsystem so that IRQ number 'irq' is dispatched to 92 | * 'isr' 93 | * 94 | ****************************************************************************/ 95 | 96 | int irq_attach(int irq, xcpt_t isr); 97 | int up_prioritize_irq(int irq, int priority); 98 | 99 | void __attribute__((weak)) irq_initialize(void); 100 | int irq_unexpected_isr(int irq, void *context); 101 | 102 | #undef EXTERN 103 | #ifdef __cplusplus 104 | } 105 | #endif 106 | #endif 107 | 108 | #endif /* __INCLUDE_NUTTX_IRQ_H */ 109 | 110 | -------------------------------------------------------------------------------- /arch/src/chip/stm32_memorymap.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/chip/stm32_memorymap.h 3 | * 4 | * Copyright (C) 2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************************/ 35 | 36 | #ifndef __ARCH_ARM_SRC_STM32_CHIP_STM32_MEMORYMAP_H 37 | #define __ARCH_ARM_SRC_STM32_CHIP_STM32_MEMORYMAP_H 38 | 39 | /************************************************************************************ 40 | * Included Files 41 | ************************************************************************************/ 42 | 43 | #include "chip.h" 44 | 45 | #if defined(CONFIG_STM32_STM32L15XX) 46 | # include "chip/stm32l15xxx_memorymap.h" 47 | #elif defined(CONFIG_STM32_STM32F10XX) 48 | # include "chip/stm32f10xxx_memorymap.h" 49 | #elif defined(CONFIG_STM32_STM32F20XX) 50 | # include "chip/stm32f20xxx_memorymap.h" 51 | #elif defined(CONFIG_STM32_STM32F30XX) 52 | # include "chip/stm32f30xxx_memorymap.h" 53 | #elif defined(CONFIG_STM32_STM32F37XX) 54 | # include "chip/stm32f37xxx_memorymap.h" 55 | #elif defined(CONFIG_STM32_STM32F40XX) 56 | # include "chip/stm32f40xxx_memorymap.h" 57 | #else 58 | # error "Unsupported STM32 memory map" 59 | #endif 60 | 61 | #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_MEMORYMAP_H */ 62 | 63 | -------------------------------------------------------------------------------- /arch/src/chip/stm32_otgfs.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thiemar/vectorcontrol/6bc7bcfdcfb4216523195ab530babfbd9a974024/arch/src/chip/stm32_otgfs.h -------------------------------------------------------------------------------- /arch/src/chip/stm32_rng.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/chip/stm32_rng.h 3 | * 4 | * Copyright (C) 2012 Max Holtzberg. All rights reserved. 5 | * Author: Max Holtzberg 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************************/ 35 | 36 | #ifndef __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H 37 | #define __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H 38 | 39 | /************************************************************************************ 40 | * Included Files 41 | ************************************************************************************/ 42 | 43 | #include "chip.h" 44 | 45 | /************************************************************************************ 46 | * Pre-processor Definitions 47 | ************************************************************************************/ 48 | 49 | /* Register Offsets *****************************************************************/ 50 | 51 | #define STM32_RNG_CR_OFFSET 0x0000 /* RNG Control Register */ 52 | #define STM32_RNG_SR_OFFSET 0x0004 /* RNG Status Register */ 53 | #define STM32_RNG_DR_OFFSET 0x0008 /* RNG Data Register */ 54 | 55 | /* Register Addresses ***************************************************************/ 56 | 57 | #define STM32_RNG_CR (STM32_RNG_BASE+STM32_RNG_CR_OFFSET) 58 | #define STM32_RNG_SR (STM32_RNG_BASE+STM32_RNG_SR_OFFSET) 59 | #define STM32_RNG_DR (STM32_RNG_BASE+STM32_RNG_DR_OFFSET) 60 | 61 | /* Register Bitfield Definitions ****************************************************/ 62 | 63 | /* RNG Control Register */ 64 | 65 | #define RNG_CR_RNGEN (1 << 2) /* Bit 2: RNG enable */ 66 | #define RNG_CR_IE (1 << 3) /* Bit 3: Interrupt enable */ 67 | 68 | /* RNG Status Register */ 69 | 70 | #define RNG_SR_DRDY (1 << 0) /* Bit 0: Data ready */ 71 | #define RNG_SR_CECS (1 << 1) /* Bit 1: Clock error current status */ 72 | #define RNG_SR_SECS (1 << 2) /* Bit 2: Seed error current status */ 73 | #define RNG_SR_CEIS (1 << 5) /* Bit 5: Clock error interrupt status */ 74 | #define RNG_SR_SEIS (1 << 6) /* Bit 6: Seed error interrupt status */ 75 | 76 | #endif /* __ARCH_ARM_STC_STM32_CHIP_STM32_RNG_H */ 77 | -------------------------------------------------------------------------------- /arch/src/irq_dispatch.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * sched/irq/irq_dispatch.c 3 | * 4 | * Copyright (C) 2007, 2008 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | 44 | /**************************************************************************** 45 | * Pre-processor Definitions 46 | ****************************************************************************/ 47 | 48 | /**************************************************************************** 49 | * Private Type Declarations 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Global Variables 54 | ****************************************************************************/ 55 | 56 | /**************************************************************************** 57 | * Private Variables 58 | ****************************************************************************/ 59 | 60 | /**************************************************************************** 61 | * Private Function Prototypes 62 | ****************************************************************************/ 63 | 64 | /**************************************************************************** 65 | * Public Functions 66 | ****************************************************************************/ 67 | 68 | /**************************************************************************** 69 | * Name: irq_dispatch 70 | * 71 | * Description: 72 | * This function must be called from the achitecture-specific logic in 73 | * order to dispatch an interrupt to the appropriate, registered handling 74 | * logic. 75 | * 76 | ***************************************************************************/ 77 | 78 | void irq_dispatch(int irq, void *context) 79 | { 80 | xcpt_t vector; 81 | 82 | /* Perform some sanity checks */ 83 | 84 | #if NR_IRQS > 0 85 | if ((unsigned)irq >= NR_IRQS || g_irqvector[irq] == NULL) 86 | { 87 | vector = irq_unexpected_isr; 88 | } 89 | else 90 | { 91 | vector = g_irqvector[irq]; 92 | } 93 | #else 94 | vector = irq_unexpected_isr; 95 | #endif 96 | 97 | /* Then dispatch to the interrupt handler */ 98 | 99 | vector(irq, context); 100 | } 101 | 102 | -------------------------------------------------------------------------------- /arch/src/irq_initialize.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * sched/irq/irq_initialize.c 3 | * 4 | * Copyright (C) 2007-2008, 2010 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | 44 | /**************************************************************************** 45 | * Pre-processor Definitions 46 | ****************************************************************************/ 47 | 48 | /**************************************************************************** 49 | * Private Type Declarations 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Global Variables 54 | ****************************************************************************/ 55 | 56 | xcpt_t g_irqvector[NR_IRQS+1]; 57 | 58 | /**************************************************************************** 59 | * Private Variables 60 | ****************************************************************************/ 61 | 62 | /**************************************************************************** 63 | * Private Function Prototypes 64 | ****************************************************************************/ 65 | 66 | /**************************************************************************** 67 | * Public Functions 68 | ****************************************************************************/ 69 | 70 | /**************************************************************************** 71 | * Name: irq_initialize 72 | * 73 | * Description: 74 | * Configure the IRQ subsystem 75 | * 76 | ****************************************************************************/ 77 | 78 | void irq_initialize(void) 79 | { 80 | int i; 81 | 82 | /* Point all interrupt vectors to the unexpected interrupt */ 83 | 84 | for (i = 0; i < NR_IRQS; i++) 85 | { 86 | g_irqvector[i] = irq_unexpected_isr; 87 | } 88 | } 89 | 90 | -------------------------------------------------------------------------------- /arch/src/irq_unexpectedisr.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * sched/irq/irq_unexpectedisr.c 3 | * 4 | * Copyright (C) 2007-2009, 2013 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | 44 | /**************************************************************************** 45 | * Pre-processor Definitions 46 | ****************************************************************************/ 47 | 48 | /**************************************************************************** 49 | * Private Type Declarations 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Global Variables 54 | ****************************************************************************/ 55 | 56 | /**************************************************************************** 57 | * Private Variables 58 | ****************************************************************************/ 59 | 60 | /**************************************************************************** 61 | * Private Function Prototypes 62 | ****************************************************************************/ 63 | 64 | /**************************************************************************** 65 | * Public Functions 66 | ****************************************************************************/ 67 | 68 | /**************************************************************************** 69 | * Name: irq_unexpected_isr 70 | * 71 | * Description: 72 | * An interrupt has been received for an IRQ that was never registered 73 | * with the system. 74 | * 75 | ****************************************************************************/ 76 | 77 | int irq_unexpected_isr(int irq, void *context) 78 | { 79 | (void)irqsave(); 80 | while (1); 81 | return 0; /* Won't get here */ 82 | } 83 | -------------------------------------------------------------------------------- /arch/src/stm32.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/stm32.h 3 | * 4 | * Copyright (C) 2011 Uros Platise. All rights reserved. 5 | * Copyright (C) 2011 Gregory Nutt. All rights reserved. 6 | * Authors: Uros Platise 7 | * Gregory Nutt 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * 1. Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * 2. Redistributions in binary form must reproduce the above copyright 16 | * notice, this list of conditions and the following disclaimer in 17 | * the documentation and/or other materials provided with the 18 | * distribution. 19 | * 3. Neither the name NuttX nor the names of its contributors may be 20 | * used to endorse or promote products derived from this software 21 | * without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 30 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 31 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | * 36 | ************************************************************************************/ 37 | 38 | #ifndef __ARCH_ARM_SRC_STM32_STM32_H 39 | #define __ARCH_ARM_SRC_STM32_STM32_H 40 | 41 | /************************************************************************************ 42 | * Included Files 43 | ************************************************************************************/ 44 | 45 | #ifndef __ASSEMBLY__ 46 | #include 47 | #include 48 | #include 49 | 50 | #include "up_internal.h" 51 | #endif 52 | 53 | /************************************************************************************ 54 | * Pre-processor Definitions 55 | ************************************************************************************/ 56 | 57 | /* Additional Configuration *********************************************************/ 58 | /* Custom debug settings used in the STM32 port. These are managed by STM32-specific 59 | * logic and not the common logic in include/debug.h. NOTE: Some of these also 60 | * depend on CONFIG_DEBUG_VERBOSE 61 | */ 62 | 63 | #ifndef CONFIG_DEBUG 64 | # undef CONFIG_DEBUG_DMA 65 | # undef CONFIG_DEBUG_RTC 66 | # undef CONFIG_DEBUG_I2C 67 | # undef CONFIG_DEBUG_CAN 68 | # undef CONFIG_DEBUG_PWM 69 | # undef CONFIG_DEBUG_SENSORS 70 | #endif 71 | 72 | /* Peripherals **********************************************************************/ 73 | 74 | #include "chip.h" 75 | //#include "stm32_adc.h" 76 | //#include "stm32_bkp.h" 77 | #include "stm32_can.h" 78 | #include "stm32_dbgmcu.h" 79 | #include "stm32_exti.h" 80 | #include "stm32_flash.h" 81 | #include "stm32_gpio.h" 82 | #include "stm32_i2c.h" 83 | #include "stm32_ltdc.h" 84 | #include "stm32_pwr.h" 85 | #include "stm32_rcc.h" 86 | #include "stm32_rtc.h" 87 | #include "stm32_sdio.h" 88 | #include "stm32_spi.h" 89 | #include "stm32_tim.h" 90 | #include "stm32_usbdev.h" 91 | #include "stm32_wdg.h" 92 | #include "stm32_eth.h" 93 | 94 | #endif /* __ARCH_ARM_SRC_STM32_STM32_H */ 95 | 96 | -------------------------------------------------------------------------------- /arch/src/stm32_dbgmcu.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/stm32_dbgmcu.h 3 | * 4 | * Copyright (C) 2009 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************************/ 35 | 36 | #ifndef __ARCH_ARM_SRC_STM32_STM32_DBGMCU_H 37 | #define __ARCH_ARM_SRC_STM32_STM32_DBGMCU_H 38 | 39 | /************************************************************************************ 40 | * Included Files 41 | ************************************************************************************/ 42 | 43 | #include "chip.h" 44 | #include "chip/stm32_dbgmcu.h" 45 | 46 | /************************************************************************************ 47 | * Pre-processor Definitions 48 | ************************************************************************************/ 49 | 50 | /************************************************************************************ 51 | * Public Types 52 | ************************************************************************************/ 53 | 54 | /************************************************************************************ 55 | * Public Data 56 | ************************************************************************************/ 57 | 58 | /************************************************************************************ 59 | * Public Functions 60 | ************************************************************************************/ 61 | 62 | #endif /* __ARCH_ARM_SRC_STM32_STM32_DBGMCU_H */ 63 | -------------------------------------------------------------------------------- /arch/src/stm32_flash.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/stm32_flash.h 3 | * 4 | * Copyright (C) 2011 Uros Platise. All rights reserved. 5 | * Author: Uros Platise 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************************/ 35 | 36 | #ifndef __ARCH_ARM_SRC_STM32_STM32_FLASH_H 37 | #define __ARCH_ARM_SRC_STM32_STM32_FLASH_H 38 | 39 | /************************************************************************************ 40 | * Included Files 41 | ************************************************************************************/ 42 | 43 | #include "chip.h" 44 | #include "chip/stm32_flash.h" 45 | 46 | #endif /* __ARCH_ARM_SRC_STM32_STM32_FLASH_H */ 47 | -------------------------------------------------------------------------------- /arch/src/stm32_syscfg.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************************************** 2 | * arch/arm/src/stm32/stm32_syscfg.h 3 | * 4 | * Copyright (C) 2011, 2013 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************************************/ 35 | 36 | #ifndef __ARCH_ARM_SRC_STM32_STM32_SYSCFG_H 37 | #define __ARCH_ARM_SRC_STM32_STM32_SYSCFG_H 38 | 39 | /**************************************************************************************************** 40 | * Included Files 41 | ****************************************************************************************************/ 42 | 43 | #include "chip.h" 44 | 45 | #if defined(CONFIG_STM32_STM32L15XX) 46 | # include "chip/stm32l15xxx_syscfg.h" 47 | #elif defined(CONFIG_STM32_STM32F20XX) 48 | # include "chip/stm32f20xxx_syscfg.h" 49 | #elif defined(CONFIG_STM32_STM32F30XX) 50 | # include "chip/stm32f30xxx_syscfg.h" 51 | #elif defined(CONFIG_STM32_STM32F37XX) 52 | # include "chip/stm32f37xxx_syscfg.h" 53 | #elif defined(CONFIG_STM32_STM32F427) || defined(CONFIG_STM32_STM32F429) 54 | # include "chip/stm32f42xxx_syscfg.h" 55 | #elif defined(CONFIG_STM32_STM32F446) 56 | # include "chip/stm32f44xxx_syscfg.h" 57 | #elif defined(CONFIG_STM32_STM32F40XX) 58 | # include "chip/stm32f40xxx_syscfg.h" 59 | #endif 60 | 61 | /**************************************************************************************************** 62 | * Pre-processor Definitions 63 | ****************************************************************************************************/ 64 | 65 | #endif /* __ARCH_ARM_SRC_STM32_STM32_SYSCFG_H */ 66 | -------------------------------------------------------------------------------- /arch/src/stm32f30xxx_rcc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thiemar/vectorcontrol/6bc7bcfdcfb4216523195ab530babfbd9a974024/arch/src/stm32f30xxx_rcc.c -------------------------------------------------------------------------------- /arch/src/stm32f37xxx_rcc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/thiemar/vectorcontrol/6bc7bcfdcfb4216523195ab530babfbd9a974024/arch/src/stm32f37xxx_rcc.c -------------------------------------------------------------------------------- /arch/src/up_exit.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * common/up_exit.c 3 | * 4 | * Copyright (C) 2007-2009, 201-2014 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | #include "up_internal.h" 44 | 45 | /**************************************************************************** 46 | * Pre-processor Definitions 47 | ****************************************************************************/ 48 | 49 | /**************************************************************************** 50 | * Private Data 51 | ****************************************************************************/ 52 | 53 | /**************************************************************************** 54 | * Private Functions 55 | ****************************************************************************/ 56 | 57 | void _exit(int status); 58 | 59 | /**************************************************************************** 60 | * Public Functions 61 | ****************************************************************************/ 62 | 63 | /**************************************************************************** 64 | * Name: _exit 65 | * 66 | * Description: 67 | * This function causes the currently executing task to cease 68 | * to exist. This is a special case of task_delete() where the task to 69 | * be deleted is the currently executing task. It is more complex because 70 | * a context switch must be perform to the next ready to run task. 71 | * 72 | ****************************************************************************/ 73 | 74 | void _exit(int status) 75 | { 76 | 77 | /* Disable interrupts. They will be restored when the next 78 | * task is started. 79 | */ 80 | 81 | (void)irqsave(); 82 | while (1); 83 | } 84 | 85 | -------------------------------------------------------------------------------- /arch/src/up_idle.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/common/up_idle.c 3 | * 4 | * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include "up_internal.h" 41 | 42 | /**************************************************************************** 43 | * Pre-processor Definitions 44 | ****************************************************************************/ 45 | 46 | /**************************************************************************** 47 | * Private Data 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Private Functions 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Public Functions 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Name: up_idle 60 | * 61 | * Description: 62 | * up_idle() is the logic that will be executed when their is no other 63 | * ready-to-run task. This is processor idle time and will continue until 64 | * some interrupt occurs to cause a context switch from the idle task. 65 | * 66 | * Processing in this state may be processor-specific. e.g., this is where 67 | * power management operations might be performed. 68 | * 69 | ****************************************************************************/ 70 | 71 | void up_idle(void) 72 | { 73 | #if defined(CONFIG_SUPPRESS_INTERRUPTS) || defined(CONFIG_SUPPRESS_TIMER_INTS) 74 | /* If the system is idle and there are no timer interrupts, then process 75 | * "fake" timer interrupts. Hopefully, something will wake up. 76 | */ 77 | 78 | sched_process_timer(); 79 | #else 80 | 81 | /* Sleep until an interrupt occurs to save power */ 82 | 83 | #if 0 84 | asm("WFI"); /* For example */ 85 | #endif 86 | #endif 87 | } 88 | 89 | -------------------------------------------------------------------------------- /arch/src/up_modifyreg16.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/common/up_modifyreg16.c 3 | * 4 | * Copyright (C) 2009 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | #include "irq.h" 44 | #include "up_arch.h" 45 | 46 | /**************************************************************************** 47 | * Pre-processor Definitions 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Private Data 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Private Functions 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Public Functions 60 | ****************************************************************************/ 61 | 62 | /**************************************************************************** 63 | * Name: modifyreg16 64 | * 65 | * Description: 66 | * Atomically modify the specified bits in a memory mapped register 67 | * 68 | ****************************************************************************/ 69 | 70 | void modifyreg16(unsigned int addr, uint16_t clearbits, uint16_t setbits) 71 | { 72 | irqstate_t flags; 73 | uint16_t regval; 74 | 75 | flags = irqsave(); 76 | regval = getreg16(addr); 77 | regval &= ~clearbits; 78 | regval |= setbits; 79 | putreg16(regval, addr); 80 | irqrestore(flags); 81 | } 82 | -------------------------------------------------------------------------------- /arch/src/up_modifyreg32.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/common/up_modifyreg32.c 3 | * 4 | * Copyright (C) 2009 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include "irq.h" 43 | #include "up_arch.h" 44 | 45 | /**************************************************************************** 46 | * Pre-processor Definitions 47 | ****************************************************************************/ 48 | 49 | /**************************************************************************** 50 | * Private Data 51 | ****************************************************************************/ 52 | 53 | /**************************************************************************** 54 | * Private Functions 55 | ****************************************************************************/ 56 | 57 | /**************************************************************************** 58 | * Public Functions 59 | ****************************************************************************/ 60 | 61 | /**************************************************************************** 62 | * Name: modifyreg32 63 | * 64 | * Description: 65 | * Atomically modify the specified bits in a memory mapped register 66 | * 67 | ****************************************************************************/ 68 | 69 | void modifyreg32(unsigned int addr, uint32_t clearbits, uint32_t setbits) 70 | { 71 | irqstate_t flags; 72 | uint32_t regval; 73 | 74 | flags = irqsave(); 75 | regval = getreg32(addr); 76 | regval &= ~clearbits; 77 | regval |= setbits; 78 | putreg32(regval, addr); 79 | irqrestore(flags); 80 | } 81 | -------------------------------------------------------------------------------- /arch/src/up_modifyreg8.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/common/up_modifyreg8.c 3 | * 4 | * Copyright (C) 2009 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | #include "irq.h" 44 | #include "up_arch.h" 45 | 46 | /**************************************************************************** 47 | * Pre-processor Definitions 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Private Data 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Private Functions 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Public Functions 60 | ****************************************************************************/ 61 | 62 | /**************************************************************************** 63 | * Name: modifyreg8 64 | * 65 | * Description: 66 | * Atomically modify the specified bits in a memory mapped register 67 | * 68 | ****************************************************************************/ 69 | 70 | void modifyreg8(unsigned int addr, uint8_t clearbits, uint8_t setbits) 71 | { 72 | irqstate_t flags; 73 | uint8_t regval; 74 | 75 | flags = irqsave(); 76 | regval = getreg8(addr); 77 | regval &= ~clearbits; 78 | regval |= setbits; 79 | putreg8(regval, addr); 80 | irqrestore(flags); 81 | } 82 | -------------------------------------------------------------------------------- /arch/src/up_systemreset.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/armv7-m/up_systemreset.c 3 | * 4 | * Copyright (C) 2012 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * Darcy Gong 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name NuttX nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /**************************************************************************** 38 | * Included Files 39 | ****************************************************************************/ 40 | 41 | #include 42 | 43 | #include "up_arch.h" 44 | #include "nvic.h" 45 | 46 | /**************************************************************************** 47 | * Public functions 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Name: up_systemreset 52 | * 53 | * Description: 54 | * Internal, Cortex-M0 reset logic. 55 | * 56 | ****************************************************************************/ 57 | 58 | void up_systemreset(void) 59 | { 60 | uint32_t regval; 61 | 62 | /* Set up for the system reset, retaining the priority group from the 63 | * the AIRCR register. 64 | */ 65 | 66 | regval = getreg32(NVIC_AIRCR) & NVIC_AIRCR_PRIGROUP_MASK; 67 | regval |= ((0x5fa << NVIC_AIRCR_VECTKEY_SHIFT) | NVIC_AIRCR_SYSRESETREQ); 68 | putreg32(regval, NVIC_AIRCR); 69 | 70 | /* Ensure completion of memory accesses */ 71 | 72 | __asm volatile ("dsb"); 73 | 74 | /* Wait for the reset */ 75 | 76 | for (;;); 77 | } 78 | 79 | 80 | /**************************************************************************** 81 | * Name: board_reset 82 | * 83 | * Description: 84 | * Reset board. This function may or may not be supported by a 85 | * particular board architecture. 86 | * 87 | * Input Parameters: 88 | * status - Status information provided with the reset event. This 89 | * meaning of this status information is board-specific. If not used by 90 | * a board, the value zero may be provided in calls to board_reset. 91 | * 92 | * Returned Value: 93 | * If this function returns, then it was not possible to power-off the 94 | * board due to some constraints. The return value int this case is a 95 | * board-specific reason for the failure to shutdown. 96 | * 97 | ****************************************************************************/ 98 | 99 | #ifdef CONFIG_BOARDCTL_RESET 100 | int board_reset(int status) 101 | { 102 | up_systemreset(); 103 | return 0; 104 | } 105 | #endif 106 | -------------------------------------------------------------------------------- /bootloader/include/bitmanip.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /* 35 | * @file board.h 36 | * 37 | * bootloader board interface 38 | * This file contains the common interfaces that all boards 39 | * have to supply 40 | */ 41 | 42 | #pragma once 43 | 44 | 45 | /************************************************************************************ 46 | * Included Files 47 | ************************************************************************************/ 48 | /**************************************************************************** 49 | * Pre-processor Definitions 50 | ****************************************************************************/ 51 | #define BIT(pos) (1ull<<(pos)) 52 | #define BIT_MASK(length) (BIT(length)-1) 53 | 54 | #define BITFIELD_MASK(lsb_pos, length) ( BIT_MASK(length) << (lsb_pos)) 55 | #define BITFIELD_ISOLATE(x, lsb_pos, length) ((x) & (BITFIELD_MASK((lsb_pos), (length)))) 56 | #define BITFIELD_EXCLUDE(x, lsb_pos, length) ((x) & ~(BITFIELD_MASK((lsb_pos), (length)))) 57 | #define BITFIELD_GET(y, lsb_pos, length) (((y)>>(lsb_pos)) & BIT_MASK(length)) 58 | #define BITFIELD_SET(y, x, lsb_pos, length) ( y= ((y) & ~BITFIELD_MASK(lsb_pos, length)) | BITFIELD_ISOLATE(x, lsb_pos, length)) 59 | -------------------------------------------------------------------------------- /bootloader/include/macros.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | /**************************************************************************** 40 | * Pre-processor Definitions 41 | ****************************************************************************/ 42 | 43 | #if !defined(arraySize) 44 | #define arraySize(a) (sizeof((a))/sizeof((a[0]))) 45 | #endif 46 | 47 | #if !defined(UNUSED) 48 | #define UNUSED(var) (void)(var) 49 | #endif 50 | 51 | #if !defined(CAT) 52 | #if !defined(_CAT) 53 | #define _CAT(a, b) a ## b 54 | #endif 55 | #define CAT(a, b) _CAT(a, b) 56 | #endif 57 | 58 | #if !defined(CCASSERT) 59 | #if defined(static_assert) 60 | # define CCASSERT(predicate) static_assert(predicate) 61 | # else 62 | # define CCASSERT(predicate) _x_CCASSERT_LINE(predicate, __LINE__) 63 | # if !defined(_x_CCASSERT_LINE) 64 | # define _x_CCASSERT_LINE(predicate, line) typedef char CAT(constraint_violated_on_line_,line)[2*((predicate)!=0)-1] __attribute__ ((unused)) ; 65 | # endif 66 | # endif 67 | #endif 68 | -------------------------------------------------------------------------------- /bootloader/include/random.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: Ben Dyer 5 | * Pavel Kirienko 6 | * David Sidrane 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | #pragma once 38 | 39 | /**************************************************************************** 40 | * Included Files 41 | ****************************************************************************/ 42 | /**************************************************************************** 43 | * Pre-processor Definitions 44 | ****************************************************************************/ 45 | #define CRC16_INITIAL 0xFFFFu 46 | #define CRC16_OUTPUT_XOR 0x0000u 47 | #define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull 48 | #define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull 49 | 50 | /**************************************************************************** 51 | * Public Type Definitions 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Global Variables 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Public Function Prototypes 60 | ****************************************************************************/ 61 | /**************************************************************************** 62 | * Name: util_srand 63 | * 64 | * Description: 65 | * This function seeds the random number generator 66 | * 67 | * 68 | * Input Parameters: 69 | * seed - The seed 70 | * 71 | * Returned value: 72 | * None 73 | * 74 | ****************************************************************************/ 75 | void util_srand(uint16_t seed); 76 | 77 | /**************************************************************************** 78 | * Name: util_random 79 | * 80 | * Description: 81 | * This function returns a random number between min and max 82 | * 83 | * 84 | * Input Parameters: 85 | * min - The minimum value the return value can be. 86 | * max - The maximum value the return value can be. 87 | * 88 | * Returned value: 89 | * A random number 90 | * 91 | ****************************************************************************/ 92 | uint16_t util_random(uint16_t min, uint16_t max); 93 | 94 | -------------------------------------------------------------------------------- /bootloader/include/uavcan_can_id_defs.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: Ben Dyer 5 | * Pavel Kirienko 6 | * David Sidrane 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name PX4 nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /* This file not a typical h file, is defines the UAVCAN dsdl 38 | * usage and may be included several times in header or source 39 | * file 40 | */ 41 | 42 | /* UAVCAN_BIT_DEFINE( field_name, lsb_pos, length) */ 43 | 44 | /* Message Frame Format */ 45 | UAVCAN_BIT_DEFINE(UavCanMessagePriority, 24, 5) 46 | UAVCAN_BIT_DEFINE(UavCanMessageTypeID, 8, 16) 47 | UAVCAN_BIT_DEFINE(UavCanMessageServiceNotMessage, 7, 1) 48 | UAVCAN_BIT_DEFINE(UavCanMessageSourceNodeID, 0, 7) 49 | /* Anonymous message Frame Format */ 50 | UAVCAN_BIT_DEFINE(UavCanAnonMessagePriority, 24, 5) 51 | UAVCAN_BIT_DEFINE(UavCanAnonMessageDiscriminator, 10, 14) 52 | UAVCAN_BIT_DEFINE(UavCanAnonMessageTypeID, 8, 2) 53 | UAVCAN_BIT_DEFINE(UavCanAnonMessageServiceNotMessage, 7, 1) 54 | UAVCAN_BIT_DEFINE(UavCanAnonMessageSourceNodeID, 0, 7) 55 | /* Service Frame Format */ 56 | UAVCAN_BIT_DEFINE(UavCanServicePriority, 24, 5) 57 | UAVCAN_BIT_DEFINE(UavCanServiceTypeID, 16, 8) 58 | UAVCAN_BIT_DEFINE(UavCanServiceRequestNotResponse, 15, 1) 59 | UAVCAN_BIT_DEFINE(UavCanServiceDestinationNodeID, 8, 7) 60 | UAVCAN_BIT_DEFINE(UavCanServiceServiceNotMessage, 7, 1) 61 | UAVCAN_BIT_DEFINE(UavCanServiceSourceNodeID, 0, 7) 62 | /* Tail Format */ 63 | UAVCAN_BIT_DEFINE(UavCanStartOfTransfer, 7, 1) 64 | UAVCAN_BIT_DEFINE(UavCanEndOfTransfer, 6, 1) 65 | UAVCAN_BIT_DEFINE(UavCanToggle, 5, 1) 66 | UAVCAN_BIT_DEFINE(UavCanTransferID, 0, 5) 67 | 68 | -------------------------------------------------------------------------------- /dsdl/readme.txt: -------------------------------------------------------------------------------- 1 | In project root directory: 2 | 3 | libuavcan_dsdlc --outdir libuavcan/include/dsdlc_generated/ dsdl/thiemar/ 4 | -------------------------------------------------------------------------------- /dsdl/thiemar/equipment/esc/20034.Status.uavcan: -------------------------------------------------------------------------------- 1 | # 2 | # FOC ESC extended status. 3 | # 4 | 5 | float16[2] i_dq # Ampere 6 | float16 i_setpoint # Ampere 7 | 8 | float16[2] v_dq # Volt 9 | float16 angle # Radian 10 | 11 | uint5 esc_index 12 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/debug.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Debug stuff, should only be used for library development. 3 | * Copyright (C) 2014 Pavel Kirienko 4 | */ 5 | 6 | #ifndef UAVCAN_DEBUG_HPP_INCLUDED 7 | #define UAVCAN_DEBUG_HPP_INCLUDED 8 | 9 | #include 10 | 11 | #if UAVCAN_DEBUG 12 | 13 | # include 14 | # include 15 | 16 | # if __GNUC__ 17 | __attribute__ ((format(printf, 2, 3))) 18 | # endif 19 | static void UAVCAN_TRACE(const char* src, const char* fmt, ...) 20 | { 21 | va_list args; 22 | (void)std::printf("UAVCAN: %s: ", src); 23 | va_start(args, fmt); 24 | (void)std::vprintf(fmt, args); 25 | va_end(args); 26 | (void)std::puts(""); 27 | } 28 | 29 | #else 30 | 31 | # define UAVCAN_TRACE(...) ((void)0) 32 | 33 | #endif 34 | 35 | #endif // UAVCAN_DEBUG_HPP_INCLUDED 36 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/driver/system_clock.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * System clock driver interface. 3 | * Copyright (C) 2014 Pavel Kirienko 4 | */ 5 | 6 | #ifndef UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED 7 | #define UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uavcan 14 | { 15 | 16 | /** 17 | * System clock interface - monotonic and UTC. 18 | */ 19 | class UAVCAN_EXPORT ISystemClock 20 | { 21 | public: 22 | virtual ~ISystemClock() { } 23 | 24 | /** 25 | * Monototic system clock. 26 | * 27 | * This clock shall never jump or change rate; the base time is irrelevant. 28 | * This clock is mandatory and must remain functional at all times. 29 | * 30 | * On POSIX systems use clock_gettime() with CLOCK_MONOTONIC. 31 | */ 32 | virtual MonotonicTime getMonotonic() const = 0; 33 | 34 | /** 35 | * Global network clock. 36 | * It doesn't have to be UTC, the name is a bit misleading - actual time base doesn't matter. 37 | * 38 | * This clock can be synchronized with other nodes on the bus, hence it can jump and/or change 39 | * rate occasionally. 40 | * This clock is optional; if it is not supported, return zero. Also return zero if the UTC time 41 | * is not available yet (e.g. the device has just started up with no battery clock). 42 | * 43 | * For POSIX refer to clock_gettime(), gettimeofday(). 44 | */ 45 | virtual UtcTime getUtc() const = 0; 46 | 47 | /** 48 | * Adjust the network-synchronized clock. 49 | * Refer to @ref getUtc() for details. 50 | * 51 | * For POSIX refer to adjtime(), settimeofday(). 52 | * 53 | * @param [in] adjustment Amount of time to add to the clock value. 54 | */ 55 | virtual void adjustUtc(UtcDuration adjustment) = 0; 56 | }; 57 | 58 | } 59 | 60 | #endif // UAVCAN_DRIVER_SYSTEM_CLOCK_HPP_INCLUDED 61 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/error.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_ERROR_HPP_INCLUDED 6 | #define UAVCAN_ERROR_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | namespace 14 | { 15 | /** 16 | * Common error codes. 17 | * 18 | * Functions that return signed integers may also return inverted error codes, 19 | * i.e. returned value should be inverted back to get the actual error code. 20 | * 21 | * Return code 0 (zero) means no error. 22 | * 23 | * @{ 24 | */ 25 | const int16_t ErrFailure = 1; ///< General failure 26 | const int16_t ErrInvalidParam = 2; 27 | const int16_t ErrMemory = 3; 28 | const int16_t ErrDriver = 4; ///< Platform driver error 29 | const int16_t ErrUnknownDataType = 5; 30 | const int16_t ErrInvalidMarshalData = 6; 31 | const int16_t ErrInvalidTransferListener = 7; 32 | const int16_t ErrNotInited = 8; 33 | const int16_t ErrRecursiveCall = 9; 34 | const int16_t ErrLogic = 10; 35 | const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode 36 | const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type 37 | const int16_t ErrInvalidConfiguration = 13; 38 | /** 39 | * @} 40 | */ 41 | 42 | } 43 | 44 | /** 45 | * Fatal error handler. 46 | * Behavior: 47 | * - If exceptions are enabled, throws std::runtime_error() with the supplied message text; 48 | * - If assertions are enabled (see UAVCAN_ASSERT()), aborts execution using zero assertion. 49 | * - Otherwise aborts execution via std::abort(). 50 | */ 51 | #if __GNUC__ 52 | __attribute__ ((noreturn)) 53 | #endif 54 | UAVCAN_EXPORT 55 | // coverity[+kill] 56 | void handleFatalError(const char* msg); 57 | 58 | } 59 | 60 | #endif // UAVCAN_ERROR_HPP_INCLUDED 61 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/helpers/ostream.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED 6 | #define UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | /** 14 | * Compact replacement for std::ostream for use on embedded systems. 15 | * Can be used for printing UAVCAN messages to stdout. 16 | * 17 | * Relevant discussion: https://groups.google.com/forum/#!topic/px4users/6c1CLNutN90 18 | * 19 | * Usage: 20 | * OStream::instance() << "Hello world!" << OStream::endl; 21 | */ 22 | class UAVCAN_EXPORT OStream : uavcan::Noncopyable 23 | { 24 | OStream() { } 25 | 26 | public: 27 | static OStream& instance() 28 | { 29 | static OStream s; 30 | return s; 31 | } 32 | 33 | static OStream& endl(OStream& stream) 34 | { 35 | std::printf("\n"); 36 | return stream; 37 | } 38 | }; 39 | 40 | inline OStream& operator<<(OStream& s, long long x) { std::printf("%lld", x); return s; } 41 | inline OStream& operator<<(OStream& s, unsigned long long x) { std::printf("%llu", x); return s; } 42 | 43 | inline OStream& operator<<(OStream& s, long x) { std::printf("%ld", x); return s; } 44 | inline OStream& operator<<(OStream& s, unsigned long x) { std::printf("%lu", x); return s; } 45 | 46 | inline OStream& operator<<(OStream& s, int x) { std::printf("%d", x); return s; } 47 | inline OStream& operator<<(OStream& s, unsigned int x) { std::printf("%u", x); return s; } 48 | 49 | inline OStream& operator<<(OStream& s, short x) { return operator<<(s, static_cast(x)); } 50 | inline OStream& operator<<(OStream& s, unsigned short x) { return operator<<(s, static_cast(x)); } 51 | 52 | inline OStream& operator<<(OStream& s, long double x) { std::printf("%Lg", x); return s; } 53 | inline OStream& operator<<(OStream& s, double x) { std::printf("%g", x); return s; } 54 | inline OStream& operator<<(OStream& s, float x) { return operator<<(s, static_cast(x)); } 55 | 56 | inline OStream& operator<<(OStream& s, char x) { std::printf("%c", x); return s; } 57 | inline OStream& operator<<(OStream& s, const char* x) { std::printf("%s", x); return s; } 58 | 59 | inline OStream& operator<<(OStream& s, OStream&(*manip)(OStream&)) { return manip(s); } 60 | 61 | } 62 | 63 | #endif // UAVCAN_HELPERS_OSTREAM_HPP_INCLUDED 64 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/marshal/bit_stream.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED 6 | #define UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | namespace uavcan 15 | { 16 | /** 17 | * This function implements fast copy of unaligned bit arrays. It isn't part of the library API, so it is not exported. 18 | * @param src_org Source array 19 | * @param src_offset Bit offset of the first source byte 20 | * @param src_len Number of bits to copy 21 | * @param dst_org Destination array 22 | * @param dst_offset Bit offset of the first destination byte 23 | */ 24 | void bitarrayCopy(const unsigned char* src_org, std::size_t src_offset, std::size_t src_len, 25 | unsigned char* dst_org, std::size_t dst_offset); 26 | 27 | /** 28 | * This class treats a chunk of memory as an array of bits. 29 | * It is used by the bit codec for serialization/deserialization. 30 | */ 31 | class UAVCAN_EXPORT BitStream 32 | { 33 | static const unsigned MaxBytesPerRW = 16; 34 | 35 | ITransferBuffer& buf_; 36 | unsigned bit_offset_; 37 | uint8_t byte_cache_; 38 | 39 | static inline unsigned bitlenToBytelen(unsigned bits) { return (bits + 7) / 8; } 40 | 41 | static inline void copyBitArrayAlignedToUnaligned(const uint8_t* src_org, unsigned src_len, 42 | uint8_t* dst_org, unsigned dst_offset) 43 | { 44 | bitarrayCopy(reinterpret_cast(src_org), 0, src_len, 45 | reinterpret_cast(dst_org), dst_offset); 46 | } 47 | 48 | static inline void copyBitArrayUnalignedToAligned(const uint8_t* src_org, unsigned src_offset, unsigned src_len, 49 | uint8_t* dst_org) 50 | { 51 | bitarrayCopy(reinterpret_cast(src_org), src_offset, src_len, 52 | reinterpret_cast(dst_org), 0); 53 | } 54 | 55 | public: 56 | static const unsigned MaxBitsPerRW = MaxBytesPerRW * 8; 57 | 58 | enum 59 | { 60 | ResultOutOfBuffer = 0, 61 | ResultOk = 1 62 | }; 63 | 64 | explicit BitStream(ITransferBuffer& buf) 65 | : buf_(buf) 66 | , bit_offset_(0) 67 | , byte_cache_(0) 68 | { 69 | StaticAssert::check(); 70 | } 71 | 72 | /** 73 | * Write/read calls interpret bytes as bit arrays, 8 bits per byte, where the most 74 | * significant bits have lower index, i.e.: 75 | * Hex: 55 2d 76 | * Bits: 01010101 00101101 77 | * Indices: 0 .. 7 8 .. 15 78 | * Return values: 79 | * Negative - Error 80 | * Zero - Out of buffer space 81 | * Positive - OK 82 | */ 83 | int write(const uint8_t* bytes, const unsigned bitlen); 84 | int read(uint8_t* bytes, const unsigned bitlen); 85 | 86 | #if UAVCAN_TOSTRING 87 | std::string toString() const; 88 | #endif 89 | }; 90 | 91 | } 92 | 93 | #endif // UAVCAN_MARSHAL_BIT_STREAM_HPP_INCLUDED 94 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/marshal/char_array_formatter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED 6 | #define UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) 12 | # error UAVCAN_CPP_VERSION 13 | #endif 14 | 15 | #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 16 | # include 17 | #endif 18 | 19 | namespace uavcan 20 | { 21 | 22 | #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 23 | 24 | template 25 | class UAVCAN_EXPORT CharArrayFormatter 26 | { 27 | ArrayType_& array_; 28 | 29 | template 30 | typename std::enable_if::value>::type 31 | writeValue(T value) 32 | { 33 | array_.template appendFormatted("%g", double(value)); 34 | } 35 | 36 | template 37 | typename std::enable_if::value>::type 38 | writeValue(T value) 39 | { 40 | if (std::is_same()) 41 | { 42 | if (array_.size() != array_.capacity()) 43 | { 44 | array_.push_back(typename ArrayType_::ValueType(value)); 45 | } 46 | } 47 | else if (std::is_signed()) 48 | { 49 | array_.template appendFormatted("%lli", static_cast(value)); 50 | } 51 | else 52 | { 53 | array_.template appendFormatted("%llu", static_cast(value)); 54 | } 55 | } 56 | 57 | template 58 | typename std::enable_if::value && !std::is_same::value>::type 59 | writeValue(T value) 60 | { 61 | array_.template appendFormatted("%p", static_cast(value)); 62 | } 63 | 64 | void writeValue(const char* value) 65 | { 66 | array_.template appendFormatted("%s", value); 67 | } 68 | 69 | public: 70 | typedef ArrayType_ ArrayType; 71 | 72 | explicit CharArrayFormatter(ArrayType& array) 73 | : array_(array) 74 | { } 75 | 76 | ArrayType& getArray() { return array_; } 77 | const ArrayType& getArray() const { return array_; } 78 | 79 | void write(const char* text) 80 | { 81 | writeValue(text); 82 | } 83 | 84 | template 85 | void write(const char* s, T value, Args... args) 86 | { 87 | while (s && *s) 88 | { 89 | if (*s == '%') 90 | { 91 | s += 1; 92 | if (*s != '%') 93 | { 94 | writeValue(value); 95 | write(++s, args...); 96 | break; 97 | } 98 | } 99 | writeValue(*s++); 100 | } 101 | } 102 | }; 103 | 104 | #else 105 | 106 | template 107 | class UAVCAN_EXPORT CharArrayFormatter 108 | { 109 | ArrayType_& array_; 110 | 111 | public: 112 | typedef ArrayType_ ArrayType; 113 | 114 | CharArrayFormatter(ArrayType& array) 115 | : array_(array) 116 | { } 117 | 118 | ArrayType& getArray() { return array_; } 119 | const ArrayType& getArray() const { return array_; } 120 | 121 | void write(const char* const text) 122 | { 123 | array_.template appendFormatted("%s", text); 124 | } 125 | 126 | /** 127 | * This version does not support more than one formatted argument, though it can be improved. 128 | * And it is unsafe. 129 | * There is typesafe version for C++11 above. 130 | */ 131 | template 132 | void write(const char* const format, const A value) 133 | { 134 | array_.template appendFormatted(format, value); 135 | } 136 | }; 137 | 138 | #endif 139 | 140 | } 141 | 142 | #endif // UAVCAN_MARSHAL_CHAR_ARRAY_FORMATTER_HPP_INCLUDED 143 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/marshal/type_util.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED 6 | #define UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace uavcan 13 | { 14 | /** 15 | * Read the specs to learn more about cast modes. 16 | */ 17 | enum CastMode { CastModeSaturate, CastModeTruncate }; 18 | 19 | /** 20 | * Read the specs to learn more about tail array optimizations. 21 | */ 22 | enum TailArrayOptimizationMode { TailArrayOptDisabled, TailArrayOptEnabled }; 23 | 24 | /** 25 | * Compile-time: Returns the number of bits needed to represent an integer value. 26 | */ 27 | template 28 | struct IntegerBitLen 29 | { 30 | enum { Result = 1 + IntegerBitLen<(Num >> 1)>::Result }; 31 | }; 32 | template <> 33 | struct IntegerBitLen<0> 34 | { 35 | enum { Result = 0 }; 36 | }; 37 | 38 | /** 39 | * Compile-time: Returns the number of bytes needed to contain the given number of bits. Assumes 1 byte == 8 bit. 40 | */ 41 | template 42 | struct BitLenToByteLen 43 | { 44 | enum { Result = (BitLen + 7) / 8 }; 45 | }; 46 | 47 | /** 48 | * Compile-time: Helper for integer and float specialization classes. Returns the platform-specific storage type. 49 | */ 50 | template 51 | struct StorageType 52 | { 53 | typedef T Type; 54 | }; 55 | template 56 | struct StorageType::Type> 57 | { 58 | typedef typename T::StorageType Type; 59 | }; 60 | 61 | /** 62 | * Compile-time: Whether T is a primitive type on this platform. 63 | */ 64 | template 65 | class IsPrimitiveType 66 | { 67 | typedef char Yes; 68 | struct No { Yes dummy[8]; }; 69 | 70 | template 71 | static typename EnableIf::Type test(int); 72 | 73 | template 74 | static No test(...); 75 | 76 | public: 77 | enum { Result = sizeof(test(0)) == sizeof(Yes) }; 78 | }; 79 | 80 | /** 81 | * Streams a given value into YAML string. Please see the specializations. 82 | */ 83 | template 84 | class UAVCAN_EXPORT YamlStreamer; 85 | 86 | } 87 | 88 | #endif // UAVCAN_MARSHAL_TYPE_UTIL_HPP_INCLUDED 89 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/marshal/types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_MARSHAL_TYPES_HPP_INCLUDED 6 | #define UAVCAN_MARSHAL_TYPES_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #endif // UAVCAN_MARSHAL_TYPES_HPP_INCLUDED 14 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/node/publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_NODE_PUBLISHER_HPP_INCLUDED 6 | #define UAVCAN_NODE_PUBLISHER_HPP_INCLUDED 7 | 8 | #include 9 | 10 | namespace uavcan 11 | { 12 | /** 13 | * Use this class to publish messages to the bus (broadcast, unicast, or both). 14 | * 15 | * @tparam DataType_ Message data type 16 | */ 17 | template 18 | class UAVCAN_EXPORT Publisher : protected GenericPublisher 19 | { 20 | typedef GenericPublisher BaseType; 21 | 22 | public: 23 | typedef DataType_ DataType; ///< Message data type 24 | 25 | /** 26 | * @param node Node instance this publisher will be registered with. 27 | * 28 | * @param tx_timeout If CAN frames of this message are not delivered to the bus 29 | * in this amount of time, they will be discarded. Default value 30 | * is good enough for rather high-frequency, high-priority messages. 31 | * 32 | * @param max_transfer_interval Maximum expected transfer interval. It's absolutely safe 33 | * to leave default value here. It just defines how soon the 34 | * Transfer ID tracking objects associated with this message type 35 | * will be garbage collected by the library if it's no longer 36 | * being published. 37 | */ 38 | explicit Publisher(INode& node, MonotonicDuration tx_timeout = getDefaultTxTimeout(), 39 | MonotonicDuration max_transfer_interval = TransferSender::getDefaultMaxTransferInterval()) 40 | : BaseType(node, tx_timeout, max_transfer_interval) 41 | { 42 | #if UAVCAN_DEBUG 43 | UAVCAN_ASSERT(getTxTimeout() == tx_timeout); // Making sure default values are OK 44 | #endif 45 | StaticAssert::check(); 46 | } 47 | 48 | /** 49 | * Broadcast the message. 50 | * Returns negative error code. 51 | */ 52 | int broadcast(const DataType& message) 53 | { 54 | return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast); 55 | } 56 | 57 | /** 58 | * Warning: You probably don't want to use this method; it's for advanced use cases like 59 | * e.g. network time synchronization. Use the overloaded method with fewer arguments instead. 60 | * This overload allows to explicitly specify Transfer ID. 61 | * Returns negative error code. 62 | */ 63 | int broadcast(const DataType& message, TransferID tid) 64 | { 65 | return BaseType::publish(message, TransferTypeMessageBroadcast, NodeID::Broadcast, tid); 66 | } 67 | 68 | static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(100); } 69 | 70 | /** 71 | * Init method can be called prior first publication, but it's not necessary 72 | * because the publisher can be automatically initialized ad-hoc. 73 | */ 74 | using BaseType::init; 75 | 76 | using BaseType::allowAnonymousTransfers; 77 | using BaseType::getTransferSender; 78 | using BaseType::getMinTxTimeout; 79 | using BaseType::getMaxTxTimeout; 80 | using BaseType::getTxTimeout; 81 | using BaseType::setTxTimeout; 82 | using BaseType::getPriority; 83 | using BaseType::setPriority; 84 | using BaseType::getNode; 85 | }; 86 | 87 | } 88 | 89 | #endif // UAVCAN_NODE_PUBLISHER_HPP_INCLUDED 90 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/node/sub_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_SUB_NODE_NODE_HPP_INCLUDED 6 | #define UAVCAN_SUB_NODE_NODE_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #if UAVCAN_TINY 13 | # error "This functionality is not available in tiny mode" 14 | #endif 15 | 16 | namespace uavcan 17 | { 18 | /** 19 | * This node object can be used in multiprocess UAVCAN nodes. 20 | * Please refer to the @ref Node<> for documentation concerning the template arguments; refer to the tutorials 21 | * to lean how to use libuavcan in multiprocess applications. 22 | */ 23 | template 24 | class UAVCAN_EXPORT SubNode : public INode 25 | { 26 | typedef typename 27 | Select<(MemPoolSize > 0), 28 | PoolAllocator, // If pool size is specified, use default allocator 29 | IPoolAllocator& // Otherwise use reference to user-provided allocator 30 | >::Result Allocator; 31 | 32 | Allocator pool_allocator_; 33 | Scheduler scheduler_; 34 | 35 | uint64_t internal_failure_cnt_; 36 | 37 | protected: 38 | virtual void registerInternalFailure(const char* msg) 39 | { 40 | internal_failure_cnt_++; 41 | UAVCAN_TRACE("Node", "Internal failure: %s", msg); 42 | (void)msg; 43 | } 44 | 45 | public: 46 | /** 47 | * This overload is only valid if MemPoolSize > 0. 48 | */ 49 | SubNode(ICanDriver& can_driver, 50 | ISystemClock& system_clock) : 51 | scheduler_(can_driver, pool_allocator_, system_clock), 52 | internal_failure_cnt_(0) 53 | { } 54 | 55 | /** 56 | * This overload is only valid if MemPoolSize == 0. 57 | */ 58 | SubNode(ICanDriver& can_driver, 59 | ISystemClock& system_clock, 60 | IPoolAllocator& allocator) : 61 | pool_allocator_(allocator), 62 | scheduler_(can_driver, pool_allocator_, system_clock), 63 | internal_failure_cnt_(0) 64 | { } 65 | 66 | virtual typename RemoveReference::Type& getAllocator() { return pool_allocator_; } 67 | 68 | virtual Scheduler& getScheduler() { return scheduler_; } 69 | virtual const Scheduler& getScheduler() const { return scheduler_; } 70 | 71 | uint64_t getInternalFailureCount() const { return internal_failure_cnt_; } 72 | }; 73 | 74 | } 75 | 76 | #endif // Include guard 77 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/node/subscriber.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED 6 | #define UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) 13 | # error UAVCAN_CPP_VERSION 14 | #endif 15 | 16 | #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 17 | # include 18 | #endif 19 | 20 | namespace uavcan 21 | { 22 | /** 23 | * Use this class to subscribe to a message. 24 | * 25 | * @tparam DataType_ Message data type. 26 | * 27 | * @tparam Callback_ Type of the callback that will be used to deliver received messages 28 | * into the application. Type of the argument of the callback can be either: 29 | * - DataType_& 30 | * - const DataType_& 31 | * - ReceivedDataStructure& 32 | * - const ReceivedDataStructure& 33 | * For the first two options, @ref ReceivedDataStructure<> will be casted implicitly. 34 | * In C++11 mode this type defaults to std::function<>. 35 | * In C++03 mode this type defaults to a plain function pointer; use binder to 36 | * call member functions as callbacks. 37 | */ 38 | template = UAVCAN_CPP11 40 | typename Callback_ = std::function&)> 41 | #else 42 | typename Callback_ = void (*)(const ReceivedDataStructure&) 43 | #endif 44 | > 45 | class UAVCAN_EXPORT Subscriber 46 | : public GenericSubscriber 47 | { 48 | public: 49 | typedef Callback_ Callback; 50 | 51 | private: 52 | typedef GenericSubscriber BaseType; 53 | 54 | Callback callback_; 55 | 56 | virtual void handleReceivedDataStruct(ReceivedDataStructure& msg) 57 | { 58 | if (coerceOrFallback(callback_, true)) 59 | { 60 | callback_(msg); 61 | } 62 | else 63 | { 64 | handleFatalError("Sub clbk"); 65 | } 66 | } 67 | 68 | public: 69 | typedef DataType_ DataType; 70 | 71 | explicit Subscriber(INode& node) 72 | : BaseType(node) 73 | , callback_() 74 | { 75 | StaticAssert::check(); 76 | } 77 | 78 | /** 79 | * Begin receiving messages. 80 | * Each message will be passed to the application via the callback. 81 | * Returns negative error code. 82 | */ 83 | int start(const Callback& callback) 84 | { 85 | stop(); 86 | 87 | if (!coerceOrFallback(callback, true)) 88 | { 89 | UAVCAN_TRACE("Subscriber", "Invalid callback"); 90 | return -ErrInvalidParam; 91 | } 92 | callback_ = callback; 93 | 94 | return BaseType::startAsMessageListener(); 95 | } 96 | 97 | using BaseType::allowAnonymousTransfers; 98 | using BaseType::stop; 99 | using BaseType::getFailureCount; 100 | }; 101 | 102 | } 103 | 104 | #endif // UAVCAN_NODE_SUBSCRIBER_HPP_INCLUDED 105 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/README.md: -------------------------------------------------------------------------------- 1 | High-level protocol logic 2 | ========================= 3 | 4 | Classes defined in this directory implement some high-level functions of UAVCAN. 5 | 6 | Since most applications won't use all of them at the same time, their definitions are provided right in the header 7 | files rather than in source files, in order to not pollute the build outputs with unused code (which is also a 8 | requirement for most safety-critical software). 9 | 10 | However, some of the high-level functions that are either always used by the library itself or those that are extremely 11 | likely to be used by the application are defined in .cpp files. 12 | 13 | When adding a new class here, please think twice before putting its definition into a .cpp file. 14 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED 7 | 8 | #include 9 | 10 | namespace uavcan 11 | { 12 | namespace dynamic_node_id_server 13 | { 14 | 15 | typedef centralized::Server CentralizedServer; 16 | 17 | } 18 | } 19 | 20 | #endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED 21 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED 7 | 8 | #include 9 | 10 | namespace uavcan 11 | { 12 | namespace dynamic_node_id_server 13 | { 14 | 15 | typedef distributed::Server DistributedServer; 16 | 17 | } 18 | } 19 | 20 | #endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED 21 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | namespace dynamic_node_id_server 14 | { 15 | namespace distributed 16 | { 17 | 18 | using namespace ::uavcan::protocol::dynamic_node_id::server; 19 | 20 | /** 21 | * Raft term 22 | */ 23 | typedef StorageType::Type Term; 24 | 25 | } 26 | } 27 | } 28 | 29 | #endif // Include guard 30 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | namespace dynamic_node_id_server 14 | { 15 | /** 16 | * Node ID allocation logic 17 | */ 18 | template 19 | class NodeIDSelector 20 | { 21 | typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const; 22 | 23 | const Owner* const owner_; 24 | const IsNodeIDTakenMethod is_node_id_taken_; 25 | 26 | public: 27 | NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken) 28 | : owner_(owner) 29 | , is_node_id_taken_(is_node_id_taken) 30 | { 31 | UAVCAN_ASSERT(owner_ != NULL); 32 | UAVCAN_ASSERT(is_node_id_taken_ != NULL); 33 | } 34 | 35 | /** 36 | * Reutrns a default-constructed (invalid) node ID if a free one could not be found. 37 | */ 38 | NodeID findFreeNodeID(const NodeID preferred) const 39 | { 40 | uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; 41 | 42 | // Up 43 | while (candidate <= NodeID::MaxRecommendedForRegularNodes) 44 | { 45 | if (!(owner_->*is_node_id_taken_)(candidate)) 46 | { 47 | return candidate; 48 | } 49 | candidate++; 50 | } 51 | 52 | candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; 53 | 54 | // Down 55 | while (candidate > 0) 56 | { 57 | if (!(owner_->*is_node_id_taken_)(candidate)) 58 | { 59 | return candidate; 60 | } 61 | candidate--; 62 | } 63 | 64 | return NodeID(); 65 | } 66 | }; 67 | 68 | } 69 | } 70 | 71 | #endif // Include guard 72 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | namespace dynamic_node_id_server 14 | { 15 | /** 16 | * This interface is used by the server to read and write stable storage. 17 | * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 18 | * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward 19 | * and efficient implementation of storage backends, e.g. based on text files. 20 | * Keys and values may contain only non-whitespace, non-formatting printable characters. 21 | */ 22 | class UAVCAN_EXPORT IStorageBackend 23 | { 24 | public: 25 | /** 26 | * Maximum length of keys and values. One pair takes twice as much space. 27 | */ 28 | enum { MaxStringLength = 32 }; 29 | 30 | /** 31 | * It is guaranteed that the server will never require more than this number of key/value pairs. 32 | * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. 33 | */ 34 | enum { MaxKeyValuePairs = 512 }; 35 | 36 | /** 37 | * This type is used to exchange data chunks with the backend. 38 | * It doesn't use any dynamic memory; please refer to the Array<> class for details. 39 | */ 40 | typedef MakeString::Type String; 41 | 42 | /** 43 | * Read one value from the storage. 44 | * If such key does not exist, or if read failed, an empty string will be returned. 45 | * This method should not block for more than 50 ms. 46 | */ 47 | virtual String get(const String& key) const = 0; 48 | 49 | /** 50 | * Create or update value for the given key. Empty value should be regarded as a request to delete the key. 51 | * This method should not block for more than 50 ms. 52 | * Failures will be ignored. 53 | */ 54 | virtual void set(const String& key, const String& value) = 0; 55 | 56 | virtual ~IStorageBackend() { } 57 | }; 58 | 59 | } 60 | } 61 | 62 | #endif // Include guard 63 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED 7 | 8 | #include 9 | // UAVCAN types 10 | #include 11 | 12 | namespace uavcan 13 | { 14 | namespace dynamic_node_id_server 15 | { 16 | 17 | using namespace ::uavcan::protocol::dynamic_node_id; 18 | 19 | /** 20 | * Node Unique ID 21 | */ 22 | typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; 23 | 24 | } 25 | } 26 | 27 | #endif // Include guard 28 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/panic_broadcaster.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uavcan 14 | { 15 | /** 16 | * Helper for broadcasting the message uavcan.protocol.Panic. 17 | */ 18 | class UAVCAN_EXPORT PanicBroadcaster : private TimerBase 19 | { 20 | Publisher pub_; 21 | protocol::Panic msg_; 22 | 23 | void publishOnce() 24 | { 25 | const int res = pub_.broadcast(msg_); 26 | if (res < 0) 27 | { 28 | pub_.getNode().registerInternalFailure("Panic pub failed"); 29 | } 30 | } 31 | 32 | virtual void handleTimerEvent(const TimerEvent&) 33 | { 34 | publishOnce(); 35 | } 36 | 37 | public: 38 | explicit PanicBroadcaster(INode& node) 39 | : TimerBase(node) 40 | , pub_(node) 41 | { } 42 | 43 | /** 44 | * This method does not block and can't fail. 45 | * @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max. 46 | * If the string exceeds 7 characters, it will be truncated. 47 | * @param broadcasting_period Broadcasting period. Optional. 48 | * @param priority Transfer priority. Optional. 49 | */ 50 | void panic(const char* short_reason_description, 51 | MonotonicDuration broadcasting_period = MonotonicDuration::fromMSec(100), 52 | const TransferPriority priority = TransferPriority::Default) 53 | { 54 | msg_.reason_text.clear(); 55 | const char* p = short_reason_description; 56 | while (p && *p) 57 | { 58 | if (msg_.reason_text.size() == msg_.reason_text.capacity()) 59 | { 60 | break; 61 | } 62 | msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p)); 63 | p++; 64 | } 65 | 66 | UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); 67 | 68 | pub_.setTxTimeout(broadcasting_period); 69 | pub_.setPriority(priority); 70 | 71 | publishOnce(); 72 | startPeriodic(broadcasting_period); 73 | } 74 | 75 | /** 76 | * Stop broadcasting immediately. 77 | */ 78 | void dontPanic() // Where's my towel 79 | { 80 | stop(); 81 | msg_.reason_text.clear(); 82 | } 83 | 84 | bool isPanicking() const { return isRunning(); } 85 | 86 | const typename protocol::Panic::FieldTypes::reason_text& getReason() const 87 | { 88 | return msg_.reason_text; 89 | } 90 | }; 91 | 92 | } 93 | 94 | #endif // UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED 95 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/restart_request_server.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uavcan 14 | { 15 | /** 16 | * Implement this interface in the application to support the standard node restart service. 17 | */ 18 | class UAVCAN_EXPORT IRestartRequestHandler 19 | { 20 | public: 21 | virtual ~IRestartRequestHandler() { } 22 | 23 | /** 24 | * This method shall do either: 25 | * - restart the local node immediately; 26 | * - initiate the restart procedure to complete it asynchronously; 27 | * - reject the restart request and return false. 28 | * 29 | * If the restart requets was accepted, this method shall either return true or don't return at all. 30 | */ 31 | virtual bool handleRestartRequest(NodeID request_source) = 0; 32 | }; 33 | 34 | /** 35 | * Convenience class for supporting the standard node restart service. 36 | * Highly recommended to use. 37 | */ 38 | class UAVCAN_EXPORT RestartRequestServer : Noncopyable 39 | { 40 | typedef MethodBinder&, 42 | protocol::RestartNode::Response&) const> RestartNodeCallback; 43 | 44 | ServiceServer srv_; 45 | IRestartRequestHandler* handler_; 46 | 47 | void handleRestartNode(const ReceivedDataStructure& request, 48 | protocol::RestartNode::Response& response) const 49 | { 50 | UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get())); 51 | response.ok = false; 52 | if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER) 53 | { 54 | if (handler_) 55 | { 56 | response.ok = handler_->handleRestartRequest(request.getSrcNodeID()); 57 | } 58 | UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected")); 59 | } 60 | else 61 | { 62 | UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx", 63 | static_cast(request.magic_number)); 64 | } 65 | } 66 | 67 | public: 68 | explicit RestartRequestServer(INode& node) 69 | : srv_(node) 70 | , handler_(NULL) 71 | { } 72 | 73 | /** 74 | * Restart request handler configuration. 75 | * All restart requests will be explicitly rejected if there's no handler installed. 76 | */ 77 | IRestartRequestHandler* getHandler() const { return handler_; } 78 | void setHandler(IRestartRequestHandler* handler) { handler_ = handler; } 79 | 80 | /** 81 | * Starts the server. 82 | * Returns negative error code. 83 | */ 84 | int start() 85 | { 86 | return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode)); 87 | } 88 | }; 89 | 90 | } 91 | 92 | #endif // UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED 93 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/protocol/transport_stats_provider.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED 6 | #define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uavcan 14 | { 15 | /** 16 | * This class provides statistics about the transport layer performance on the local node. 17 | * The user's application does not deal with this class directly because it's instantiated by the node class. 18 | */ 19 | class UAVCAN_EXPORT TransportStatsProvider : Noncopyable 20 | { 21 | typedef MethodBinder 24 | GetTransportStatsCallback; 25 | 26 | ServiceServer srv_; 27 | 28 | void handleGetTransportStats(const protocol::GetTransportStats::Request&, 29 | protocol::GetTransportStats::Response& resp) const 30 | { 31 | const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter(); 32 | resp.transfer_errors = perf.getErrorCount(); 33 | resp.transfers_tx = perf.getTxTransferCount(); 34 | resp.transfers_rx = perf.getRxTransferCount(); 35 | 36 | const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager(); 37 | for (uint8_t i = 0; i < canio.getNumIfaces(); i++) 38 | { 39 | const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i); 40 | protocol::CANIfaceStats stats; 41 | stats.errors = can_perf.errors; 42 | stats.frames_tx = can_perf.frames_tx; 43 | stats.frames_rx = can_perf.frames_rx; 44 | resp.can_iface_stats.push_back(stats); 45 | } 46 | } 47 | 48 | public: 49 | explicit TransportStatsProvider(INode& node) 50 | : srv_(node) 51 | { } 52 | 53 | /** 54 | * Once started, this class requires no further attention. 55 | * Returns negative error code. 56 | */ 57 | int start() 58 | { 59 | return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats)); 60 | } 61 | }; 62 | 63 | } 64 | 65 | #endif // UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED 66 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/std.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_STD_HPP_INCLUDED 6 | #define UAVCAN_STD_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) 13 | # error UAVCAN_CPP_VERSION 14 | #endif 15 | 16 | #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 17 | 18 | # include 19 | # include 20 | 21 | namespace uavcan 22 | { 23 | 24 | typedef std::uint8_t uint8_t; 25 | typedef std::uint16_t uint16_t; 26 | typedef std::uint32_t uint32_t; 27 | typedef std::uint64_t uint64_t; 28 | 29 | typedef std::int8_t int8_t; 30 | typedef std::int16_t int16_t; 31 | typedef std::int32_t int32_t; 32 | typedef std::int64_t int64_t; 33 | 34 | } 35 | 36 | #else 37 | 38 | # include // Standard integer types from C library 39 | # include // vsnprintf() from the C library 40 | 41 | namespace uavcan 42 | { 43 | 44 | typedef ::uint8_t uint8_t; 45 | typedef ::uint16_t uint16_t; 46 | typedef ::uint32_t uint32_t; 47 | typedef ::uint64_t uint64_t; 48 | 49 | typedef ::int8_t int8_t; 50 | typedef ::int16_t int16_t; 51 | typedef ::int32_t int32_t; 52 | typedef ::int64_t int64_t; 53 | 54 | } 55 | 56 | #endif 57 | 58 | namespace uavcan 59 | { 60 | /** 61 | * Wrapper over the standard snprintf(). This wrapper is needed because different standards and different 62 | * implementations of C++ do not agree whether snprintf() should be defined in std:: or in ::. The solution 63 | * is to use 'using namespace std' hack inside the wrapper, so the compiler will be able to pick whatever 64 | * definition is available in the standard library. Alternatively, the user's application can provide a 65 | * custom implementation of uavcan::snprintf(). 66 | */ 67 | #if __GNUC__ 68 | __attribute__ ((format(printf, 3, 4))) 69 | #endif 70 | extern int snprintf(char* out, std::size_t maxlen, const char* format, ...); 71 | 72 | #if !UAVCAN_USE_EXTERNAL_SNPRINTF 73 | inline int snprintf(char* out, std::size_t maxlen, const char* format, ...) 74 | { 75 | using namespace std; // This way we can pull vsnprintf() either from std:: or from ::. 76 | va_list args; 77 | va_start(args, format); 78 | const int return_value = vsnprintf(out, maxlen, format, args); 79 | va_end(args); 80 | return return_value; 81 | } 82 | #endif 83 | 84 | } 85 | 86 | #endif // UAVCAN_STD_HPP_INCLUDED 87 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/transport/abstract_transfer_buffer.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED 6 | #define UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | /** 14 | * API for transfer buffer users. 15 | */ 16 | class UAVCAN_EXPORT ITransferBuffer 17 | { 18 | public: 19 | virtual ~ITransferBuffer() { } 20 | 21 | virtual int read(unsigned offset, uint8_t* data, unsigned len) const = 0; 22 | virtual int write(unsigned offset, const uint8_t* data, unsigned len) = 0; 23 | }; 24 | 25 | } 26 | 27 | #endif // UAVCAN_TRANSPORT_ABSTRACT_TRANSFER_BUFFER_HPP_INCLUDED 28 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/transport/crc.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_TRANSPORT_CRC_HPP_INCLUDED 6 | #define UAVCAN_TRANSPORT_CRC_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace uavcan 13 | { 14 | 15 | /** 16 | * CRC-16-CCITT 17 | * Initial value: 0xFFFF 18 | * Poly: 0x1021 19 | * Reverse: no 20 | * Output xor: 0 21 | * 22 | * import crcmod 23 | * crc = crcmod.predefined.Crc('crc-ccitt-false') 24 | * crc.update('123456789') 25 | * crc.hexdigest() 26 | * '29B1' 27 | */ 28 | class UAVCAN_EXPORT TransferCRC 29 | { 30 | #if !UAVCAN_TINY 31 | static const uint16_t Table[256]; 32 | #endif 33 | 34 | uint16_t value_; 35 | 36 | public: 37 | enum { NumBytes = 2 }; 38 | 39 | TransferCRC() 40 | : value_(0xFFFFU) 41 | { } 42 | 43 | #if UAVCAN_TINY 44 | void add(uint8_t byte) 45 | { 46 | value_ ^= uint16_t(uint16_t(byte) << 8); 47 | for (uint8_t bit = 8; bit > 0; --bit) 48 | { 49 | if (value_ & 0x8000U) 50 | { 51 | value_ = uint16_t(uint16_t(value_ << 1) ^ 0x1021U); 52 | } 53 | else 54 | { 55 | value_ = uint16_t(value_ << 1); 56 | } 57 | } 58 | } 59 | #else 60 | void add(uint8_t byte) 61 | { 62 | value_ = uint16_t(uint16_t((value_ << 8) ^ Table[uint16_t((value_ >> 8) ^ byte) & 0xFFU]) & 0xFFFFU); 63 | } 64 | #endif 65 | 66 | void add(const uint8_t* bytes, unsigned len) 67 | { 68 | UAVCAN_ASSERT(bytes); 69 | while (len--) 70 | { 71 | add(*bytes++); 72 | } 73 | } 74 | 75 | uint16_t get() const { return value_; } 76 | }; 77 | 78 | } 79 | 80 | #endif // UAVCAN_TRANSPORT_CRC_HPP_INCLUDED 81 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/transport/perf_counter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED 6 | #define UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | namespace uavcan 12 | { 13 | 14 | #if UAVCAN_TINY 15 | 16 | class UAVCAN_EXPORT TransferPerfCounter 17 | { 18 | public: 19 | void addTxTransfer() { } 20 | void addRxTransfer() { } 21 | void addError() { } 22 | void addErrors(unsigned) { } 23 | uint64_t getTxTransferCount() const { return 0; } 24 | uint64_t getRxTransferCount() const { return 0; } 25 | uint64_t getErrorCount() const { return 0; } 26 | }; 27 | 28 | #else 29 | 30 | class UAVCAN_EXPORT TransferPerfCounter 31 | { 32 | uint64_t transfers_tx_; 33 | uint64_t transfers_rx_; 34 | uint64_t errors_; 35 | 36 | public: 37 | TransferPerfCounter() 38 | : transfers_tx_(0) 39 | , transfers_rx_(0) 40 | , errors_(0) 41 | { } 42 | 43 | void addTxTransfer() { transfers_tx_++; } 44 | void addRxTransfer() { transfers_rx_++; } 45 | 46 | void addError() { errors_++; } 47 | 48 | void addErrors(unsigned errors) 49 | { 50 | errors_ += errors; 51 | } 52 | 53 | uint64_t getTxTransferCount() const { return transfers_tx_; } 54 | uint64_t getRxTransferCount() const { return transfers_rx_; } 55 | uint64_t getErrorCount() const { return errors_; } 56 | }; 57 | 58 | #endif 59 | 60 | } 61 | 62 | #endif // UAVCAN_TRANSPORT_PERF_COUNTER_HPP_INCLUDED 63 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/transport/transfer_receiver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED 6 | #define UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | namespace uavcan 14 | { 15 | 16 | class UAVCAN_EXPORT TransferReceiver 17 | { 18 | public: 19 | enum ResultCode { ResultNotComplete, ResultComplete, ResultSingleFrame }; 20 | 21 | static const uint16_t MinTransferIntervalMSec = 1; 22 | static const uint16_t MaxTransferIntervalMSec = 0xFFFF; 23 | static const uint16_t DefaultTransferIntervalMSec = 1000; 24 | static const uint16_t DefaultTidTimeoutMSec = 1000; 25 | 26 | static MonotonicDuration getDefaultTransferInterval() 27 | { 28 | return MonotonicDuration::fromMSec(DefaultTransferIntervalMSec); 29 | } 30 | static MonotonicDuration getMinTransferInterval() { return MonotonicDuration::fromMSec(MinTransferIntervalMSec); } 31 | static MonotonicDuration getMaxTransferInterval() { return MonotonicDuration::fromMSec(MaxTransferIntervalMSec); } 32 | 33 | private: 34 | enum { IfaceIndexNotSet = MaxCanIfaces }; 35 | 36 | enum { ErrorCntMask = 31 }; 37 | enum { IfaceIndexMask = MaxCanIfaces }; 38 | 39 | MonotonicTime prev_transfer_ts_; 40 | MonotonicTime this_transfer_ts_; 41 | UtcTime first_frame_ts_; 42 | uint16_t transfer_interval_msec_; 43 | uint16_t this_transfer_crc_; 44 | 45 | uint16_t buffer_write_pos_; 46 | 47 | TransferID tid_; // 1 byte field 48 | 49 | // 1 byte aligned bitfields: 50 | uint8_t next_toggle_ : 1; 51 | uint8_t iface_index_ : 2; 52 | mutable uint8_t error_cnt_ : 5; 53 | 54 | bool isInitialized() const { return iface_index_ != IfaceIndexNotSet; } 55 | 56 | bool isMidTransfer() const { return buffer_write_pos_ > 0; } 57 | 58 | MonotonicDuration getIfaceSwitchDelay() const; 59 | MonotonicDuration getTidTimeout() const; 60 | 61 | void registerError() const; 62 | 63 | void updateTransferTimings(); 64 | void prepareForNextTransfer(); 65 | 66 | bool validate(const RxFrame& frame) const; 67 | bool writePayload(const RxFrame& frame, ITransferBuffer& buf); 68 | ResultCode receive(const RxFrame& frame, TransferBufferAccessor& tba); 69 | 70 | public: 71 | TransferReceiver() : 72 | transfer_interval_msec_(DefaultTransferIntervalMSec), 73 | this_transfer_crc_(0), 74 | buffer_write_pos_(0), 75 | next_toggle_(false), 76 | iface_index_(IfaceIndexNotSet), 77 | error_cnt_(0) 78 | { } 79 | 80 | bool isTimedOut(MonotonicTime current_ts) const; 81 | 82 | ResultCode addFrame(const RxFrame& frame, TransferBufferAccessor& tba); 83 | 84 | uint8_t yieldErrorCount(); 85 | 86 | MonotonicTime getLastTransferTimestampMonotonic() const { return prev_transfer_ts_; } 87 | UtcTime getLastTransferTimestampUtc() const { return first_frame_ts_; } 88 | 89 | uint16_t getLastTransferCrc() const { return this_transfer_crc_; } 90 | 91 | MonotonicDuration getInterval() const { return MonotonicDuration::fromMSec(transfer_interval_msec_); } 92 | }; 93 | 94 | } 95 | 96 | #endif // UAVCAN_TRANSPORT_TRANSFER_RECEIVER_HPP_INCLUDED 97 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/uavcan.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | * 4 | * This header should be included by the user application. 5 | */ 6 | 7 | #ifndef UAVCAN_UAVCAN_HPP_INCLUDED 8 | #define UAVCAN_UAVCAN_HPP_INCLUDED 9 | 10 | #include 11 | #include 12 | 13 | // High-level node logic 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | // Util 23 | #include 24 | #include 25 | #include 26 | 27 | #endif // UAVCAN_UAVCAN_HPP_INCLUDED 28 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/util/method_binder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED 6 | #define UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace uavcan 13 | { 14 | /** 15 | * Use this to call member functions as callbacks in C++03 mode. 16 | * 17 | * In C++11 or newer you don't need it because you can use std::function<>/std::bind<> instead. 18 | */ 19 | template 20 | class UAVCAN_EXPORT MethodBinder 21 | { 22 | ObjectPtr obj_; 23 | MemFunPtr fun_; 24 | 25 | void validateBeforeCall() const 26 | { 27 | if (!operator bool()) 28 | { 29 | handleFatalError("Null binder"); 30 | } 31 | } 32 | 33 | public: 34 | MethodBinder() 35 | : obj_() 36 | , fun_() 37 | { } 38 | 39 | MethodBinder(ObjectPtr o, MemFunPtr f) 40 | : obj_(o) 41 | , fun_(f) 42 | { } 43 | 44 | /** 45 | * Returns true if the binder is initialized (doesn't contain null pointers). 46 | */ 47 | operator bool() const 48 | { 49 | return coerceOrFallback(obj_, true) && coerceOrFallback(fun_, true); 50 | } 51 | 52 | /** 53 | * Will raise a fatal error if either method pointer or object pointer are null. 54 | */ 55 | void operator()() 56 | { 57 | validateBeforeCall(); 58 | (obj_->*fun_)(); 59 | } 60 | 61 | /** 62 | * Will raise a fatal error if either method pointer or object pointer are null. 63 | */ 64 | template 65 | void operator()(Par1& p1) 66 | { 67 | validateBeforeCall(); 68 | (obj_->*fun_)(p1); 69 | } 70 | 71 | /** 72 | * Will raise a fatal error if either method pointer or object pointer are null. 73 | */ 74 | template 75 | void operator()(Par1& p1, Par2& p2) 76 | { 77 | validateBeforeCall(); 78 | (obj_->*fun_)(p1, p2); 79 | } 80 | }; 81 | 82 | } 83 | 84 | #endif // UAVCAN_UTIL_METHOD_BINDER_HPP_INCLUDED 85 | -------------------------------------------------------------------------------- /libuavcan/include/uavcan/util/placement_new.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #ifndef UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED 6 | #define UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED 7 | 8 | #include 9 | #include 10 | 11 | /* 12 | * Some embedded C++ implementations don't implement the placement new operator. 13 | * Define UAVCAN_IMPLEMENT_PLACEMENT_NEW to apply this workaround. 14 | */ 15 | 16 | #ifndef UAVCAN_IMPLEMENT_PLACEMENT_NEW 17 | # error UAVCAN_IMPLEMENT_PLACEMENT_NEW 18 | #endif 19 | 20 | #if UAVCAN_IMPLEMENT_PLACEMENT_NEW 21 | 22 | inline void* operator new (std::size_t, void* ptr) throw() 23 | { 24 | return ptr; 25 | } 26 | inline void* operator new[](std::size_t, void* ptr) throw() 27 | { 28 | return ptr; 29 | } 30 | 31 | inline void operator delete (void*, void*) throw() { } 32 | inline void operator delete[](void*, void*) throw() { } 33 | 34 | #else 35 | # include 36 | #endif 37 | 38 | #endif // UAVCAN_UTIL_PLACEMENT_NEW_HPP_INCLUDED 39 | -------------------------------------------------------------------------------- /libuavcan/src/driver/uc_can.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * CAN bus driver interface. 3 | * Copyright (C) 2014 Pavel Kirienko 4 | */ 5 | 6 | #include 7 | #include 8 | 9 | namespace uavcan 10 | { 11 | 12 | const uint32_t CanFrame::MaskStdID; 13 | const uint32_t CanFrame::MaskExtID; 14 | const uint32_t CanFrame::FlagEFF; 15 | const uint32_t CanFrame::FlagRTR; 16 | const uint32_t CanFrame::FlagERR; 17 | const uint8_t CanFrame::MaxDataLen; 18 | 19 | bool CanFrame::priorityHigherThan(const CanFrame& rhs) const 20 | { 21 | const uint32_t clean_id = id & MaskExtID; 22 | const uint32_t rhs_clean_id = rhs.id & MaskExtID; 23 | 24 | /* 25 | * STD vs EXT - if 11 most significant bits are the same, EXT loses. 26 | */ 27 | const bool ext = id & FlagEFF; 28 | const bool rhs_ext = rhs.id & FlagEFF; 29 | if (ext != rhs_ext) 30 | { 31 | const uint32_t arb11 = ext ? (clean_id >> 18) : clean_id; 32 | const uint32_t rhs_arb11 = rhs_ext ? (rhs_clean_id >> 18) : rhs_clean_id; 33 | if (arb11 != rhs_arb11) 34 | { 35 | return arb11 < rhs_arb11; 36 | } 37 | else 38 | { 39 | return rhs_ext; 40 | } 41 | } 42 | 43 | /* 44 | * RTR vs Data frame - if frame identifiers and frame types are the same, RTR loses. 45 | */ 46 | const bool rtr = id & FlagRTR; 47 | const bool rhs_rtr = rhs.id & FlagRTR; 48 | if (clean_id == rhs_clean_id && rtr != rhs_rtr) 49 | { 50 | return rhs_rtr; 51 | } 52 | 53 | /* 54 | * Plain ID arbitration - greater value loses. 55 | */ 56 | return clean_id < rhs_clean_id; 57 | } 58 | 59 | #if UAVCAN_TOSTRING 60 | std::string CanFrame::toString(StringRepresentation mode) const 61 | { 62 | UAVCAN_ASSERT(mode == StrTight || mode == StrAligned); 63 | 64 | static const unsigned AsciiColumnOffset = 36U; 65 | 66 | char buf[50]; 67 | char* wpos = buf; 68 | char* const epos = buf + sizeof(buf); 69 | fill(buf, buf + sizeof(buf), '\0'); 70 | 71 | if (id & FlagEFF) 72 | { 73 | wpos += snprintf(wpos, unsigned(epos - wpos), "0x%08x ", unsigned(id & MaskExtID)); 74 | } 75 | else 76 | { 77 | const char* const fmt = (mode == StrAligned) ? " 0x%03x " : "0x%03x "; 78 | wpos += snprintf(wpos, unsigned(epos - wpos), fmt, unsigned(id & MaskStdID)); 79 | } 80 | 81 | if (id & FlagRTR) 82 | { 83 | wpos += snprintf(wpos, unsigned(epos - wpos), " RTR"); 84 | } 85 | else if (id & FlagERR) 86 | { 87 | wpos += snprintf(wpos, unsigned(epos - wpos), " ERR"); 88 | } 89 | else 90 | { 91 | for (int dlen = 0; dlen < dlc; dlen++) // hex bytes 92 | { 93 | wpos += snprintf(wpos, unsigned(epos - wpos), " %02x", unsigned(data[dlen])); 94 | } 95 | 96 | while ((mode == StrAligned) && (wpos < buf + AsciiColumnOffset)) // alignment 97 | { 98 | *wpos++ = ' '; 99 | } 100 | 101 | wpos += snprintf(wpos, unsigned(epos - wpos), " \'"); // ascii 102 | for (int dlen = 0; dlen < dlc; dlen++) 103 | { 104 | uint8_t ch = data[dlen]; 105 | if (ch < 0x20 || ch > 0x7E) 106 | { 107 | ch = '.'; 108 | } 109 | wpos += snprintf(wpos, unsigned(epos - wpos), "%c", ch); 110 | } 111 | wpos += snprintf(wpos, unsigned(epos - wpos), "\'"); 112 | } 113 | (void)wpos; 114 | return std::string(buf); 115 | } 116 | #endif 117 | 118 | } 119 | -------------------------------------------------------------------------------- /libuavcan/src/marshal/uc_bit_array_copy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace uavcan 11 | { 12 | void bitarrayCopy(const unsigned char* src, std::size_t src_offset, std::size_t src_len, 13 | unsigned char* dst, std::size_t dst_offset) 14 | { 15 | /* 16 | * Should never be called on a zero-length buffer. The caller will also ensure that the bit 17 | * offsets never exceed one byte. 18 | */ 19 | 20 | UAVCAN_ASSERT(src_len > 0U); 21 | UAVCAN_ASSERT(src_offset < 8U && dst_offset < 8U); 22 | 23 | const std::size_t last_bit = src_offset + src_len; 24 | while (last_bit - src_offset) 25 | { 26 | const uint8_t src_bit_offset = src_offset % 8U; 27 | const uint8_t dst_bit_offset = dst_offset % 8U; 28 | 29 | // The number of bits to copy 30 | const uint8_t max_offset = uavcan::max(src_bit_offset, dst_bit_offset); 31 | const std::size_t copy_bits = uavcan::min(last_bit - src_offset, (std::size_t)(8U - max_offset)); 32 | 33 | /* 34 | * The mask indicating which bits of dest to update: 35 | * dst_byte_offset copy_bits write_mask 36 | * 0 8 11111111 37 | * 0 7 11111110 38 | * ... 39 | * 0 1 10000000 40 | * ... 41 | * 4 4 00001111 42 | * 4 3 00001110 43 | * 4 2 00001100 44 | * 4 1 00001000 45 | * ... 46 | * 7 1 00000001 47 | */ 48 | const uint8_t write_mask = uint8_t(uint8_t(0xFF00U >> copy_bits) >> dst_bit_offset); 49 | 50 | // The value to be extracted from src, shifted into the dst location 51 | const uint8_t src_data = uint8_t((src[src_offset / 8U] << src_bit_offset) >> dst_bit_offset); 52 | 53 | dst[dst_offset / 8U] = uint8_t((dst[dst_offset / 8U] & ~write_mask) | (src_data & write_mask)); 54 | 55 | src_offset += copy_bits; 56 | dst_offset += copy_bits; 57 | } 58 | } 59 | } 60 | -------------------------------------------------------------------------------- /libuavcan/src/marshal/uc_bit_stream.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uavcan 10 | { 11 | 12 | const unsigned BitStream::MaxBytesPerRW; 13 | const unsigned BitStream::MaxBitsPerRW; 14 | 15 | int BitStream::write(const uint8_t* bytes, const unsigned bitlen) 16 | { 17 | // Temporary buffer is needed to merge new bits with cached unaligned bits from the last write() (see byte_cache_) 18 | uint8_t tmp[MaxBytesPerRW + 1]; 19 | 20 | // Tmp space must be large enough to accomodate new bits AND unaligned bits from the last write() 21 | const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); 22 | UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); 23 | tmp[0] = 0; 24 | tmp[bytelen - 1] = 0; 25 | 26 | copyBitArrayAlignedToUnaligned(bytes, bitlen, tmp, bit_offset_ % 8); 27 | 28 | const unsigned new_bit_offset = bit_offset_ + bitlen; 29 | 30 | // Bitcopy algorithm resets skipped bits in the first byte. Restore them back. 31 | tmp[0] |= byte_cache_; 32 | 33 | // (new_bit_offset % 8 == 0) means that this write was perfectly aligned. 34 | byte_cache_ = uint8_t((new_bit_offset % 8) ? tmp[bytelen - 1] : 0); 35 | 36 | /* 37 | * Dump the data into the destination buffer. 38 | * Note that if this write was unaligned, last written byte in the buffer will be rewritten with updated value 39 | * within the next write() operation. 40 | */ 41 | const int write_res = buf_.write(bit_offset_ / 8, tmp, bytelen); 42 | if (write_res < 0) 43 | { 44 | return write_res; 45 | } 46 | if (static_cast(write_res) < bytelen) 47 | { 48 | return ResultOutOfBuffer; 49 | } 50 | 51 | bit_offset_ = new_bit_offset; 52 | return ResultOk; 53 | } 54 | 55 | int BitStream::read(uint8_t* bytes, const unsigned bitlen) 56 | { 57 | uint8_t tmp[MaxBytesPerRW + 1]; 58 | 59 | const unsigned bytelen = bitlenToBytelen(bitlen + (bit_offset_ % 8)); 60 | UAVCAN_ASSERT(MaxBytesPerRW >= bytelen); 61 | 62 | const int read_res = buf_.read(bit_offset_ / 8, tmp, bytelen); 63 | if (read_res < 0) 64 | { 65 | return read_res; 66 | } 67 | if (static_cast(read_res) < bytelen) 68 | { 69 | return ResultOutOfBuffer; 70 | } 71 | 72 | bytes[bitlenToBytelen(bitlen) - 1] = 0; 73 | copyBitArrayUnalignedToAligned(tmp, bit_offset_ % 8, bitlen, bytes); 74 | bit_offset_ += bitlen; 75 | return ResultOk; 76 | } 77 | 78 | #if UAVCAN_TOSTRING 79 | std::string BitStream::toString() const 80 | { 81 | std::string out; 82 | out.reserve(128); 83 | 84 | for (unsigned offset = 0; true; offset++) 85 | { 86 | uint8_t byte = 0; 87 | if (1U != buf_.read(offset, &byte, 1U)) 88 | { 89 | break; 90 | } 91 | for (int i = 7; i >= 0; i--) // Most significant goes first 92 | { 93 | out += (byte & (1 << i)) ? '1' : '0'; 94 | } 95 | out += ' '; 96 | } 97 | if (out.length() > 0) 98 | { 99 | (void)out.erase(out.length() - 1, 1); 100 | } 101 | return out; 102 | } 103 | #endif 104 | 105 | } 106 | -------------------------------------------------------------------------------- /libuavcan/src/marshal/uc_float_spec.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uavcan 10 | { 11 | 12 | #if !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 13 | 14 | union Fp32 15 | { 16 | uint32_t u; 17 | float f; 18 | }; 19 | 20 | /* 21 | * IEEE754Converter 22 | */ 23 | uint16_t IEEE754Converter::nativeIeeeToHalf(float value) 24 | { 25 | /* 26 | * https://gist.github.com/rygorous/2156668 27 | * Public domain, by Fabian "ryg" Giesen 28 | */ 29 | const Fp32 f32infty = { 255U << 23 }; 30 | const Fp32 f16infty = { 31U << 23 }; 31 | const Fp32 magic = { 15U << 23 }; 32 | const uint32_t sign_mask = 0x80000000U; 33 | const uint32_t round_mask = ~0xFFFU; 34 | 35 | Fp32 in; 36 | uint16_t out; 37 | 38 | in.f = value; 39 | 40 | uint32_t sign = in.u & sign_mask; 41 | in.u ^= sign; 42 | 43 | if (in.u >= f32infty.u) /* Inf or NaN (all exponent bits set) */ 44 | { 45 | /* NaN->sNaN and Inf->Inf */ 46 | out = (in.u > f32infty.u) ? 0x7FFFU : 0x7C00U; 47 | } 48 | else /* (De)normalized number or zero */ 49 | { 50 | in.u &= round_mask; 51 | in.f *= magic.f; 52 | in.u -= round_mask; 53 | if (in.u > f16infty.u) 54 | { 55 | in.u = f16infty.u; /* Clamp to signed infinity if overflowed */ 56 | } 57 | 58 | out = uint16_t(in.u >> 13); /* Take the bits! */ 59 | } 60 | 61 | out |= uint16_t(sign >> 16); 62 | 63 | return out; 64 | } 65 | 66 | float IEEE754Converter::halfToNativeIeee(uint16_t value) 67 | { 68 | /* 69 | * https://gist.github.com/rygorous/2144712 70 | * Public domain, by Fabian "ryg" Giesen 71 | */ 72 | const Fp32 magic = { (254U - 15U) << 23 }; 73 | const Fp32 was_infnan = { (127U + 16U) << 23 }; 74 | Fp32 out; 75 | 76 | out.u = (value & 0x7FFFU) << 13; /* exponent/mantissa bits */ 77 | out.f *= magic.f; /* exponent adjust */ 78 | if (out.f >= was_infnan.f) /* make sure Inf/NaN survive */ 79 | { 80 | out.u |= 255U << 23; 81 | } 82 | out.u |= (value & 0x8000U) << 16; /* sign bit */ 83 | 84 | return out.f; 85 | } 86 | 87 | #endif // !UAVCAN_USE_EXTERNAL_FLOAT16_CONVERSION 88 | 89 | } 90 | -------------------------------------------------------------------------------- /libuavcan/src/marshal/uc_scalar_codec.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | 10 | void ScalarCodec::swapByteOrder(uint8_t* const bytes, const unsigned len) 11 | { 12 | UAVCAN_ASSERT(bytes); 13 | for (unsigned i = 0, j = len - 1; i < j; i++, j--) 14 | { 15 | const uint8_t c = bytes[i]; 16 | bytes[i] = bytes[j]; 17 | bytes[j] = c; 18 | } 19 | } 20 | 21 | int ScalarCodec::encodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) 22 | { 23 | UAVCAN_ASSERT(bytes); 24 | // Underlying stream class assumes that more significant bits have lower index, so we need to shift some. 25 | if (bitlen % 8) 26 | { 27 | bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] << ((8 - (bitlen % 8)) & 7)); 28 | } 29 | return stream_.write(bytes, bitlen); 30 | } 31 | 32 | int ScalarCodec::decodeBytesImpl(uint8_t* const bytes, const unsigned bitlen) 33 | { 34 | UAVCAN_ASSERT(bytes); 35 | const int read_res = stream_.read(bytes, bitlen); 36 | if (read_res > 0) 37 | { 38 | if (bitlen % 8) 39 | { 40 | bytes[bitlen / 8] = uint8_t(bytes[bitlen / 8] >> ((8 - (bitlen % 8)) & 7)); // As in encode(), vice versa 41 | } 42 | } 43 | return read_res; 44 | } 45 | 46 | } 47 | -------------------------------------------------------------------------------- /libuavcan/src/node/uc_generic_publisher.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | 10 | bool GenericPublisherBase::isInited() const 11 | { 12 | return sender_.isInitialized(); 13 | } 14 | 15 | int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) 16 | { 17 | if (isInited()) 18 | { 19 | return 0; 20 | } 21 | 22 | GlobalDataTypeRegistry::instance().freeze(); 23 | 24 | const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); 25 | if (descr == NULL) 26 | { 27 | UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); 28 | return -ErrUnknownDataType; 29 | } 30 | 31 | sender_.init(*descr, qos); 32 | 33 | return 0; 34 | } 35 | 36 | MonotonicTime GenericPublisherBase::getTxDeadline() const 37 | { 38 | return node_.getMonotonicTime() + tx_timeout_; 39 | } 40 | 41 | int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, 42 | NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline) 43 | { 44 | if (tid) 45 | { 46 | return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), 47 | blocking_deadline, transfer_type, dst_node_id, *tid); 48 | } 49 | else 50 | { 51 | return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), 52 | blocking_deadline, transfer_type, dst_node_id); 53 | } 54 | } 55 | 56 | void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) 57 | { 58 | tx_timeout = max(tx_timeout, getMinTxTimeout()); 59 | tx_timeout = min(tx_timeout, getMaxTxTimeout()); 60 | tx_timeout_ = tx_timeout; 61 | } 62 | 63 | } 64 | -------------------------------------------------------------------------------- /libuavcan/src/node/uc_generic_subscriber.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | 10 | int GenericSubscriberBase::genericStart(TransferListener* listener, 11 | bool (Dispatcher::*registration_method)(TransferListener*)) 12 | { 13 | if (listener == NULL) 14 | { 15 | UAVCAN_ASSERT(0); 16 | return -ErrLogic; 17 | } 18 | stop(listener); 19 | if (!(node_.getDispatcher().*registration_method)(listener)) 20 | { 21 | UAVCAN_TRACE("GenericSubscriber", "Failed to register transfer listener"); 22 | return -ErrInvalidTransferListener; 23 | } 24 | return 0; 25 | } 26 | 27 | void GenericSubscriberBase::stop(TransferListener* listener) 28 | { 29 | if (listener != NULL) 30 | { 31 | node_.getDispatcher().unregisterMessageListener(listener); 32 | node_.getDispatcher().unregisterServiceRequestListener(listener); 33 | node_.getDispatcher().unregisterServiceResponseListener(listener); 34 | } 35 | } 36 | 37 | } 38 | -------------------------------------------------------------------------------- /libuavcan/src/node/uc_service_client.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | /* 10 | * ServiceClientBase::CallState 11 | */ 12 | void ServiceClientBase::CallState::handleDeadline(MonotonicTime) 13 | { 14 | UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s", 15 | int(id_.server_node_id.get()), int(id_.transfer_id.get()), 16 | (owner_.data_type_descriptor_ == NULL) ? "???" : owner_.data_type_descriptor_->getFullName()); 17 | /* 18 | * What we're doing here is relaying execution from this call stack to a different one. 19 | * We need it because call registry cannot release memory from this callback, because this will destroy the 20 | * object method of which we're executing now. 21 | */ 22 | UAVCAN_ASSERT(timed_out_ == false); 23 | timed_out_ = true; 24 | owner_.generateDeadlineImmediately(); 25 | UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback"); 26 | } 27 | 28 | /* 29 | * ServiceClientBase 30 | */ 31 | int ServiceClientBase::prepareToCall(INode& node, 32 | const char* dtname, 33 | NodeID server_node_id, 34 | ServiceCallID& out_call_id) 35 | { 36 | /* 37 | * Making sure we're not going to get transport error because of invalid input data 38 | */ 39 | if (!server_node_id.isUnicast() || (server_node_id == node.getNodeID())) 40 | { 41 | UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID"); 42 | return -ErrInvalidParam; 43 | } 44 | out_call_id.server_node_id = server_node_id; 45 | 46 | /* 47 | * Determining the Data Type ID 48 | */ 49 | if (data_type_descriptor_ == NULL) 50 | { 51 | GlobalDataTypeRegistry::instance().freeze(); 52 | data_type_descriptor_ = GlobalDataTypeRegistry::instance().find(DataTypeKindService, dtname); 53 | if (data_type_descriptor_ == NULL) 54 | { 55 | UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", dtname); 56 | return -ErrUnknownDataType; 57 | } 58 | UAVCAN_TRACE("ServiceClient", "Data type descriptor inited: %s", data_type_descriptor_->toString().c_str()); 59 | } 60 | UAVCAN_ASSERT(data_type_descriptor_ != NULL); 61 | 62 | /* 63 | * Determining the Transfer ID 64 | */ 65 | const OutgoingTransferRegistryKey otr_key(data_type_descriptor_->getID(), 66 | TransferTypeServiceRequest, server_node_id); 67 | const MonotonicTime otr_deadline = node.getMonotonicTime() + TransferSender::getDefaultMaxTransferInterval(); 68 | TransferID* const otr_tid = 69 | node.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); 70 | if (!otr_tid) 71 | { 72 | UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str()); 73 | return -ErrMemory; 74 | } 75 | out_call_id.transfer_id = *otr_tid; 76 | otr_tid->increment(); 77 | 78 | return 0; 79 | } 80 | 81 | } 82 | -------------------------------------------------------------------------------- /libuavcan/src/node/uc_timer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | 8 | namespace uavcan 9 | { 10 | /* 11 | * TimerBase 12 | */ 13 | void TimerBase::handleDeadline(MonotonicTime current) 14 | { 15 | UAVCAN_ASSERT(!isRunning()); 16 | 17 | const MonotonicTime scheduled_time = getDeadline(); 18 | 19 | if (period_ < MonotonicDuration::getInfinite()) 20 | { 21 | startWithDeadline(scheduled_time + period_); 22 | } 23 | 24 | // Application can re-register the timer with different params, it's OK 25 | handleTimerEvent(TimerEvent(scheduled_time, current)); 26 | } 27 | 28 | void TimerBase::startOneShotWithDeadline(MonotonicTime deadline) 29 | { 30 | stop(); 31 | period_ = MonotonicDuration::getInfinite(); 32 | DeadlineHandler::startWithDeadline(deadline); 33 | } 34 | 35 | void TimerBase::startOneShotWithDelay(MonotonicDuration delay) 36 | { 37 | stop(); 38 | period_ = MonotonicDuration::getInfinite(); 39 | DeadlineHandler::startWithDelay(delay); 40 | } 41 | 42 | void TimerBase::startPeriodic(MonotonicDuration period) 43 | { 44 | UAVCAN_ASSERT(period < MonotonicDuration::getInfinite()); 45 | stop(); 46 | period_ = period; 47 | DeadlineHandler::startWithDelay(period); 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /libuavcan/src/transport/uc_crc.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | 10 | #if !UAVCAN_TINY 11 | 12 | // print ', '.join(map(lambda x: '%04x' % x, map(lambda x: int(x, 0), c.crc_ccitt_tab))) 13 | const uint16_t TransferCRC::Table[256] = 14 | { 15 | 0x0000U, 0x1021U, 0x2042U, 0x3063U, 0x4084U, 0x50A5U, 0x60C6U, 0x70E7U, 16 | 0x8108U, 0x9129U, 0xA14AU, 0xB16BU, 0xC18CU, 0xD1ADU, 0xE1CEU, 0xF1EFU, 17 | 0x1231U, 0x0210U, 0x3273U, 0x2252U, 0x52B5U, 0x4294U, 0x72F7U, 0x62D6U, 18 | 0x9339U, 0x8318U, 0xB37BU, 0xA35AU, 0xD3BDU, 0xC39CU, 0xF3FFU, 0xE3DEU, 19 | 0x2462U, 0x3443U, 0x0420U, 0x1401U, 0x64E6U, 0x74C7U, 0x44A4U, 0x5485U, 20 | 0xA56AU, 0xB54BU, 0x8528U, 0x9509U, 0xE5EEU, 0xF5CFU, 0xC5ACU, 0xD58DU, 21 | 0x3653U, 0x2672U, 0x1611U, 0x0630U, 0x76D7U, 0x66F6U, 0x5695U, 0x46B4U, 22 | 0xB75BU, 0xA77AU, 0x9719U, 0x8738U, 0xF7DFU, 0xE7FEU, 0xD79DU, 0xC7BCU, 23 | 0x48C4U, 0x58E5U, 0x6886U, 0x78A7U, 0x0840U, 0x1861U, 0x2802U, 0x3823U, 24 | 0xC9CCU, 0xD9EDU, 0xE98EU, 0xF9AFU, 0x8948U, 0x9969U, 0xA90AU, 0xB92BU, 25 | 0x5AF5U, 0x4AD4U, 0x7AB7U, 0x6A96U, 0x1A71U, 0x0A50U, 0x3A33U, 0x2A12U, 26 | 0xDBFDU, 0xCBDCU, 0xFBBFU, 0xEB9EU, 0x9B79U, 0x8B58U, 0xBB3BU, 0xAB1AU, 27 | 0x6CA6U, 0x7C87U, 0x4CE4U, 0x5CC5U, 0x2C22U, 0x3C03U, 0x0C60U, 0x1C41U, 28 | 0xEDAEU, 0xFD8FU, 0xCDECU, 0xDDCDU, 0xAD2AU, 0xBD0BU, 0x8D68U, 0x9D49U, 29 | 0x7E97U, 0x6EB6U, 0x5ED5U, 0x4EF4U, 0x3E13U, 0x2E32U, 0x1E51U, 0x0E70U, 30 | 0xFF9FU, 0xEFBEU, 0xDFDDU, 0xCFFCU, 0xBF1BU, 0xAF3AU, 0x9F59U, 0x8F78U, 31 | 0x9188U, 0x81A9U, 0xB1CAU, 0xA1EBU, 0xD10CU, 0xC12DU, 0xF14EU, 0xE16FU, 32 | 0x1080U, 0x00A1U, 0x30C2U, 0x20E3U, 0x5004U, 0x4025U, 0x7046U, 0x6067U, 33 | 0x83B9U, 0x9398U, 0xA3FBU, 0xB3DAU, 0xC33DU, 0xD31CU, 0xE37FU, 0xF35EU, 34 | 0x02B1U, 0x1290U, 0x22F3U, 0x32D2U, 0x4235U, 0x5214U, 0x6277U, 0x7256U, 35 | 0xB5EAU, 0xA5CBU, 0x95A8U, 0x8589U, 0xF56EU, 0xE54FU, 0xD52CU, 0xC50DU, 36 | 0x34E2U, 0x24C3U, 0x14A0U, 0x0481U, 0x7466U, 0x6447U, 0x5424U, 0x4405U, 37 | 0xA7DBU, 0xB7FAU, 0x8799U, 0x97B8U, 0xE75FU, 0xF77EU, 0xC71DU, 0xD73CU, 38 | 0x26D3U, 0x36F2U, 0x0691U, 0x16B0U, 0x6657U, 0x7676U, 0x4615U, 0x5634U, 39 | 0xD94CU, 0xC96DU, 0xF90EU, 0xE92FU, 0x99C8U, 0x89E9U, 0xB98AU, 0xA9ABU, 40 | 0x5844U, 0x4865U, 0x7806U, 0x6827U, 0x18C0U, 0x08E1U, 0x3882U, 0x28A3U, 41 | 0xCB7DU, 0xDB5CU, 0xEB3FU, 0xFB1EU, 0x8BF9U, 0x9BD8U, 0xABBBU, 0xBB9AU, 42 | 0x4A75U, 0x5A54U, 0x6A37U, 0x7A16U, 0x0AF1U, 0x1AD0U, 0x2AB3U, 0x3A92U, 43 | 0xFD2EU, 0xED0FU, 0xDD6CU, 0xCD4DU, 0xBDAAU, 0xAD8BU, 0x9DE8U, 0x8DC9U, 44 | 0x7C26U, 0x6C07U, 0x5C64U, 0x4C45U, 0x3CA2U, 0x2C83U, 0x1CE0U, 0x0CC1U, 45 | 0xEF1FU, 0xFF3EU, 0xCF5DU, 0xDF7CU, 0xAF9BU, 0xBFBAU, 0x8FD9U, 0x9FF8U, 46 | 0x6E17U, 0x7E36U, 0x4E55U, 0x5E74U, 0x2E93U, 0x3EB2U, 0x0ED1U, 0x1EF0U 47 | }; 48 | 49 | #endif 50 | 51 | } 52 | -------------------------------------------------------------------------------- /libuavcan/src/transport/uc_outgoing_transfer_registry.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | /* 10 | * OutgoingTransferRegistryKey 11 | */ 12 | #if UAVCAN_TOSTRING 13 | std::string OutgoingTransferRegistryKey::toString() const 14 | { 15 | char buf[40]; 16 | (void)snprintf(buf, sizeof(buf), "dtid=%u tt=%u dnid=%u", 17 | int(data_type_id_.get()), int(transfer_type_), int(destination_node_id_.get())); 18 | return std::string(buf); 19 | } 20 | #endif 21 | 22 | /* 23 | * OutgoingTransferRegistry 24 | */ 25 | const MonotonicDuration OutgoingTransferRegistry::MinEntryLifetime = MonotonicDuration::fromMSec(2000); 26 | 27 | TransferID* OutgoingTransferRegistry::accessOrCreate(const OutgoingTransferRegistryKey& key, 28 | MonotonicTime new_deadline) 29 | { 30 | UAVCAN_ASSERT(!new_deadline.isZero()); 31 | Value* p = map_.access(key); 32 | if (p == NULL) 33 | { 34 | p = map_.insert(key, Value()); 35 | if (p == NULL) 36 | { 37 | return NULL; 38 | } 39 | UAVCAN_TRACE("OutgoingTransferRegistry", "Created %s", key.toString().c_str()); 40 | } 41 | p->deadline = new_deadline; 42 | return &p->tid; 43 | } 44 | 45 | bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const 46 | { 47 | return NULL != map_.find(ExistenceCheckingPredicate(dtid, tt)); 48 | } 49 | 50 | void OutgoingTransferRegistry::cleanup(MonotonicTime ts) 51 | { 52 | map_.removeAllWhere(DeadlineExpiredPredicate(ts)); 53 | } 54 | 55 | } 56 | -------------------------------------------------------------------------------- /libuavcan/src/transport/uc_transfer.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | namespace uavcan 10 | { 11 | /** 12 | * TransferPriority 13 | */ 14 | const uint8_t TransferPriority::BitLen; 15 | const uint8_t TransferPriority::NumericallyMax; 16 | const uint8_t TransferPriority::NumericallyMin; 17 | 18 | const TransferPriority TransferPriority::Default((1U << BitLen) / 2); 19 | const TransferPriority TransferPriority::MiddleLower((1U << BitLen) / 2 + (1U << BitLen) / 4); 20 | const TransferPriority TransferPriority::OneHigherThanLowest(NumericallyMax - 1); 21 | const TransferPriority TransferPriority::OneLowerThanHighest(NumericallyMin + 1); 22 | const TransferPriority TransferPriority::Lowest(NumericallyMax); 23 | 24 | /** 25 | * TransferID 26 | */ 27 | const uint8_t TransferID::BitLen; 28 | const uint8_t TransferID::Max; 29 | const uint8_t TransferID::Half; 30 | 31 | /** 32 | * NodeID 33 | */ 34 | const uint8_t NodeID::ValueBroadcast; 35 | const uint8_t NodeID::ValueInvalid; 36 | const uint8_t NodeID::BitLen; 37 | const uint8_t NodeID::Max; 38 | const uint8_t NodeID::MaxRecommendedForRegularNodes; 39 | const NodeID NodeID::Broadcast(ValueBroadcast); 40 | 41 | /** 42 | * TransferID 43 | */ 44 | int TransferID::computeForwardDistance(TransferID rhs) const 45 | { 46 | int d = int(rhs.get()) - int(get()); 47 | if (d < 0) 48 | { 49 | d += 1 << BitLen; 50 | } 51 | 52 | UAVCAN_ASSERT(((get() + d) & Max) == rhs.get()); 53 | return d; 54 | } 55 | 56 | } 57 | -------------------------------------------------------------------------------- /libuavcan/src/uc_data_type.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | namespace uavcan 11 | { 12 | /* 13 | * DataTypeID 14 | */ 15 | const uint16_t DataTypeID::MaxServiceDataTypeIDValue; 16 | const uint16_t DataTypeID::MaxMessageDataTypeIDValue; 17 | const uint16_t DataTypeID::MaxPossibleDataTypeIDValue; 18 | 19 | DataTypeID DataTypeID::getMaxValueForDataTypeKind(const DataTypeKind dtkind) 20 | { 21 | if (dtkind == DataTypeKindService) 22 | { 23 | return MaxServiceDataTypeIDValue; 24 | } 25 | else if (dtkind == DataTypeKindMessage) 26 | { 27 | return MaxMessageDataTypeIDValue; 28 | } 29 | else 30 | { 31 | UAVCAN_ASSERT(0); 32 | return DataTypeID(0); 33 | } 34 | } 35 | 36 | /* 37 | * DataTypeSignatureCRC 38 | */ 39 | DataTypeSignatureCRC DataTypeSignatureCRC::extend(uint64_t crc) 40 | { 41 | DataTypeSignatureCRC ret; 42 | ret.crc_ = crc ^ 0xFFFFFFFFFFFFFFFF; 43 | return ret; 44 | } 45 | 46 | void DataTypeSignatureCRC::add(uint8_t byte) 47 | { 48 | static const uint64_t Poly = 0x42F0E1EBA9EA3693; 49 | crc_ ^= uint64_t(byte) << 56; 50 | for (int i = 0; i < 8; i++) 51 | { 52 | crc_ = (crc_ & (uint64_t(1) << 63)) ? (crc_ << 1) ^ Poly : crc_ << 1; 53 | } 54 | } 55 | 56 | void DataTypeSignatureCRC::add(const uint8_t* bytes, unsigned len) 57 | { 58 | UAVCAN_ASSERT(bytes); 59 | while (len--) 60 | { 61 | add(*bytes++); 62 | } 63 | } 64 | 65 | /* 66 | * DataTypeSignature 67 | */ 68 | void DataTypeSignature::mixin64(uint64_t x) 69 | { 70 | DataTypeSignatureCRC crc = DataTypeSignatureCRC::extend(value_); 71 | for (int i = 0; i < 64; i += 8) // LSB first 72 | { 73 | crc.add((x >> i) & 0xFF); 74 | } 75 | value_ = crc.get(); 76 | } 77 | 78 | void DataTypeSignature::extend(DataTypeSignature dts) 79 | { 80 | const uint64_t y = value_; 81 | mixin64(dts.get()); 82 | mixin64(y); 83 | } 84 | 85 | TransferCRC DataTypeSignature::toTransferCRC() const 86 | { 87 | TransferCRC tcrc; 88 | for (int i = 0; i < 64; i += 8) // LSB first 89 | { 90 | tcrc.add((value_ >> i) & 0xFF); 91 | } 92 | return tcrc; 93 | } 94 | 95 | /* 96 | * DataTypeDescriptor 97 | */ 98 | const unsigned DataTypeDescriptor::MaxFullNameLen; 99 | 100 | bool DataTypeDescriptor::isValid() const 101 | { 102 | return id_.isValidForDataTypeKind(kind_) && 103 | (full_name_ != NULL) && 104 | (*full_name_ != '\0'); 105 | } 106 | 107 | bool DataTypeDescriptor::match(DataTypeKind kind, const char* name) const 108 | { 109 | return (kind_ == kind) && !std::strncmp(full_name_, name, MaxFullNameLen); 110 | } 111 | 112 | bool DataTypeDescriptor::match(DataTypeKind kind, DataTypeID id) const 113 | { 114 | return (kind_ == kind) && (id_ == id); 115 | } 116 | 117 | #if UAVCAN_TOSTRING 118 | std::string DataTypeDescriptor::toString() const 119 | { 120 | char kindch = '?'; 121 | switch (kind_) 122 | { 123 | case DataTypeKindMessage: 124 | { 125 | kindch = 'm'; 126 | break; 127 | } 128 | case DataTypeKindService: 129 | { 130 | kindch = 's'; 131 | break; 132 | } 133 | default: 134 | { 135 | UAVCAN_ASSERT(0); 136 | break; 137 | } 138 | } 139 | 140 | char buf[128]; 141 | (void)snprintf(buf, sizeof(buf), "%s:%u%c:%016llx", 142 | full_name_, static_cast(id_.get()), kindch, 143 | static_cast(signature_.get())); 144 | return std::string(buf); 145 | } 146 | #endif 147 | 148 | bool DataTypeDescriptor::operator==(const DataTypeDescriptor& rhs) const 149 | { 150 | return 151 | (kind_ == rhs.kind_) && 152 | (id_ == rhs.id_) && 153 | (signature_ == rhs.signature_) && 154 | !std::strncmp(full_name_, rhs.full_name_, MaxFullNameLen); 155 | } 156 | 157 | } 158 | -------------------------------------------------------------------------------- /libuavcan/src/uc_dynamic_memory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | 7 | namespace uavcan 8 | { 9 | /* 10 | * LimitedPoolAllocator 11 | */ 12 | void* LimitedPoolAllocator::allocate(std::size_t size) 13 | { 14 | if (used_blocks_ < max_blocks_) 15 | { 16 | used_blocks_++; 17 | return allocator_.allocate(size); 18 | } 19 | else 20 | { 21 | return NULL; 22 | } 23 | } 24 | 25 | void LimitedPoolAllocator::deallocate(const void* ptr) 26 | { 27 | allocator_.deallocate(ptr); 28 | 29 | UAVCAN_ASSERT(used_blocks_ > 0); 30 | if (used_blocks_ > 0) 31 | { 32 | used_blocks_--; 33 | } 34 | } 35 | 36 | uint16_t LimitedPoolAllocator::getBlockCapacity() const 37 | { 38 | return min(max_blocks_, allocator_.getBlockCapacity()); 39 | } 40 | 41 | } 42 | -------------------------------------------------------------------------------- /libuavcan/src/uc_error.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2014 Pavel Kirienko 3 | */ 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #ifndef UAVCAN_EXCEPTIONS 10 | # error UAVCAN_EXCEPTIONS 11 | #endif 12 | 13 | #if UAVCAN_EXCEPTIONS 14 | # include 15 | #endif 16 | 17 | namespace uavcan 18 | { 19 | 20 | void handleFatalError(const char* msg) 21 | { 22 | #if UAVCAN_EXCEPTIONS 23 | throw std::runtime_error(msg); 24 | #else 25 | (void)msg; 26 | UAVCAN_ASSERT(0); 27 | std::abort(); 28 | #endif 29 | } 30 | 31 | } 32 | -------------------------------------------------------------------------------- /openocd_jcesc_1_0.cfg: -------------------------------------------------------------------------------- 1 | source /usr/local/share/openocd/scripts/interface/stlink-v2.cfg 2 | 3 | reset_config none 4 | transport select hla_swd 5 | 6 | source /usr/local/share/openocd/scripts/target/stm32f3x.cfg 7 | 8 | -------------------------------------------------------------------------------- /openocd_miniquad_1_0.cfg: -------------------------------------------------------------------------------- 1 | source /usr/local/share/openocd/scripts/interface/stlink-v2.cfg 2 | 3 | reset_config none 4 | transport select hla_swd 5 | 6 | source /usr/local/share/openocd/scripts/target/stm32f3x.cfg 7 | -------------------------------------------------------------------------------- /openocd_px4esc_1_6.cfg: -------------------------------------------------------------------------------- 1 | source /usr/local/share/openocd/scripts/interface/stlink-v2.cfg 2 | 3 | reset_config none 4 | transport select hla_swd 5 | 6 | source /usr/local/share/openocd/scripts/target/stm32f4x.cfg 7 | 8 | -------------------------------------------------------------------------------- /openocd_s2740vc_1_0.cfg: -------------------------------------------------------------------------------- 1 | source /usr/local/share/openocd/scripts/interface/stlink-v2.cfg 2 | 3 | reset_config srst_only srst_nogate connect_assert_srst srst_open_drain 4 | transport select hla_swd 5 | 6 | source /usr/local/share/openocd/scripts/target/stm32f3x.cfg 7 | 8 | -------------------------------------------------------------------------------- /uavcan_ident/jcesc-v1-1.0.mk: -------------------------------------------------------------------------------- 1 | UAVCANBLID_HW_VERSION_MAJOR=1 2 | UAVCANBLID_HW_VERSION_MINOR=0 3 | UAVCANBLID_NAME=com.proficnc.jcesc-v1 4 | -------------------------------------------------------------------------------- /uavcan_ident/miniquad-v1-1.0.mk: -------------------------------------------------------------------------------- 1 | UAVCANBLID_HW_VERSION_MAJOR=1 2 | UAVCANBLID_HW_VERSION_MINOR=0 3 | UAVCANBLID_NAME=com.thiemar.miniquad-v1 4 | -------------------------------------------------------------------------------- /uavcan_ident/px4esc-v1-1.6.mk: -------------------------------------------------------------------------------- 1 | UAVCANBLID_HW_VERSION_MAJOR=1 2 | UAVCANBLID_HW_VERSION_MINOR=6 3 | UAVCANBLID_NAME=org.pixhawk.px4esc-v1 4 | -------------------------------------------------------------------------------- /uavcan_ident/s2740vc-v1-1.0.mk: -------------------------------------------------------------------------------- 1 | UAVCANBLID_HW_VERSION_MAJOR=1 2 | UAVCANBLID_HW_VERSION_MINOR=0 3 | UAVCANBLID_NAME=com.thiemar.s2740vc-v1 4 | --------------------------------------------------------------------------------