├── .gitignore ├── .gitmodules ├── .travis.yml ├── LICENSE ├── README.md ├── bootloader ├── Makefile ├── 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.h ├── docs └── reference_manual │ ├── .gitignore │ ├── ConfigParamIndex.sty │ ├── Sapog_v2_Reference_Manual.tex │ ├── bdmf_samples_per_comm_period_formula.nb │ ├── bemf_trapezoidal_vs_sinusoidal.pdf │ ├── commutation_basics.pdf │ ├── phase_voltage_sampling.pdf │ ├── phase_voltages_at_high_advance_angle.pdf │ ├── phase_voltages_at_high_load.pdf │ ├── phase_voltages_braking_high_advance_angle.pdf │ ├── plots.nb │ ├── pmsm_vs_bldc.nb │ ├── power_stage_schematic.inkscape.svg │ ├── power_stage_schematic.pdf │ ├── pwm_maximum_speed.pdf │ ├── rcpwm_setpoint_handling.pdf │ ├── rcpwm_signal_plot.pdf │ ├── sapog_reference_hardware.pdf │ ├── scoping │ ├── README.md │ ├── commutation_basics.csv │ ├── commutation_basics.inkscape.svg │ ├── disjoint_phase_voltage_plots.nb │ ├── phase_voltage_plots.nb │ ├── phase_voltage_sampling.csv │ ├── phase_voltage_sampling.inkscape.svg │ ├── phase_voltages_at_high_advance_angle.csv │ ├── phase_voltages_at_high_load.csv │ ├── phase_voltages_at_high_load.inkscape.svg │ ├── phase_voltages_braking_high_advance_angle.csv │ ├── phase_voltages_braking_high_advance_angle.inkscape.svg │ ├── spinup.csv.tar.gz │ └── spinup_phase_voltages.inkscape.svg │ ├── sine_torque_deviation.pdf │ ├── spinup_phase_voltages.pdf │ ├── spinup_voltage_ramp.pdf │ ├── timing_advance_interpolation_plot.pdf │ └── zubaxdoc.cls ├── firmware ├── Makefile ├── linker.ld └── src │ ├── board │ ├── board.cpp │ ├── board.hpp │ ├── led.cpp │ └── led.hpp │ ├── console.cpp │ ├── console.hpp │ ├── main.cpp │ ├── motor │ ├── motor.c │ ├── motor.h │ ├── realtime │ │ ├── adc.h │ │ ├── api.h │ │ ├── forced_rotation_detection.h │ │ ├── internal.h │ │ ├── irq.h │ │ ├── motor_adc.c │ │ ├── motor_debug_cli.c │ │ ├── motor_forced_rotation_detection.c │ │ ├── motor_pwm.c │ │ ├── motor_rtctl.c │ │ ├── motor_rtctl_test.c │ │ ├── motor_timer.c │ │ ├── pwm.h │ │ └── timer.h │ ├── rpmctl.c │ └── rpmctl.h │ ├── os_config │ ├── board.h │ ├── chconf.h │ ├── halconf.h │ └── mcuconf.h │ ├── pwm_input.cpp │ ├── pwm_input.hpp │ ├── temperature_sensor.cpp │ ├── temperature_sensor.hpp │ └── uavcan_node │ ├── bootloader_interface.cpp │ ├── bootloader_interface.hpp │ ├── esc_controller.cpp │ ├── esc_controller.hpp │ ├── indication_controller.cpp │ ├── indication_controller.hpp │ ├── uavcan_node.cpp │ └── uavcan_node.hpp ├── openocd.cfg └── tools ├── ardubenchmark ├── Makefile └── ardubenchmark.c ├── blackmagic.gdbinit ├── blackmagic_flash.sh └── drwatson ├── README.md ├── drwatson_sapog.py ├── run.sh └── setup.sh /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.elf 3 | *.bin 4 | *.hex 5 | lib*.so 6 | lib*.so.* 7 | *.a 8 | *.d 9 | .dep 10 | build 11 | .gdbinit 12 | *.swp 13 | *~ 14 | *.pyc 15 | 16 | # IDE 17 | .metadata/* 18 | .settings 19 | .project 20 | .cproject 21 | .idea 22 | .pydevproject 23 | 24 | # Generated 25 | dsdlc_generated 26 | *.log 27 | 28 | # Mac 29 | .DS_Store 30 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "firmware/zubax_chibios"] 2 | path = firmware/zubax_chibios 3 | url = https://github.com/Zubax/zubax_chibios 4 | [submodule "firmware/libuavcan"] 5 | path = firmware/libuavcan 6 | url = https://github.com/UAVCAN/libuavcan 7 | [submodule "docs/reference_manual/document_templates"] 8 | path = docs/reference_manual/document_templates 9 | url = https://github.com/Zubax/document_templates 10 | [submodule "tools/drwatson/drwatson"] 11 | path = tools/drwatson/drwatson 12 | url = https://github.com/Zubax/drwatson 13 | [submodule "tools/drwatson/pyuavcan"] 14 | path = tools/drwatson/pyuavcan 15 | url = https://github.com/UAVCAN/pyuavcan 16 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: python 2 | install: pip install flake8 3 | script: flake8 tools/ --count --select=E9,F63,F7,F82 --show-source --statistics --exclude=tools/drwatson/pyuavcan 4 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2013, Pavel Kirienko 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, 5 | are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, this 8 | list of conditions and the following disclaimer. 9 | 10 | * Redistributions in binary form must reproduce the above copyright notice, this 11 | list of conditions and the following disclaimer in the documentation and/or 12 | other materials provided with the distribution. 13 | 14 | * Neither the name of the {organization} nor the names of its 15 | contributors may be used to endorse or promote products derived from 16 | this software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 22 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 25 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | PX4 Sapog 2 | ========= 3 | 4 | [![Forum](https://img.shields.io/discourse/https/discuss.px4.io/users.svg)](https://discuss.px4.io/) 5 | [![Forum](https://img.shields.io/discourse/https/forum.uavcan.org/users.svg)](https://forum.uavcan.org) 6 | 7 | ## Documentation 8 | 9 | **[Sapog v2 Reference Manual](https://files.zubax.com/products/io.px4.sapog/Sapog_v2_Reference_Manual.pdf)** 10 | is the main piece of project documentation; make sure to read it. 11 | Additional documentation and related resources can be found at the 12 | **[Zubax Knowledge Base](https://kb.zubax.com/x/cYAh)**. 13 | 14 | ## Change Log 15 | 16 | ### v2.3 17 | 18 | * Configuration parameter `mot_i_shunt_mr` has been removed; now the firmware detects the shunt resistance 19 | based on the HW ID pin states. 20 | * Migrated from GCC 4.9 to GCC 7.2. 21 | 22 | ### v2.2 23 | 24 | * Added a low-pass filter on the estimated angular speed output. 25 | This change led to improved performance on motors with severe phase asummetry. 26 | * Improved RGB LED indication in the bootloader. 27 | 28 | ### v2.1 29 | 30 | * Fixed stability issues at extremely high RPM (>10k mechanical RPM for 14 pole motor). 31 | * Default PWM frequency set to 60 kHz. 32 | 33 | ### v2.0 34 | 35 | * Completely new, more robust spin up algorithm. Supports smooth start-up from standstill as well as picking up 36 | the rotation if the rotor is already spinning. 37 | * Significantly more reliable operation during rapid acceleration and deceleration, 38 | especially at high advance angles. 39 | * Wider PWM frequency range: 20...75 kHz. 40 | * Raised the maximum RPM limit; the new maximum for 14 pole motor exceeds 14000 mechanical RPM. 41 | 42 | ## Firmware 43 | 44 | If you're not running Linux or OSX natively, you can use 45 | [**Bistromathic** - a Linux virtual machine pre-configured for embedded development](https://kb.zubax.com/x/KIEh). 46 | 47 | ### Bootloader 48 | 49 | The bootloader allows to update the firmware via the standard UAVCAN firmware upgrade protocol, 50 | which is documented at [uavcan.org](http://uavcan.org/Specification/6._Application_level_functions/#firmware-update). 51 | No additional steps are needed to build the bootloader - the build system will build it automatically together with 52 | the firmware. The resulting `*.elf` file will be extended with the bootloader too, so it can be flashed directly into an 53 | factory fresh MCU. 54 | 55 | ### Build Instructions 56 | 57 | **Prebuilt binaries are available at .** 58 | 59 | Prerequisites: 60 | 61 | * GCC ARM 7.2 62 | * Python 3.2+ 63 | * Linux or OSX host computer (Windows is not supported) 64 | 65 | ```bash 66 | git submodule update --init --recursive 67 | cd firmware 68 | make RELEASE=1 # RELEASE is optional; omit to build the debug version 69 | ``` 70 | 71 | The build outputs will be stored into `build/`: 72 | 73 | * `*.application.bin` - built application binary, suitable for uploading via the bootloader; 74 | * `*.compound.bin` - application binary together with the bootloader, in one image; 75 | * `compound.elf` - application ELF together with the bootloader, in one file; this option is recommended for debugging. 76 | 77 | Execute `./blackmagic_flash.sh [portname]` from the `tools` directory to flash the firmware with a Black Magic Debug Probe. 78 | 79 | ### Development 80 | 81 | We recommend Eclipse for IDE, but any other IDE will work equally well. 82 | If you prefer Eclipse and need GUI debugging, avoid upgrading to any version newer than Luna, 83 | since in newer releases GUI GDB debugging of embedded targets is broken. 84 | Otherwise we recommend to use the latest Eclipse together with CLI GDB client. 85 | It's inconvenient, but unlike Eclipse it works reliably. 86 | 87 | When editing code, please follow the 88 | [PX4 coding conventions](https://github.com/PX4/Firmware/blob/master/CONTRIBUTING.md). 89 | 90 | ### Hardware Timer Usage 91 | 92 | * TIM1 - 3-phase FET bridge PWM 93 | * TIM2 - ADC synchronization, works in lockstep with TIM1 94 | * TIM3 - RGB LED PWM 95 | * TIM4 - Hard real time callout interface for motor control logic (preempts the kernel) 96 | * TIM5 - RC PWM input capture 97 | * TIM6 - High precision timestamping for motor control logic (sub-microsecond resolution, never overflows) 98 | * TIM7 - General purpose timestamping 99 | 100 | ## Hardware 101 | 102 | Reference hardware design is published under CC BY-SA 3.0 in the [PX4 Hardware repository](https://github.com/PX4/Hardware). 103 | 104 | Known commercially available compatible hardware designs are listed below. 105 | 106 | - [Zubax Orel 20](https://zubax.com/products/orel_20) 107 | - [Holybro Kotleta20](https://shop.holybro.com/kotleta20_p1156.html) 108 | -------------------------------------------------------------------------------- /bootloader/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 | 98 | void __attribute__((weak)) irq_initialize(void); 99 | int irq_unexpected_isr(int irq, void *context); 100 | 101 | #undef EXTERN 102 | #ifdef __cplusplus 103 | } 104 | #endif 105 | #endif 106 | 107 | #endif /* __INCLUDE_NUTTX_IRQ_H */ 108 | 109 | -------------------------------------------------------------------------------- /bootloader/arch/include/types.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/include/types.h 3 | * 4 | * Copyright (C) 2007-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 | /* This file should never be included directed but, rather, only indirectly 37 | * through sys/types.h 38 | */ 39 | 40 | #ifndef __ARCH_ARM_INCLUDE_TYPES_H 41 | #define __ARCH_ARM_INCLUDE_TYPES_H 42 | 43 | /**************************************************************************** 44 | * Included Files 45 | ****************************************************************************/ 46 | 47 | /**************************************************************************** 48 | * Pre-processor Definitions 49 | ****************************************************************************/ 50 | 51 | /**************************************************************************** 52 | * Type Declarations 53 | ****************************************************************************/ 54 | 55 | #ifndef __ASSEMBLY__ 56 | 57 | /* These are the sizes of the standard integer types. NOTE that these type 58 | * names have a leading underscore character. This file will be included 59 | * (indirectly) by include/stdint.h and typedef'ed to the final name without 60 | * the underscore character. This roundabout way of doings things allows 61 | * the stdint.h to be removed from the include/ directory in the event that 62 | * the user prefers to use the definitions provided by their toolchain header 63 | * files 64 | */ 65 | 66 | typedef signed char _int8_t; 67 | typedef unsigned char _uint8_t; 68 | 69 | typedef signed short _int16_t; 70 | typedef unsigned short _uint16_t; 71 | 72 | typedef signed int _int32_t; 73 | typedef unsigned int _uint32_t; 74 | 75 | typedef signed long long _int64_t; 76 | typedef unsigned long long _uint64_t; 77 | #define __INT64_DEFINED 78 | 79 | /* A pointer is 4 bytes */ 80 | 81 | typedef signed int _intptr_t; 82 | typedef unsigned int _uintptr_t; 83 | 84 | /* This is the size of the interrupt state save returned by irqsave(). For 85 | * ARM, a 32 register value is returned, for the thumb2, Cortex-M3, the 16-bit 86 | * primask register value is returned, 87 | */ 88 | 89 | #ifdef __thumb2__ 90 | #if defined(CONFIG_ARMV7M_USEBASEPRI) || defined(CONFIG_ARCH_ARMV6M) 91 | typedef unsigned char irqstate_t; 92 | #else 93 | typedef unsigned short irqstate_t; 94 | #endif 95 | #else /* __thumb2__ */ 96 | typedef unsigned int irqstate_t; 97 | #endif /* __thumb2__ */ 98 | 99 | #endif /* __ASSEMBLY__ */ 100 | 101 | /**************************************************************************** 102 | * Global Function Prototypes 103 | ****************************************************************************/ 104 | 105 | #endif /* __ARCH_ARM_INCLUDE_TYPES_H */ 106 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/arch/src/chip/stm32_otgfs.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/bootloader/arch/src/chip/stm32_otgfs.h -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/arch/src/chip/stm32_rtc.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/stm32/chip/stm32_rtc.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_RTC_H 37 | #define __ARCH_ARM_SRC_STM32_CHIP_STM32_RTC_H 38 | 39 | /************************************************************************************ 40 | * Pre-processor Definitions 41 | ************************************************************************************/ 42 | 43 | /* Register Offsets *****************************************************************/ 44 | 45 | #define STM32_RTC_CRH_OFFSET 0x0000 /* RTC control register High (16-bit) */ 46 | #define STM32_RTC_CRL_OFFSET 0x0004 /* RTC control register low (16-bit) */ 47 | #define STM32_RTC_PRLH_OFFSET 0x0008 /* RTC prescaler load register high (16-bit) */ 48 | #define STM32_RTC_PRLL_OFFSET 0x000c /* RTC prescaler load register low (16-bit) */ 49 | #define STM32_RTC_DIVH_OFFSET 0x0010 /* RTC prescaler divider register high (16-bit) */ 50 | #define STM32_RTC_DIVL_OFFSET 0x0014 /* RTC prescaler divider register low (16-bit) */ 51 | #define STM32_RTC_CNTH_OFFSET 0x0018 /* RTC counter register high (16-bit) */ 52 | #define STM32_RTC_CNTL_OFFSET 0x001c /* RTC counter register low (16-bit) */ 53 | #define STM32_RTC_ALRH_OFFSET 0x0020 /* RTC alarm register high (16-bit) */ 54 | #define STM32_RTC_ALRL_OFFSET 0x0024 /* RTC alarm register low (16-bit) */ 55 | 56 | /* Register Addresses ***************************************************************/ 57 | 58 | #define STM32_RTC_CRH (STM32_RTC_BASE+STM32_RTC_CRH_OFFSET) 59 | #define STM32_RTC_CRL (STM32_RTC_BASE+STM32_RTC_CRL_OFFSET) 60 | #define STM32_RTC_PRLH (STM32_RTC_BASE+STM32_RTC_PRLH_OFFSET) 61 | #define STM32_RTC_PRLL (STM32_RTC_BASE+STM32_RTC_PRLL_OFFSET) 62 | #define STM32_RTC_DIVH (STM32_RTC_BASE+STM32_RTC_DIVH_OFFSET) 63 | #define STM32_RTC_DIVL (STM32_RTC_BASE+STM32_RTC_DIVL_OFFSET) 64 | #define STM32_RTC_CNTH (STM32_RTC_BASE+STM32_RTC_CNTH_OFFSET) 65 | #define STM32_RTC_CNTL (STM32_RTC_BASE+STM32_RTC_CNTL_OFFSET) 66 | #define STM32_RTC_ALRH (STM32_RTC_BASE+STM32_RTC_ALRH_OFFSET) 67 | #define STM32_RTC_ALRL (STM32_RTC_BASE+STM32_RTC_ALRL_OFFSET) 68 | 69 | /* Register Bitfield Definitions ****************************************************/ 70 | 71 | /* RTC control register High (16-bit) */ 72 | 73 | #define RTC_CRH_SECIE (1 << 0) /* Bit 0 : Second Interrupt Enable */ 74 | #define RTC_CRH_ALRIE (1 << 1) /* Bit 1: Alarm Interrupt Enable */ 75 | #define RTC_CRH_OWIE (1 << 2) /* Bit 2: OverfloW Interrupt Enable */ 76 | 77 | /* RTC control register low (16-bit) */ 78 | 79 | #define RTC_CRL_SECF (1 << 0) /* Bit 0: Second Flag */ 80 | #define RTC_CRL_ALRF (1 << 1) /* Bit 1: Alarm Flag */ 81 | #define RTC_CRL_OWF (1 << 2) /* Bit 2: Overflow Flag */ 82 | #define RTC_CRL_RSF (1 << 3) /* Bit 3: Registers Synchronized Flag */ 83 | #define RTC_CRL_CNF (1 << 4) /* Bit 4: Configuration Flag */ 84 | #define RTC_CRL_RTOFF (1 << 5) /* Bit 5: RTC operation OFF */ 85 | 86 | /* RTC prescaler load register high (16-bit) */ 87 | 88 | #define RTC_PRLH_PRL_SHIFT (0) /* Bits 3-0: RTC Prescaler Reload Value High */ 89 | #define RTC_PRLH_PRL_MASK (0x0f << RTC_PRLH_PRL_SHIFT) 90 | 91 | /* RTC prescaler divider register high (16-bit) */ 92 | 93 | #define RTC_DIVH_RTC_DIV_SHIFT (0) /* Bits 3-0: RTC Clock Divider High */ 94 | #define RTC_DIVH_RTC_DIV_MASK (0x0f << RTC_DIVH_RTC_DIV_SHIFT) 95 | 96 | #endif /* __ARCH_ARM_SRC_STM32_CHIP_STM32_RTC_H */ 97 | -------------------------------------------------------------------------------- /bootloader/arch/src/irq_attach.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * sched/irq/irq_attach.c 3 | * 4 | * Copyright (C) 2007-2008, 2010, 2012 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 | * Pre-processor Definitions 45 | ****************************************************************************/ 46 | 47 | /**************************************************************************** 48 | * Private Type Declarations 49 | ****************************************************************************/ 50 | 51 | /**************************************************************************** 52 | * Global Variables 53 | ****************************************************************************/ 54 | 55 | /**************************************************************************** 56 | * Private Variables 57 | ****************************************************************************/ 58 | 59 | /**************************************************************************** 60 | * Private Functions 61 | ****************************************************************************/ 62 | 63 | /**************************************************************************** 64 | * Public Functions 65 | ****************************************************************************/ 66 | 67 | /**************************************************************************** 68 | * Name: irq_attach 69 | * 70 | * Description: 71 | * Configure the IRQ subsystem so that IRQ number 'irq' is dispatched to 72 | * 'isr' 73 | * 74 | ****************************************************************************/ 75 | 76 | int irq_attach(int irq, xcpt_t isr) 77 | { 78 | #if NR_IRQS > 0 79 | int ret = -1; 80 | 81 | if ((unsigned)irq < NR_IRQS) 82 | { 83 | irqstate_t state; 84 | 85 | /* If the new ISR is NULL, then the ISR is being detached. 86 | * In this case, disable the ISR and direct any interrupts 87 | * to the unexpected interrupt handler. 88 | */ 89 | 90 | state = irqsave(); 91 | if (isr == NULL) 92 | { 93 | /* Disable the interrupt if we can before detaching it. We might 94 | * not be able to do this if: (1) the device does not have a 95 | * centralized interrupt controller (so up_disable_irq() is not 96 | * supported). Or (2) if the device has different number for vector 97 | * numbers and IRQ numbers (in that case, we don't know the correct 98 | * IRQ number to use to disable the interrupt). In those cases, the 99 | * code will just need to be careful that it disables all interrupt 100 | * sources before detaching from the interrupt vector. 101 | */ 102 | 103 | #if !defined(CONFIG_ARCH_NOINTC) && !defined(CONFIG_ARCH_VECNOTIRQ) 104 | up_disable_irq(irq); 105 | #endif 106 | /* Detaching the ISR really means re-attaching it to the 107 | * unexpected exception handler. 108 | */ 109 | 110 | isr = irq_unexpected_isr; 111 | } 112 | 113 | /* Save the new ISR in the table. */ 114 | 115 | g_irqvector[irq] = isr; 116 | irqrestore(state); 117 | ret = 0; 118 | } 119 | 120 | return ret; 121 | #else 122 | return 0; 123 | #endif 124 | } 125 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/arch/src/psr.h: -------------------------------------------------------------------------------- 1 | /************************************************************************************ 2 | * arch/arm/src/armv7-m/psr.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_COMMON_ARMV7_M_PSR_H 37 | #define __ARCH_ARM_SRC_COMMON_ARMV7_M_PSR_H 38 | 39 | /************************************************************************************ 40 | * Included Files 41 | ************************************************************************************/ 42 | 43 | /************************************************************************************ 44 | * Pre-processor Definitions 45 | ************************************************************************************/ 46 | 47 | /* Application Program Status Register (APSR) */ 48 | 49 | #define ARMV7M_APSR_Q (1 << 27) /* Bit 27: Sticky saturation flag */ 50 | #define ARMV7M_APSR_V (1 << 28) /* Bit 28: Overflow flag */ 51 | #define ARMV7M_APSR_C (1 << 29) /* Bit 29: Carry/borrow flag */ 52 | #define ARMV7M_APSR_Z (1 << 30) /* Bit 30: Zero flag */ 53 | #define ARMV7M_APSR_N (1 << 31) /* Bit 31: Negative, less than flag */ 54 | 55 | /* Interrupt Program Status Register (IPSR) */ 56 | 57 | #define ARMV7M_IPSR_ISR_SHIFT 0 /* Bits 8-0: ISR number */ 58 | #define ARMV7M_IPSR_ISR_MASK (0x1ff << ARMV7M_IPSR_ISR_SHIFT) 59 | 60 | /* Execution PSR Register (EPSR) */ 61 | 62 | #define ARMV7M_EPSR_ICIIT1_SHIFT 10 /* Bits 15-10: Interrupt-Continuable-Instruction/If-Then bits */ 63 | #define ARMV7M_EPSR_ICIIT1_MASK (3 << ARMV7M_EPSR_ICIIT1_SHIFT) 64 | #define ARMV7M_EPSR_T (1 << 24) /* Bit 24: T-bit */ 65 | #define ARMV7M_EPSR_ICIIT2_SHIFT 25 /* Bits 26-25: Interrupt-Continuable-Instruction/If-Then bits */ 66 | #define ARMV7M_EPSR_ICIIT2_MASK (3 << ARMV7M_EPSR_ICIIT2_SHIFT) 67 | 68 | /* Save xPSR bits */ 69 | 70 | #define ARMV7M_XPSR_ISR_SHIFT ARMV7M_IPSR_ISR_SHIFT 71 | #define ARMV7M_XPSR_ISR_MASK ARMV7M_IPSR_ISR_MASK 72 | #define ARMV7M_XPSR_ICIIT1_SHIFT ARMV7M_EPSR_ICIIT1_SHIFT/ 73 | #define ARMV7M_XPSR_ICIIT1_MASK ARMV7M_EPSR_ICIIT1_MASK 74 | #define ARMV7M_XPSR_T ARMV7M_EPSR_T 75 | #define ARMV7M_XPSR_ICIIT2_SHIFT ARMV7M_EPSR_ICIIT2_SHIFT 76 | #define ARMV7M_XPSR_ICIIT2_MASK ARMV7M_EPSR_ICIIT2_MASK 77 | #define ARMV7M_XPSR_Q ARMV7M_APSR_Q 78 | #define ARMV7M_XPSR_V ARMV7M_APSR_V 79 | #define ARMV7M_XPSR_C ARMV7M_APSR_C 80 | #define ARMV7M_XPSR_Z ARMV7M_APSR_Z 81 | #define ARMV7M_XPSR_N ARMV7M_APSR_N 82 | 83 | /************************************************************************************ 84 | * Inline Functions 85 | ************************************************************************************/ 86 | 87 | #endif /* __ARCH_ARM_SRC_COMMON_ARMV7_M_PSR_H */ 88 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/arch/src/stm32f30xxx_rcc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/bootloader/arch/src/stm32f30xxx_rcc.c -------------------------------------------------------------------------------- /bootloader/arch/src/stm32f37xxx_rcc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/bootloader/arch/src/stm32f37xxx_rcc.c -------------------------------------------------------------------------------- /bootloader/arch/src/up_doirq.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * arch/arm/src/armv7-m/up_doirq.c 3 | * 4 | * Copyright (C) 2009, 2011, 2013-2015 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_arch.h" 44 | #include "up_internal.h" 45 | 46 | /**************************************************************************** 47 | * Pre-processor Definitions 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Public Data 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Private Data 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Private Functions 60 | ****************************************************************************/ 61 | 62 | void irq_dispatch(int irq, void *context); 63 | 64 | /**************************************************************************** 65 | * Public Functions 66 | ****************************************************************************/ 67 | 68 | uint32_t *up_doirq(int irq, uint32_t *regs) 69 | { 70 | #ifdef CONFIG_SUPPRESS_INTERRUPTS 71 | while (1); 72 | #else 73 | uint32_t *savestate; 74 | 75 | /* Nested interrupts are not supported in this implementation. If you want 76 | * to implement nested interrupts, you would have to (1) change the way that 77 | * current_regs is handled and (2) the design associated with 78 | * CONFIG_ARCH_INTERRUPTSTACK. The savestate variable will not work for 79 | * that purpose as implemented here because only the outermost nested 80 | * interrupt can result in a context switch (it can probably be deleted). 81 | */ 82 | 83 | /* Current regs non-zero indicates that we are processing an interrupt; 84 | * current_regs is also used to manage interrupt level context switches. 85 | */ 86 | 87 | savestate = (uint32_t*)current_regs; 88 | current_regs = regs; 89 | 90 | /* Acknowledge the interrupt */ 91 | 92 | up_ack_irq(irq); 93 | 94 | /* Deliver the IRQ */ 95 | 96 | irq_dispatch(irq, regs); 97 | 98 | /* If a context switch occurred while processing the interrupt then 99 | * current_regs may have change value. If we return any value different 100 | * from the input regs, then the lower level will know that a context 101 | * switch occurred during interrupt processing. 102 | */ 103 | 104 | regs = (uint32_t*)current_regs; 105 | 106 | /* Restore the previous value of current_regs. NULL would indicate that 107 | * we are no longer in an interrupt handler. It will be non-NULL if we 108 | * are returning from a nested interrupt. 109 | */ 110 | 111 | current_regs = savestate; 112 | #endif 113 | return regs; 114 | } 115 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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 | -------------------------------------------------------------------------------- /bootloader/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/bootloader/bootloader.ld: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * bootloader/bootloader.ld 3 | * 4 | * Copyright (C) 2015 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 | /* The STM32F302K8 has 64KiB of FLASH beginning at address 0x0800:0000 and 37 | * 16KiB of SRAM beginning at address 0x2000:0000. When booting from FLASH, 38 | * FLASH memory is aliased to address 0x0000:0000 where the code expects to 39 | * begin execution by jumping to the entry point in the 0x0800:0000 address 40 | * range. 41 | * The bootloader only uses the first 16KiB of flash. 42 | */ 43 | 44 | MEMORY 45 | { 46 | flash (rx) : ORIGIN = 0x08000000, LENGTH = 16128 /* Reserved 256 bytes for signature after the bootloader */ 47 | sram (rwx) : ORIGIN = 0x20000000, LENGTH = 16K 48 | } 49 | 50 | OUTPUT_ARCH(arm) 51 | 52 | ENTRY(__start) /* treat __start as the anchor for dead code stripping */ 53 | EXTERN(_vectors) /* force the vectors to be included in the output */ 54 | /* 55 | * Ensure that abort() is present in the final object. The exception handling 56 | * code pulled in by libgcc.a requires it (and that code cannot be easily avoided). 57 | */ 58 | EXTERN(abort) 59 | SECTIONS 60 | { 61 | .text : { 62 | _stext = ABSOLUTE(.); 63 | *(.vectors) 64 | *(.text .text.*) 65 | *(.fixup) 66 | *(.gnu.warning) 67 | *(.rodata .rodata.*) 68 | *(.gnu.linkonce.t.*) 69 | *(.got) 70 | *(.gcc_except_table) 71 | *(.gnu.linkonce.r.*) 72 | _etext = ABSOLUTE(.); 73 | } > flash 74 | 75 | /* 76 | * Init functions (static constructors and the like) 77 | */ 78 | .init_section : { 79 | _sinit = ABSOLUTE(.); 80 | KEEP(*(.init_array .init_array.*)) 81 | _einit = ABSOLUTE(.); 82 | } > flash 83 | 84 | .ARM.extab : { 85 | *(.ARM.extab*) 86 | } > flash 87 | 88 | __exidx_start = ABSOLUTE(.); 89 | .ARM.exidx : { 90 | *(.ARM.exidx*) 91 | } > flash 92 | __exidx_end = ABSOLUTE(.); 93 | 94 | _eronly = ABSOLUTE(.); 95 | 96 | .data : { 97 | _sdata = ABSOLUTE(.); 98 | *(.data .data.*) 99 | *(.gnu.linkonce.d.*) 100 | CONSTRUCTORS 101 | _edata = ABSOLUTE(.); 102 | } > sram AT > flash 103 | 104 | .bss : { 105 | _sbss = ABSOLUTE(.); 106 | *(.bss .bss.*) 107 | *(.gnu.linkonce.b.*) 108 | *(COMMON) 109 | _ebss = ABSOLUTE(.); 110 | } > sram 111 | 112 | /* Stabs debugging sections. */ 113 | .stab 0 : { *(.stab) } 114 | .stabstr 0 : { *(.stabstr) } 115 | .stab.excl 0 : { *(.stab.excl) } 116 | .stab.exclstr 0 : { *(.stab.exclstr) } 117 | .stab.index 0 : { *(.stab.index) } 118 | .stab.indexstr 0 : { *(.stab.indexstr) } 119 | .comment 0 : { *(.comment) } 120 | .debug_abbrev 0 : { *(.debug_abbrev) } 121 | .debug_info 0 : { *(.debug_info) } 122 | .debug_line 0 : { *(.debug_line) } 123 | .debug_pubnames 0 : { *(.debug_pubnames) } 124 | .debug_aranges 0 : { *(.debug_aranges) } 125 | } 126 | -------------------------------------------------------------------------------- /bootloader/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/bootloader/include/crc.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 | #include 44 | #include 45 | 46 | /**************************************************************************** 47 | * Pre-processor Definitions 48 | ****************************************************************************/ 49 | #define CRC16_INITIAL 0xFFFFu 50 | #define CRC16_OUTPUT_XOR 0x0000u 51 | #define CRC64_INITIAL 0xFFFFFFFFFFFFFFFFull 52 | #define CRC64_OUTPUT_XOR 0xFFFFFFFFFFFFFFFFull 53 | 54 | /**************************************************************************** 55 | * Public Type Definitions 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Global Variables 60 | ****************************************************************************/ 61 | 62 | /**************************************************************************** 63 | * Public Function Prototypes 64 | ****************************************************************************/ 65 | /**************************************************************************** 66 | * Name: crc16_add 67 | * 68 | * Description: 69 | * Use to calculates a CRC-16-CCITT using the polynomial of 70 | * 0x1021 by adding a value successive values. 71 | * 72 | * Input Parameters: 73 | * crc - The running total of the crc 16 74 | * value - The value to add 75 | * 76 | * Returned Value: 77 | * The current crc16 with the value processed. 78 | * 79 | ****************************************************************************/ 80 | 81 | uint16_t crc16_add(uint16_t crc, uint8_t value); 82 | 83 | /**************************************************************************** 84 | * Name: crc16_signature 85 | * 86 | * Description: 87 | * Calculates a CRC-16-CCITT using the crc16_add 88 | * function 89 | * 90 | * Input Parameters: 91 | * initial - The Initial value to uses as the crc's starting point 92 | * length - The number of bytes to add to the crc 93 | * bytes - A pointer to any array of length bytes 94 | * 95 | * Returned Value: 96 | * The crc16 of the array of bytes 97 | * 98 | ****************************************************************************/ 99 | 100 | uint16_t crc16_signature(uint16_t initial, size_t length, 101 | const uint8_t *bytes); 102 | 103 | /**************************************************************************** 104 | * Name: crc64_add_word 105 | * 106 | * Description: 107 | * Calculates a CRC-64-WE using the polynomial of 0x42F0E1EBA9EA3693 108 | * See http://reveng.sourceforge.net/crc-catalogue/17plus.htm#crc.cat-bits.64 109 | * Check: 0x62EC59E3F1A4F00A 110 | * 111 | * Input Parameters: 112 | * crc - The running total of the crc 64 113 | * value - The value to add 114 | * 115 | * Returned Value: 116 | * The current crc64 with the value processed. 117 | * 118 | ****************************************************************************/ 119 | 120 | uint64_t crc64_add_word(uint64_t crc, uint32_t value); 121 | -------------------------------------------------------------------------------- /bootloader/bootloader/include/flash.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 | #include 44 | #include 45 | #include 46 | 47 | /**************************************************************************** 48 | * Pre-processor Definitions 49 | ****************************************************************************/ 50 | 51 | /**************************************************************************** 52 | * Public Type Definitions 53 | ****************************************************************************/ 54 | 55 | typedef enum { 56 | FLASH_OK = 0, 57 | FLASH_ERROR, 58 | FLASH_ERASE_ERROR, 59 | FLASH_ERASE_VERIFY_ERROR, 60 | FLASH_ERROR_SUICIDE, 61 | FLASH_ERROR_AFU, 62 | 63 | } flash_error_t; 64 | 65 | /**************************************************************************** 66 | * Global Variables 67 | ****************************************************************************/ 68 | 69 | /**************************************************************************** 70 | * Public Function Prototypes 71 | ****************************************************************************/ 72 | /**************************************************************************** 73 | * Name: bl_flash_erase 74 | * 75 | * Description: 76 | * This function erases the flash starting at address and ending at 77 | * address + nbytes. 78 | * 79 | * Input Parameters: 80 | * address - A word-aligned address within the first page of flash to erase 81 | * nbytes - The number of bytes to erase, rounding up to the next page. 82 | * 83 | * 84 | * 85 | * Returned value: 86 | * On success FLASH_OK On Error one of the flash_error_t 87 | * 88 | ****************************************************************************/ 89 | 90 | flash_error_t bl_flash_erase(size_t address, size_t nbytes); 91 | 92 | /**************************************************************************** 93 | * Name: bl_flash_write 94 | * 95 | * Description: 96 | * This function writes the flash starting at the given address 97 | * 98 | * Input Parameters: 99 | * flash_address - The address of the flash to write 100 | * must be word aligned 101 | * data - A pointer to a buffer count bytes to be written 102 | * to the flash. 103 | * count - Number of bytes to write 104 | * 105 | * Returned value: 106 | * On success FLASH_OK On Error one of the flash_error_t 107 | * 108 | ****************************************************************************/ 109 | 110 | flash_error_t bl_flash_write(uint32_t flash_address, uint8_t *data, ssize_t count); 111 | -------------------------------------------------------------------------------- /bootloader/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/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/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 | -------------------------------------------------------------------------------- /bootloader/bootloader/src/random.c: -------------------------------------------------------------------------------- 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 | /**************************************************************************** 38 | * Included Files 39 | ****************************************************************************/ 40 | 41 | #include 42 | #include "random.h" 43 | 44 | /**************************************************************************** 45 | * Pre-processor Definitions 46 | ****************************************************************************/ 47 | #define RAND_MAX_32 ((1U << 31) - 1) 48 | #define RAND_MAX ((1U << 15) - 1) 49 | 50 | /**************************************************************************** 51 | * Private Types 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Private Function Prototypes 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Private Data 60 | ****************************************************************************/ 61 | static uint32_t rseed; 62 | 63 | 64 | /**************************************************************************** 65 | * Public Data 66 | ****************************************************************************/ 67 | 68 | /**************************************************************************** 69 | * Private Functions 70 | ****************************************************************************/ 71 | 72 | /**************************************************************************** 73 | * Public Functions 74 | ****************************************************************************/ 75 | /**************************************************************************** 76 | * Name: util_srand 77 | * 78 | * Description: 79 | * This function seeds the random number generator 80 | * 81 | * 82 | * Input Parameters: 83 | * seed - The seed 84 | * 85 | * Returned value: 86 | * None 87 | * 88 | ****************************************************************************/ 89 | void util_srand(uint16_t seed) 90 | { 91 | rseed = seed; 92 | } 93 | 94 | 95 | /**************************************************************************** 96 | * Name: util_random 97 | * 98 | * Description: 99 | * This function returns a random number between min and max 100 | * 101 | * 102 | * Input Parameters: 103 | * min - The minimum value the return value can be. 104 | * max - The maximum value the return value can be. 105 | * 106 | * Returned value: 107 | * A random number 108 | * 109 | ****************************************************************************/ 110 | 111 | uint16_t util_random(uint16_t min, uint16_t max) 112 | { 113 | uint16_t rand = (rseed = (rseed * 214013 + 2531011) & RAND_MAX_32) >> 16; 114 | return rand % (max - min) + min; 115 | } 116 | 117 | -------------------------------------------------------------------------------- /docs/reference_manual/.gitignore: -------------------------------------------------------------------------------- 1 | ## Core latex/pdflatex auxiliary files: 2 | *.aux 3 | *.lof 4 | *.log 5 | *.lot 6 | *.fls 7 | *.out 8 | *.toc 9 | *.fmt 10 | *.fot 11 | *.cb 12 | *.cb2 13 | 14 | ## Intermediate documents: 15 | *.dvi 16 | *-converted-to.* 17 | # these rules might exclude image files for figures etc. 18 | # *.ps 19 | # *.eps 20 | # *.pdf 21 | 22 | ## Bibliography auxiliary files (bibtex/biblatex/biber): 23 | *.bbl 24 | *.bcf 25 | *.blg 26 | *-blx.aux 27 | *-blx.bib 28 | *.brf 29 | *.run.xml 30 | 31 | ## Build tool auxiliary files: 32 | *.fdb_latexmk 33 | *.synctex 34 | *.synctex.gz 35 | *.synctex.gz(busy) 36 | *.pdfsync 37 | 38 | ## Auxiliary and intermediate files from other packages: 39 | # algorithms 40 | *.alg 41 | *.loa 42 | 43 | # achemso 44 | acs-*.bib 45 | 46 | # amsthm 47 | *.thm 48 | 49 | # beamer 50 | *.nav 51 | *.snm 52 | *.vrb 53 | 54 | # cprotect 55 | *.cpt 56 | 57 | # fixme 58 | *.lox 59 | 60 | #(r)(e)ledmac/(r)(e)ledpar 61 | *.end 62 | *.?end 63 | *.[1-9] 64 | *.[1-9][0-9] 65 | *.[1-9][0-9][0-9] 66 | *.[1-9]R 67 | *.[1-9][0-9]R 68 | *.[1-9][0-9][0-9]R 69 | *.eledsec[1-9] 70 | *.eledsec[1-9]R 71 | *.eledsec[1-9][0-9] 72 | *.eledsec[1-9][0-9]R 73 | *.eledsec[1-9][0-9][0-9] 74 | *.eledsec[1-9][0-9][0-9]R 75 | 76 | # glossaries 77 | *.acn 78 | *.acr 79 | *.glg 80 | *.glo 81 | *.gls 82 | *.glsdefs 83 | 84 | # gnuplottex 85 | *-gnuplottex-* 86 | 87 | # hyperref 88 | *.brf 89 | 90 | # knitr 91 | *-concordance.tex 92 | # TODO Comment the next line if you want to keep your tikz graphics files 93 | *.tikz 94 | *-tikzDictionary 95 | 96 | # listings 97 | *.lol 98 | 99 | # makeidx 100 | *.idx 101 | *.ilg 102 | *.ind 103 | *.ist 104 | 105 | # minitoc 106 | *.maf 107 | *.mlf 108 | *.mlt 109 | *.mtc 110 | *.mtc[0-9] 111 | *.mtc[1-9][0-9] 112 | 113 | # minted 114 | _minted* 115 | *.pyg 116 | 117 | # morewrites 118 | *.mw 119 | 120 | # mylatexformat 121 | *.fmt 122 | 123 | # nomencl 124 | *.nlo 125 | 126 | # sagetex 127 | *.sagetex.sage 128 | *.sagetex.py 129 | *.sagetex.scmd 130 | 131 | # sympy 132 | *.sout 133 | *.sympy 134 | sympy-plots-for-*.tex/ 135 | 136 | # pdfcomment 137 | *.upa 138 | *.upb 139 | 140 | # pythontex 141 | *.pytxcode 142 | pythontex-files-*/ 143 | 144 | # thmtools 145 | *.loe 146 | 147 | # TikZ & PGF 148 | *.dpth 149 | *.md5 150 | *.auxlock 151 | 152 | # todonotes 153 | *.tdo 154 | 155 | # xindy 156 | *.xdy 157 | 158 | # xypic precompiled matrices 159 | *.xyc 160 | 161 | # endfloat 162 | *.ttt 163 | *.fff 164 | 165 | # Latexian 166 | TSWLatexianTemp* 167 | 168 | ## Editors: 169 | # WinEdt 170 | *.bak 171 | *.sav 172 | 173 | # Texpad 174 | .texpadtmp 175 | 176 | # Kile 177 | *.backup 178 | 179 | # KBibTeX 180 | *~[0-9]* 181 | 182 | # Outputs 183 | Sapog_v2_Reference_Manual.pdf 184 | -------------------------------------------------------------------------------- /docs/reference_manual/ConfigParamIndex.sty: -------------------------------------------------------------------------------- 1 | document_templates/latex_utilities/ConfigParamIndex.sty -------------------------------------------------------------------------------- /docs/reference_manual/bdmf_samples_per_comm_period_formula.nb: -------------------------------------------------------------------------------- 1 | (* Content-type: application/vnd.wolfram.mathematica *) 2 | 3 | (*** Wolfram Notebook File ***) 4 | (* http://www.wolfram.com/nb *) 5 | 6 | (* CreatedBy='Mathematica 11.0' *) 7 | 8 | (*CacheID: 234*) 9 | (* Internal cache information: 10 | NotebookFileLineBreakTest 11 | NotebookFileLineBreakTest 12 | NotebookDataPosition[ 158, 7] 13 | NotebookDataLength[ 3304, 115] 14 | NotebookOptionsPosition[ 2851, 94] 15 | NotebookOutlinePosition[ 3188, 109] 16 | CellTagsIndexPosition[ 3145, 106] 17 | WindowFrame->Normal*) 18 | 19 | (* Beginning of Notebook Content *) 20 | Notebook[{ 21 | Cell[BoxData[ 22 | RowBox[{ 23 | RowBox[{"(*", " ", 24 | RowBox[{ 25 | "These", " ", "equations", " ", "reconstruct", " ", "the", " ", "logic", 26 | " ", "implemented", " ", "in", " ", "the", " ", 27 | RowBox[{"code", ".", "\[IndentingNewLine]", "Refer"}], " ", "to", " ", 28 | "the", " ", "sources", " ", "for", " ", "the", " ", "background", " ", 29 | RowBox[{"info", "."}]}], " ", "*)"}], "\[IndentingNewLine]", 30 | RowBox[{ 31 | RowBox[{ 32 | RowBox[{"denom", "=", 33 | RowBox[{"BWD", "+", 34 | FractionBox[ 35 | RowBox[{"BWD", " ", "\[Alpha]"}], "15"]}]}], ";"}], 36 | "\[IndentingNewLine]", 37 | RowBox[{ 38 | RowBox[{ 39 | SubscriptBox["T", "pwm"], "=", 40 | FractionBox["1", 41 | SubscriptBox["F", "pwm"]]}], ";"}]}]}]], "Input", 42 | CellChangeTimes->{{3.704190010205987*^9, 3.704190101005756*^9}, { 43 | 3.704190311391583*^9, 3.704190340768074*^9}, {3.704190522337776*^9, 44 | 3.7041905307375727`*^9}, {3.70419110239116*^9, 3.704191104090575*^9}}], 45 | 46 | Cell[CellGroupData[{ 47 | 48 | Cell[BoxData[ 49 | RowBox[{ 50 | SubscriptBox["n", "samples"], "=", 51 | RowBox[{ 52 | RowBox[{ 53 | RowBox[{ 54 | RowBox[{"(", 55 | FractionBox[ 56 | SubscriptBox["T", "comm"], 57 | SubscriptBox["T", "pwm"]], ")"}], "/", "denom"}], "+", "2"}], "//", 58 | "FullSimplify"}]}]], "Input", 59 | CellChangeTimes->{{3.704190108648011*^9, 3.704190193991633*^9}, { 60 | 3.704190255631549*^9, 3.704190261935751*^9}}], 61 | 62 | Cell[BoxData[ 63 | RowBox[{"2", "+", 64 | FractionBox[ 65 | RowBox[{ 66 | SubscriptBox["F", "pwm"], " ", 67 | SubscriptBox["T", "comm"]}], 68 | RowBox[{"BWD", "+", 69 | FractionBox[ 70 | RowBox[{"BWD", " ", "\[Alpha]"}], "15"]}]]}]], "Output", 71 | CellChangeTimes->{{3.7041901733801527`*^9, 3.7041901943645983`*^9}, { 72 | 3.704190263337057*^9, 3.7041902666092*^9}, 3.704190534293233*^9, 73 | 3.704191105677307*^9}] 74 | }, Open ]], 75 | 76 | Cell[CellGroupData[{ 77 | 78 | Cell[BoxData[ 79 | RowBox[{ 80 | RowBox[{"2", "+", 81 | FractionBox[ 82 | RowBox[{"60000", " ", "200", " ", 83 | SuperscriptBox["10", 84 | RowBox[{"-", "6"}]]}], 85 | RowBox[{"4", "+", 86 | FractionBox[ 87 | RowBox[{"29", " ", "4"}], "15"]}]]}], "//", "N"}]], "Input", 88 | CellChangeTimes->{{3.7041908018267603`*^9, 3.7041909032054253`*^9}}], 89 | 90 | Cell[BoxData["3.022727272727273`"], "Output", 91 | CellChangeTimes->{{3.7041908672160397`*^9, 3.7041909036083097`*^9}, 92 | 3.7041911064767647`*^9}] 93 | }, Open ]] 94 | }, 95 | WindowSize->{808, 911}, 96 | WindowMargins->{{Automatic, 0}, {4, Automatic}}, 97 | FrontEndVersion->"11.0 for Linux x86 (64-bit) (September 21, 2016)", 98 | StyleDefinitions->"Default.nb" 99 | ] 100 | (* End of Notebook Content *) 101 | 102 | (* Internal cache information *) 103 | (*CellTagsOutline 104 | CellTagsIndex->{} 105 | *) 106 | (*CellTagsIndex 107 | CellTagsIndex->{} 108 | *) 109 | (*NotebookFileOutline 110 | Notebook[{ 111 | Cell[558, 20, 939, 23, 142, "Input"], 112 | Cell[CellGroupData[{ 113 | Cell[1522, 47, 394, 12, 59, "Input"], 114 | Cell[1919, 61, 399, 11, 58, "Output"] 115 | }, Open ]], 116 | Cell[CellGroupData[{ 117 | Cell[2355, 77, 334, 10, 66, "Input"], 118 | Cell[2692, 89, 143, 2, 32, "Output"] 119 | }, Open ]] 120 | } 121 | ] 122 | *) 123 | 124 | -------------------------------------------------------------------------------- /docs/reference_manual/bemf_trapezoidal_vs_sinusoidal.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/bemf_trapezoidal_vs_sinusoidal.pdf -------------------------------------------------------------------------------- /docs/reference_manual/commutation_basics.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/commutation_basics.pdf -------------------------------------------------------------------------------- /docs/reference_manual/phase_voltage_sampling.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/phase_voltage_sampling.pdf -------------------------------------------------------------------------------- /docs/reference_manual/phase_voltages_at_high_advance_angle.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/phase_voltages_at_high_advance_angle.pdf -------------------------------------------------------------------------------- /docs/reference_manual/phase_voltages_at_high_load.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/phase_voltages_at_high_load.pdf -------------------------------------------------------------------------------- /docs/reference_manual/phase_voltages_braking_high_advance_angle.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/phase_voltages_braking_high_advance_angle.pdf -------------------------------------------------------------------------------- /docs/reference_manual/power_stage_schematic.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/power_stage_schematic.pdf -------------------------------------------------------------------------------- /docs/reference_manual/pwm_maximum_speed.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/pwm_maximum_speed.pdf -------------------------------------------------------------------------------- /docs/reference_manual/rcpwm_setpoint_handling.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/rcpwm_setpoint_handling.pdf -------------------------------------------------------------------------------- /docs/reference_manual/rcpwm_signal_plot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/rcpwm_signal_plot.pdf -------------------------------------------------------------------------------- /docs/reference_manual/sapog_reference_hardware.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/sapog_reference_hardware.pdf -------------------------------------------------------------------------------- /docs/reference_manual/scoping/README.md: -------------------------------------------------------------------------------- 1 | Annotated oscillograms 2 | ====================== 3 | 4 | How to make annotated oscillograms: 5 | 6 | 1. Scope the necessary signals using Saleae Logic or any other oscilloscope capable of exporting data in CSV. 7 | Export the data as CSV. 8 | 2. Using Wolfram Mathematica and the included notebooks, plot the data from the CSV file and adjust it as necessary. 9 | 3. Export the plot from Mathematica and annotate it with Inkscape if necessary. 10 | **Refer to the instructions and recommendations provided with the LaTeX template**, 11 | they will help you to avoid pitfalls in this process. 12 | 4. Include the final image into the document. 13 | -------------------------------------------------------------------------------- /docs/reference_manual/scoping/spinup.csv.tar.gz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/scoping/spinup.csv.tar.gz -------------------------------------------------------------------------------- /docs/reference_manual/sine_torque_deviation.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/sine_torque_deviation.pdf -------------------------------------------------------------------------------- /docs/reference_manual/spinup_phase_voltages.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/spinup_phase_voltages.pdf -------------------------------------------------------------------------------- /docs/reference_manual/spinup_voltage_ramp.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/spinup_voltage_ramp.pdf -------------------------------------------------------------------------------- /docs/reference_manual/timing_advance_interpolation_plot.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/sapog/601f4580b71c3c4da65cc52237e62a163d6a6a16/docs/reference_manual/timing_advance_interpolation_plot.pdf -------------------------------------------------------------------------------- /docs/reference_manual/zubaxdoc.cls: -------------------------------------------------------------------------------- 1 | document_templates/documentation_template_latex/zubaxdoc.cls -------------------------------------------------------------------------------- /firmware/linker.ld: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2015 Zubax Robotics 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU General Public License as published by 6 | * the Free Software Foundation, either version 3 of the License, or 7 | * (at your option) any later version. 8 | * 9 | * This program is distributed in the hope that it will be useful, 10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | * GNU General Public License for more details. 13 | * 14 | * You should have received a copy of the GNU General Public License 15 | * along with this program. If not, see . 16 | * 17 | * Author: Pavel Kirienko 18 | */ 19 | 20 | /* 21 | * Based on a ChibiOS demo app. 22 | */ 23 | 24 | __main_stack_size__ = 0x0400; 25 | __process_stack_size__ = 0x0400; 26 | 27 | MEMORY 28 | { 29 | flash : org = 0x08004000, len = 239k /* 16K for bootloader, 1K for configs */ 30 | ram : org = 0x20000000, len = 64k 31 | } 32 | 33 | 34 | __ram_start__ = ORIGIN(ram); 35 | __ram_size__ = LENGTH(ram); 36 | __ram_end__ = __ram_start__ + __ram_size__; 37 | 38 | ENTRY(Reset_Handler) 39 | 40 | PROVIDE(DeviceSignatureStorage = ORIGIN(flash) - 256); 41 | 42 | SECTIONS 43 | { 44 | . = 0; 45 | _text = .; 46 | 47 | startup : ALIGN(16) SUBALIGN(16) 48 | { 49 | KEEP(*(.vectors)) 50 | . = ALIGN(8); 51 | KEEP(*(.app_descriptor)); 52 | . = ALIGN(8); 53 | } > flash 54 | 55 | constructors : ALIGN(4) SUBALIGN(4) 56 | { 57 | PROVIDE(__init_array_start = .); 58 | KEEP(*(SORT(.init_array.*))) 59 | KEEP(*(.init_array)) 60 | PROVIDE(__init_array_end = .); 61 | } > flash 62 | 63 | destructors : ALIGN(4) SUBALIGN(4) 64 | { 65 | PROVIDE(__fini_array_start = .); 66 | KEEP(*(.fini_array)) 67 | KEEP(*(SORT(.fini_array.*))) 68 | PROVIDE(__fini_array_end = .); 69 | } > flash 70 | 71 | /* 72 | * This padding section must be inserted before every section with greater alignment. 73 | */ 74 | .padding1 : 75 | { 76 | /* 77 | * Filling is important: if it is not specified, gaps between aligned sections will be filled with zero when 78 | * exporting binaries, and will be skipped (i.e. set to 0xFF) when flashing .elf directly during debugging. 79 | * The discrepancy causes the firmware CRC to mismatch, preventing the bootloader from running it. 80 | * Assigning a well-defined fill byte eliminates the discrepancy (binary files will also contain 0xFF), 81 | * allowing to flash the firmware directly with an SWD debugger during development. 82 | * See http://www.math.utah.edu/docs/info/ld_3.html 83 | */ 84 | FILL(0xFF); 85 | . += 1; /* This increment prevents moving location counter backwards if no alignment is needed */ 86 | . = ALIGN(16) - 1; 87 | BYTE(0xFF) /* Without this, the linker will ignore aligmnent */ 88 | } > flash 89 | 90 | .text : ALIGN(16) SUBALIGN(16) 91 | { 92 | *(.text) 93 | *(.text.*) 94 | *(.rodata) 95 | *(.rodata.*) 96 | *(.glue_7t) 97 | *(.glue_7) 98 | *(.gcc*) 99 | } > flash 100 | 101 | .ARM.extab : 102 | { 103 | *(.ARM.extab* .gnu.linkonce.armextab.*) 104 | } > flash 105 | 106 | .ARM.exidx : 107 | { 108 | PROVIDE(__exidx_start = .); 109 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 110 | PROVIDE(__exidx_end = .); 111 | } > flash 112 | 113 | .eh_frame_hdr : 114 | { 115 | *(.eh_frame_hdr) 116 | } > flash 117 | 118 | .eh_frame : ONLY_IF_RO 119 | { 120 | *(.eh_frame) 121 | } > flash 122 | 123 | .textalign : ONLY_IF_RO 124 | { 125 | . = ALIGN(8); 126 | } > flash 127 | 128 | . = ALIGN(4); 129 | _etext = .; 130 | _textdata_start = _etext; 131 | 132 | .mstack : 133 | { 134 | . = ALIGN(8); 135 | __main_stack_base__ = .; 136 | . += __main_stack_size__; 137 | . = ALIGN(8); 138 | __main_stack_end__ = .; 139 | } > ram 140 | 141 | .pstack : 142 | { 143 | __process_stack_base__ = .; 144 | __main_thread_stack_base__ = .; 145 | . += __process_stack_size__; 146 | . = ALIGN(8); 147 | __process_stack_end__ = .; 148 | __main_thread_stack_end__ = .; 149 | } > ram 150 | 151 | .data : ALIGN(4) 152 | { 153 | . = ALIGN(4); 154 | PROVIDE(_data_start = .); 155 | *(.data) 156 | *(.data.*) 157 | *(.ramtext) 158 | . = ALIGN(4); 159 | PROVIDE(_data_end = .); 160 | } > ram AT > flash 161 | 162 | .bss : ALIGN(4) 163 | { 164 | . = ALIGN(4); 165 | PROVIDE(_bss_start = .); 166 | *(.bss) 167 | *(.bss.*) 168 | *(COMMON) 169 | . = ALIGN(4); 170 | PROVIDE(_bss_end = .); 171 | PROVIDE(end = .); 172 | } > ram 173 | 174 | .ram (NOLOAD) : ALIGN(4) 175 | { 176 | . = ALIGN(4); 177 | *(.ram) 178 | *(.ram.*) 179 | . = ALIGN(4); 180 | __ram_free__ = .; 181 | } > ram 182 | } 183 | 184 | /* Heap default boundaries, it is defaulted to be the non-used part of ram region.*/ 185 | __heap_base__ = __ram_free__; 186 | __heap_end__ = __ram_end__; 187 | 188 | -------------------------------------------------------------------------------- /firmware/src/board/board.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace board 43 | { 44 | 45 | os::watchdog::Timer init(unsigned watchdog_timeout_ms); 46 | 47 | /** 48 | * Performs an I2C transaction. 49 | * This function is thread safe. 50 | */ 51 | int i2c_exchange(std::uint8_t address, 52 | const void* tx_data, const std::uint16_t tx_size, 53 | void* rx_data, const std::uint16_t rx_size); 54 | 55 | /** 56 | * Safer wrapper over @ref i2c_exchange(). 57 | */ 58 | template 59 | inline int i2c_exchange(std::uint8_t address, 60 | const std::array& tx, 61 | std::array& rx) 62 | { 63 | return i2c_exchange(address, tx.data(), TxSize, rx.data(), RxSize); 64 | } 65 | 66 | __attribute__((noreturn)) 67 | void die(int error); 68 | 69 | void reboot(); 70 | 71 | typedef std::array UniqueID; 72 | UniqueID read_unique_id(); 73 | 74 | struct HardwareVersion 75 | { 76 | std::uint8_t major; 77 | std::uint8_t minor; 78 | }; 79 | 80 | HardwareVersion detect_hardware_version(); 81 | 82 | float get_current_shunt_resistance(); 83 | 84 | typedef std::array DeviceSignature; 85 | bool try_read_device_signature(DeviceSignature& out_sign); 86 | bool try_write_device_signature(const DeviceSignature& sign); 87 | 88 | } 89 | -------------------------------------------------------------------------------- /firmware/src/board/led.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | namespace board 41 | { 42 | /** 43 | * Basic color codes 44 | */ 45 | enum class LEDColor : unsigned 46 | { 47 | OFF = 0x000000, 48 | WHITE = 0xFFFFFF, 49 | 50 | // Basic colors 51 | RED = 0xFF0000, 52 | YELLOW = 0xFFFF00, 53 | GREEN = 0x00FF00, 54 | CYAN = 0x00FFFF, 55 | BLUE = 0x0000FF, 56 | PURPLE = 0xFF00FF, 57 | 58 | // Shades 59 | PALE_WHITE = 0x0F0F0F, 60 | DARK_RED = 0x0F0000, 61 | DARK_GREEN = 0x000F00, 62 | DARK_BLUE = 0x00000F 63 | }; 64 | 65 | /** 66 | * This function can be called from any context. 67 | * It sets LED to a specified state overriding all existing layers. 68 | * Accepts @ref Color. 69 | */ 70 | void led_emergency_override(LEDColor color); 71 | 72 | /** 73 | * This class allows to control the same LED from many sources in a stacked manner. 74 | * The instance of this class initialized last would be able to control the LED state, while 75 | * other instances (initialized earlier) would be shadowed until the top one is removed. 76 | */ 77 | class LEDOverlay 78 | { 79 | static constexpr int MAX_LAYERS = 4; 80 | 81 | static LEDOverlay* layers[MAX_LAYERS]; 82 | static chibios_rt::Mutex mutex; 83 | 84 | std::uint32_t color = 0; 85 | 86 | LEDOverlay& operator=(const LEDOverlay&) = delete; 87 | LEDOverlay(const LEDOverlay&) = delete; 88 | 89 | public: 90 | LEDOverlay() { } 91 | ~LEDOverlay() { unset(); } 92 | 93 | /** 94 | * Accepts standard RGB hex, e.g. 0xFFFFFF for white. 95 | * This function is thread-safe. 96 | */ 97 | void set_hex_rgb(std::uint32_t hex_rgb); 98 | 99 | /** 100 | * Accepts @ref Color. 101 | * This function is thread-safe. 102 | */ 103 | void set(LEDColor new_color) { set_hex_rgb(unsigned(new_color)); } 104 | 105 | /** 106 | * Accepts @ref Color. 107 | * Blinks the specified color. 108 | */ 109 | void blink(LEDColor new_color) { set_hex_rgb((color > 0) ? 0 : unsigned(new_color)); } 110 | 111 | /** 112 | * Returns the current color code. 113 | */ 114 | std::uint32_t get_hex_rgb() const { return color; } 115 | 116 | /** 117 | * Makes the layer inactive. 118 | * Next lower-priority layer will become active instead. 119 | * This function is thread-safe. 120 | */ 121 | void unset(); 122 | }; 123 | 124 | } 125 | -------------------------------------------------------------------------------- /firmware/src/console.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | // TODO: rewrite in C++ 36 | 37 | #pragma once 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | void console_init(void); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /firmware/src/motor/motor.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | 44 | enum motor_control_mode 45 | { 46 | MOTOR_CONTROL_MODE_OPENLOOP, 47 | MOTOR_CONTROL_MODE_RPM 48 | }; 49 | 50 | enum motor_limit_mask 51 | { 52 | MOTOR_LIMIT_RPM = 1, 53 | MOTOR_LIMIT_CURRENT = 2, 54 | MOTOR_LIMIT_ACCEL = 4 55 | }; 56 | 57 | enum motor_forced_rotation_direction 58 | { 59 | MOTOR_FORCED_ROTATION_NONE, 60 | MOTOR_FORCED_ROTATION_FORWARD, 61 | MOTOR_FORCED_ROTATION_REVERSE, 62 | }; 63 | 64 | /** 65 | * @param current_shunt_resistance resistance of the DC current shunt [ohm] 66 | * 67 | * @return negative on error, zero otherwise 68 | */ 69 | int motor_init(float current_shunt_resistance); 70 | 71 | /** 72 | * Sets the duty cycle. Control mode will be OPENLOOP. 73 | * TTL is the amount of time to keep this setpoint before stopping the motor if no new setpoints were set. 74 | * @param [in] dc Duty cycle [0.0; 1.0] 75 | * @param [in] ttl_ms TTL in milliseconds 76 | */ 77 | void motor_set_duty_cycle(float dc, int ttl_ms); 78 | 79 | /** 80 | * Sets the RPM setpoint. Control mode will be RPM. 81 | * TTL is the amount of time to keep this setpoint before stopping the motor if no new setpoints were set. 82 | * @param [in] rpm RPM setpoint 83 | * @param [in] ttl_ms TTL in milliseconds 84 | */ 85 | void motor_set_rpm(unsigned rpm, int ttl_ms); 86 | 87 | /** 88 | * Returns current duty cycle. 89 | */ 90 | float motor_get_duty_cycle(void); 91 | 92 | /** 93 | * Returns current RPM. 94 | */ 95 | unsigned motor_get_rpm(void); 96 | 97 | void motor_stop(void); 98 | 99 | enum motor_control_mode motor_get_control_mode(void); 100 | 101 | /** 102 | * Returns the motor state. 103 | * @return True if the motor is running; false if starting or not running. 104 | */ 105 | bool motor_is_running(void); 106 | 107 | /** 108 | * Returns the motor state. 109 | * @return True if the motor is idle; false if starting or running. 110 | */ 111 | bool motor_is_idle(void); 112 | 113 | /** 114 | * Returns true if the motor controller has given up trying to start. 115 | * @return True if the motor controller is locked. 116 | */ 117 | bool motor_is_blocked(void); 118 | 119 | /** 120 | * Returns the bitmask of currently active limits. 121 | */ 122 | int motor_get_limit_mask(void); 123 | 124 | /** 125 | * Returns filtered input voltage and current. 126 | * @param [out] out_voltage Volts 127 | * @param [out] out_current Amperes 128 | */ 129 | void motor_get_input_voltage_current(float* out_voltage, float* out_current); 130 | 131 | /** 132 | * Simple wrappers; refer to RTCTL API docs to learn more 133 | * @{ 134 | */ 135 | void motor_confirm_initialization(void); 136 | uint64_t motor_get_zc_failures_since_start(void); 137 | enum motor_forced_rotation_direction motor_get_forced_rotation_direction(void); 138 | int motor_test_hardware(void); 139 | int motor_test_motor(void); 140 | void motor_beep(int frequency, int duration_msec); 141 | void motor_print_debug_info(void); 142 | void motor_emergency(void); 143 | void motor_execute_cli_command(int argc, const char* argv[]); 144 | /** 145 | * @} 146 | */ 147 | 148 | #ifdef __cplusplus 149 | } 150 | #endif 151 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/adc.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko (pavel.kirienko@gmail.com) 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | #define MOTOR_ADC_RESOLUTION 12 44 | 45 | extern const int MOTOR_ADC_SYNC_ADVANCE_NANOSEC; 46 | extern const int MOTOR_ADC_SAMPLE_WINDOW_NANOSEC; 47 | extern const int MOTOR_ADC_MIN_BLANKING_TIME_NANOSEC; 48 | 49 | 50 | struct motor_adc_sample 51 | { 52 | uint64_t timestamp; 53 | int phase_values[3]; 54 | int input_voltage; 55 | int input_current; 56 | }; 57 | 58 | 59 | int motor_adc_init(float current_shunt_resistance); 60 | 61 | void motor_adc_enable_from_isr(void); 62 | void motor_adc_disable_from_isr(void); 63 | 64 | struct motor_adc_sample motor_adc_get_last_sample(void); 65 | 66 | float motor_adc_convert_input_voltage(int raw); 67 | float motor_adc_convert_input_current(int raw); 68 | 69 | /** 70 | * No OS API can be used from this callback! 71 | */ 72 | extern void motor_adc_sample_callback(const struct motor_adc_sample* sample); 73 | 74 | #ifdef __cplusplus 75 | } 76 | #endif 77 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/forced_rotation_detection.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include "api.h" 38 | #include "pwm.h" 39 | #include "adc.h" 40 | 41 | #ifdef __cplusplus 42 | extern "C" { 43 | #endif 44 | 45 | void motor_forced_rotation_detector_init(void); 46 | 47 | void motor_forced_rotation_detector_reset(void); 48 | 49 | void motor_forced_rotation_detector_update_from_adc_callback( 50 | const struct motor_pwm_commutation_step comm_table[MOTOR_NUM_COMMUTATION_STEPS], 51 | const struct motor_adc_sample* adc_sample); 52 | 53 | enum motor_rtctl_forced_rotation motor_forced_rotation_detector_get_state(void); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/internal.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko (pavel.kirienko@gmail.com) 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | #ifdef __cplusplus 41 | extern "C" { 42 | #endif 43 | 44 | #define MOTOR_NUM_PHASES 3 45 | 46 | #define MOTOR_NUM_COMMUTATION_STEPS 6 47 | 48 | #ifndef STRINGIZE 49 | # define STRINGIZE2(x) #x 50 | # define STRINGIZE(x) STRINGIZE2(x) 51 | #endif 52 | 53 | #ifndef ASSERT_ALWAYS 54 | # define ASSERT_ALWAYS(x) \ 55 | do { \ 56 | if ((x) == 0) { \ 57 | chSysHalt(__FILE__ ":" STRINGIZE(__LINE__) ":" STRINGIZE(x)); \ 58 | } \ 59 | } while (0) 60 | #endif 61 | 62 | /** 63 | * Faster alternatives for GPIO API that can be used from IRQ handlers. 64 | */ 65 | #if DEBUG_BUILD 66 | # define TESTPAD_SET(port, pin) (port)->BSRR = 1 << (pin) 67 | # define TESTPAD_CLEAR(port, pin) (port)->BRR = 1 << (pin) 68 | #else 69 | # define TESTPAD_SET(port, pin) 70 | # define TESTPAD_CLEAR(port, pin) 71 | #endif 72 | 73 | /** 74 | * Common priority for all hard real time IRQs. 75 | * Shall be set to maximum, which is zero. 76 | */ 77 | #define MOTOR_IRQ_PRIORITY_MASK 0 78 | 79 | #ifdef __cplusplus 80 | } 81 | #endif 82 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/irq.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | typedef uint32_t irqstate_t; 42 | 43 | __attribute__((optimize(3), always_inline)) 44 | static inline irqstate_t irq_primask_save(void) 45 | { 46 | const irqstate_t primask = __get_PRIMASK(); 47 | __set_PRIMASK(1); 48 | return primask; 49 | } 50 | 51 | __attribute__((optimize(3), always_inline)) 52 | static inline void irq_primask_restore(irqstate_t state) 53 | { 54 | assert(state == 1 || state == 0); 55 | __set_PRIMASK(state); 56 | } 57 | 58 | __attribute__((optimize(3), always_inline)) 59 | static inline void irq_primask_disable(void) 60 | { 61 | assert(__get_PRIMASK() == 0); // Make sure PRIMASK is not set 62 | __set_PRIMASK(1); 63 | } 64 | 65 | __attribute__((optimize(3), always_inline)) 66 | static inline void irq_primask_enable(void) { 67 | __set_PRIMASK(0); 68 | } 69 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/motor_debug_cli.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #include "api.h" 36 | #include "adc.h" 37 | #include "pwm.h" 38 | #include "irq.h" 39 | #include 40 | #include 41 | #include 42 | 43 | static enum motor_pwm_phase_manip arg_to_pwm_manip(const char* arg) 44 | { 45 | if (arg[0] == '+') { 46 | return MOTOR_PWM_MANIP_HIGH; 47 | } else if (arg[0] == '-') { 48 | return MOTOR_PWM_MANIP_LOW; 49 | } else if (arg[0] == '/') { 50 | return MOTOR_PWM_MANIP_HALF; 51 | } else { 52 | return MOTOR_PWM_MANIP_FLOATING; 53 | } 54 | } 55 | 56 | void motor_rtctl_execute_cli_command(int argc, const char* argv[]) 57 | { 58 | if (argc < 0 || argv == NULL) { 59 | printf("Invalid args\n"); 60 | return; 61 | } 62 | 63 | const struct motor_adc_sample adc_sample = motor_adc_get_last_sample(); 64 | 65 | printf("ADC raw phases: %i %i %i\n", 66 | adc_sample.phase_values[0], adc_sample.phase_values[1], adc_sample.phase_values[2]); 67 | 68 | printf("ADC raw vtg/cur: V=%i I=%i\n", adc_sample.input_voltage, adc_sample.input_current); 69 | 70 | if ((argc > 0) && !strcmp("step", argv[0])) { // step 71 | // Step manually with the configured duty cycle 72 | if (argc != 4) { 73 | printf("Usage: step \n"); 74 | return; 75 | } 76 | 77 | const float dc = atof(argv[1]); 78 | if ((dc < -1.0F) || (dc > 1.0F)) { 79 | puts("ERROR: Invalid duty cycle"); 80 | return; 81 | } 82 | 83 | struct motor_pwm_commutation_step step; 84 | memset(&step, 0, sizeof(step)); 85 | step.positive = atoi(argv[2]); 86 | step.negative = atoi(argv[3]); 87 | 88 | if ((step.positive < 0) || (step.positive > 2) || 89 | (step.negative < 0) || (step.negative > 2)) { 90 | puts("ERROR: Invalid phase index"); 91 | return; 92 | } 93 | 94 | if (step.positive == step.negative) { 95 | puts("ERROR: Phase polarity collision: pos == neg"); 96 | return; 97 | } 98 | 99 | // We have verified above that pos != neg and that both pos and neg are in {0, 1, 2}. 100 | for (int i = 0; i < 3; i++) { 101 | if ((i != step.positive) && (i != step.negative)) { 102 | step.floating = i; 103 | break; 104 | } 105 | } 106 | 107 | const int pwm_val = motor_pwm_compute_pwm_val(dc); 108 | 109 | printf("PWM val: %d, pos: %d, neg: %d, flt: %d\n", 110 | pwm_val, step.positive, step.negative, step.floating); 111 | 112 | motor_pwm_set_freewheeling(); // Pre-reset is required 113 | 114 | irq_primask_disable(); 115 | motor_pwm_set_step_from_isr(&step, pwm_val); 116 | irq_primask_enable(); 117 | 118 | } else if ((argc >= 1) && (argc <= 3)) { 119 | const enum motor_pwm_phase_manip manip_cmd[MOTOR_NUM_PHASES] = { 120 | arg_to_pwm_manip(argv[0]), 121 | (argc > 1) ? arg_to_pwm_manip(argv[1]) : MOTOR_PWM_MANIP_FLOATING, 122 | (argc > 2) ? arg_to_pwm_manip(argv[2]) : MOTOR_PWM_MANIP_FLOATING 123 | }; 124 | printf("Manip %i %i %i\n", (int)manip_cmd[0], (int)manip_cmd[1], (int)manip_cmd[2]); 125 | motor_pwm_manip(manip_cmd); 126 | 127 | } else { 128 | motor_pwm_set_freewheeling(); 129 | printf("Freewheeling\n"); 130 | } 131 | } 132 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/pwm.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko (pavel.kirienko@gmail.com) 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | #include "internal.h" 40 | 41 | #ifdef __cplusplus 42 | extern "C" { 43 | #endif 44 | 45 | #define MOTOR_ADC1_2_TRIGGER (ADC_CR2_EXTSEL_1 | ADC_CR2_EXTSEL_0) 46 | 47 | struct motor_pwm_commutation_step 48 | { 49 | int_fast8_t positive; 50 | int_fast8_t negative; 51 | int_fast8_t floating; 52 | }; 53 | 54 | enum motor_pwm_phase_manip 55 | { 56 | MOTOR_PWM_MANIP_LOW, 57 | MOTOR_PWM_MANIP_HIGH, 58 | MOTOR_PWM_MANIP_FLOATING, 59 | MOTOR_PWM_MANIP_HALF, 60 | MOTOR_PWM_MANIP_END_ 61 | }; 62 | 63 | /** 64 | * Sanity constraints. 65 | * The maximum PWM frequency is limited by the performance of the BEMF signal conditioning circuits. 66 | */ 67 | #define MOTOR_PWM_MIN_FREQUENCY 20000 68 | #define MOTOR_PWM_MAX_FREQUENCY 75000 69 | 70 | /** 71 | * Initialize the PWM hardware. 72 | * @return 0 on success, anything else if the requested frequency is invalid 73 | */ 74 | int motor_pwm_init(void); 75 | 76 | /** 77 | * ADC converstions are triggered by the PWM hardware, so this function is here 78 | */ 79 | uint32_t motor_adc_sampling_period_hnsec(void); 80 | 81 | /** 82 | * Direct phase control - for self-testing 83 | */ 84 | void motor_pwm_manip(const enum motor_pwm_phase_manip command[MOTOR_NUM_PHASES]); 85 | 86 | void motor_pwm_set_freewheeling(void); 87 | 88 | void motor_pwm_emergency(void); 89 | 90 | /** 91 | * Duty cycle in [-1; 1] 92 | */ 93 | int motor_pwm_compute_pwm_val(float duty_cycle); 94 | 95 | void motor_pwm_set_step_from_isr(const struct motor_pwm_commutation_step* step, int pwm_val); 96 | 97 | /** 98 | * Should be called from high priority threads 99 | */ 100 | void motor_pwm_beep(int frequency, int duration_msec); 101 | 102 | #ifdef __cplusplus 103 | } 104 | #endif 105 | -------------------------------------------------------------------------------- /firmware/src/motor/realtime/timer.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko (pavel.kirienko@gmail.com) 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | #define HNSEC_PER_USEC 10 44 | #define HNSEC_PER_MSEC 10000 45 | #define HNSEC_PER_SEC 10000000 46 | #define HNSEC_PER_MINUTE (HNSEC_PER_SEC * 60) 47 | #define NSEC_PER_HNSEC 100 48 | 49 | void motor_timer_init(void); 50 | 51 | /** 52 | * Returns the current timestamp in hectonanoseconds (10^-7). 53 | * Can be called from any context (IRQ safe, thread safe). 54 | */ 55 | uint64_t motor_timer_hnsec(void); 56 | 57 | /** 58 | * There is no limit on the maximum interval. 59 | * The driver may automatically split a very long interval into several timer IRQs. 60 | */ 61 | void motor_timer_set_relative(int64_t delay_hnsec); 62 | 63 | /** 64 | * Returns the time delta between the requested deadline and the current time. 65 | * Positive value indicates that the deadline is in the future; negative indicates that it is in the past. 66 | */ 67 | int64_t motor_timer_set_absolute(uint64_t timestamp_hnsec); 68 | 69 | void motor_timer_cancel(void); 70 | 71 | /** 72 | * No OS API can be used from this callback! 73 | */ 74 | extern void motor_timer_callback(uint64_t timestamp_hnsec); 75 | 76 | /** 77 | * Busy loop delay 78 | */ 79 | void motor_timer_hndelay(int hnsecs); 80 | 81 | #ifdef __cplusplus 82 | } 83 | #endif 84 | -------------------------------------------------------------------------------- /firmware/src/motor/rpmctl.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #include "rpmctl.h" 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | static struct state 42 | { 43 | float integrated; 44 | float prev_error; 45 | } _state; 46 | 47 | static struct params 48 | { 49 | float p; 50 | float d; 51 | float i; 52 | } _params; 53 | 54 | 55 | CONFIG_PARAM_FLOAT("rpmctl_p", 0.0001, 0.0, 1.0) 56 | CONFIG_PARAM_FLOAT("rpmctl_d", 0.0, 0.0, 1.0) 57 | CONFIG_PARAM_FLOAT("rpmctl_i", 0.001, 0.0, 10.0) 58 | 59 | 60 | int rpmctl_init(void) 61 | { 62 | _params.p = configGet("rpmctl_p"); 63 | _params.d = configGet("rpmctl_d"); 64 | _params.i = configGet("rpmctl_i"); 65 | return 0; 66 | } 67 | 68 | void rpmctl_reset(void) 69 | { 70 | _state.integrated = 0.0; 71 | _state.prev_error = nan(""); 72 | } 73 | 74 | float rpmctl_update(const struct rpmctl_input* input) 75 | { 76 | const float error = input->sp - input->pv; 77 | assert(isfinite(error)); 78 | 79 | if (!isfinite(_state.prev_error)) { 80 | _state.prev_error = error; 81 | } 82 | 83 | const float p = error * _params.p; 84 | const float i = _state.integrated + error * input->dt * _params.i; 85 | const float d = ((error - _state.prev_error) / input->dt) * _params.d; 86 | 87 | _state.prev_error = error; 88 | 89 | float output = p + i + d; 90 | if (output > 1.0) { 91 | output = 1.0; 92 | } else if (output < -1.0) { 93 | output = -1.0; 94 | } else if (input->limit_mask == 0) { 95 | _state.integrated = i; 96 | } 97 | return output; 98 | } 99 | -------------------------------------------------------------------------------- /firmware/src/motor/rpmctl.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | struct rpmctl_input 42 | { 43 | int limit_mask; 44 | float dt; 45 | float pv; 46 | float sp; 47 | }; 48 | 49 | int rpmctl_init(void); 50 | 51 | void rpmctl_reset(void); 52 | 53 | float rpmctl_update(const struct rpmctl_input* input); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /firmware/src/os_config/board.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #define STM32_HSECLK 24000000 38 | 39 | #define STM32F10X_CL 40 | #define STM32F105xC 41 | 42 | /* 43 | * GPIO 44 | */ 45 | // Misc 46 | #define GPIO_PORT_SERIAL_RX GPIOB 47 | #define GPIO_PIN_SERIAL_RX 7 48 | 49 | // I2C 50 | #define GPIO_PORT_I2C_SCL GPIOB 51 | #define GPIO_PIN_I2C_SCL 8 52 | 53 | #define GPIO_PORT_I2C_SDA GPIOB 54 | #define GPIO_PIN_I2C_SDA 9 55 | 56 | // Testpoints 57 | #define GPIO_PORT_TEST_A GPIOC 58 | #define GPIO_PIN_TEST_A 11 59 | 60 | #define GPIO_PORT_TEST_ZC GPIOC 61 | #define GPIO_PIN_TEST_ZC 12 62 | 63 | /* 64 | * I/O ports initial setup, this configuration is established soon after reset 65 | * in the initialization code. 66 | * 67 | * The digits have the following meaning: 68 | * 0 - Analog input. 69 | * 1 - Push Pull output 10MHz. 70 | * 2 - Push Pull output 2MHz. 71 | * 3 - Push Pull output 50MHz. 72 | * 4 - Digital input. 73 | * 5 - Open Drain output 10MHz. 74 | * 6 - Open Drain output 2MHz. 75 | * 7 - Open Drain output 50MHz. 76 | * 8 - Digital input with PullUp or PullDown resistor depending on ODR. 77 | * 9 - Alternate Push Pull output 10MHz. 78 | * A - Alternate Push Pull output 2MHz. 79 | * B - Alternate Push Pull output 50MHz. 80 | * C - Reserved. 81 | * D - Alternate Open Drain output 10MHz. 82 | * E - Alternate Open Drain output 2MHz. 83 | * F - Alternate Open Drain output 50MHz. 84 | * Please refer to the STM32 Reference Manual for details. 85 | */ 86 | 87 | #define VAL_GPIOACRL 0xb8000008 // 7..0 88 | #define VAL_GPIOACRH 0x888b8bbb // 15..8 89 | #define VAL_GPIOAODR ((1 << 11)) 90 | 91 | #define VAL_GPIOBCRL 0x8b8888bb 92 | #define VAL_GPIOBCRH 0x38b88866 93 | #define VAL_GPIOBODR ((1 << GPIO_PIN_I2C_SCL) | (1 << GPIO_PIN_I2C_SDA) | (1 << 12)) 94 | 95 | #define VAL_GPIOCCRL 0xbb884444 96 | #define VAL_GPIOCCRH 0x8881188b 97 | #define VAL_GPIOCODR 0x00000000 98 | 99 | #define VAL_GPIODCRL 0x88888888 100 | #define VAL_GPIODCRH 0x88888888 101 | #define VAL_GPIODODR 0x00000000 102 | 103 | #define VAL_GPIOECRL 0x88888888 104 | #define VAL_GPIOECRH 0x88888888 105 | #define VAL_GPIOEODR 0x00000000 106 | 107 | #if !defined(_FROM_ASM_) 108 | #ifdef __cplusplus 109 | extern "C" { 110 | #endif 111 | void boardInit(void); 112 | #ifdef __cplusplus 113 | } 114 | #endif 115 | #endif /* _FROM_ASM_ */ 116 | -------------------------------------------------------------------------------- /firmware/src/os_config/chconf.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | /* 38 | * ChibiOS supports tickless mode (and it's enabled by default), but it requires a dedicated hardware timer. 39 | * We don't have any free hardware timers, so we use classic ticked mode. 40 | */ 41 | #define CH_CFG_ST_TIMEDELTA 0 42 | #define CH_CFG_ST_FREQUENCY 1000 43 | 44 | #define PORT_IDLE_THREAD_STACK_SIZE 64 45 | #define PORT_INT_REQUIRED_STACK 512 46 | 47 | #include 48 | -------------------------------------------------------------------------------- /firmware/src/os_config/halconf.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include "mcuconf.h" 38 | 39 | #define HAL_USE_TM TRUE 40 | #define HAL_USE_PAL TRUE 41 | #define HAL_USE_ADC FALSE 42 | #define HAL_USE_CAN FALSE 43 | #define HAL_USE_DAC FALSE 44 | #define HAL_USE_EXT FALSE 45 | #define HAL_USE_GPT FALSE 46 | #define HAL_USE_I2C FALSE 47 | #define HAL_USE_I2S FALSE 48 | #define HAL_USE_ICU TRUE 49 | #define HAL_USE_MAC FALSE 50 | #define HAL_USE_MMC_SPI FALSE 51 | #define HAL_USE_PWM FALSE 52 | #define HAL_USE_RTC FALSE 53 | #define HAL_USE_SDC FALSE 54 | #define HAL_USE_SERIAL TRUE 55 | #define HAL_USE_SERIAL_USB FALSE 56 | #define HAL_USE_SPI FALSE 57 | #define HAL_USE_UART FALSE 58 | #define HAL_USE_USB FALSE 59 | #define HAL_USE_WDG FALSE 60 | 61 | #define SERIAL_DEFAULT_BITRATE 115200 62 | #define SERIAL_BUFFERS_SIZE 512 63 | 64 | #include 65 | -------------------------------------------------------------------------------- /firmware/src/pwm_input.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | // TODO: rewrite in C++ 36 | 37 | #pragma once 38 | 39 | #ifdef __cplusplus 40 | extern "C" { 41 | #endif 42 | 43 | void pwm_input_init(void); 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | -------------------------------------------------------------------------------- /firmware/src/temperature_sensor.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace temperature_sensor 44 | { 45 | namespace 46 | { 47 | 48 | static constexpr unsigned SENSOR_ADDRESS = 0b1001000; 49 | static constexpr std::int16_t KELVIN_OFFSET = 273; 50 | 51 | std::int16_t temperature = -1; 52 | bool functional = false; 53 | 54 | std::int16_t convert_lm75b_to_kelvin(const std::array& raw) 55 | { 56 | auto x = std::int16_t((std::uint16_t(raw[0]) << 8) | raw[1]) >> 5; 57 | if (x & (1U << 10)) { 58 | x |= 0xFC00; 59 | } 60 | return x / 8 + KELVIN_OFFSET; 61 | } 62 | 63 | std::int16_t try_read() 64 | { 65 | const std::array tx = { 0 }; 66 | std::array rx; 67 | if (board::i2c_exchange(SENSOR_ADDRESS, tx, rx) < 0) { 68 | return -1; 69 | } 70 | return convert_lm75b_to_kelvin(rx); 71 | } 72 | 73 | class : public chibios_rt::BaseStaticThread<128> 74 | { 75 | void main() override 76 | { 77 | os::watchdog::Timer wdt; 78 | wdt.startMSec(2000); 79 | setName("tempsens"); 80 | 81 | while (!os::isRebootRequested()) { 82 | wdt.reset(); 83 | ::usleep(500 * 1000); 84 | 85 | if (!motor_is_running() && !motor_is_idle()) { 86 | // When the motor is starting, I2C goes bananas 87 | continue; 88 | } 89 | 90 | const std::int16_t new_temp = try_read(); 91 | if (new_temp >= 0) { 92 | temperature = std::int16_t((std::int32_t(temperature) * 7 + new_temp + 7) / 8); 93 | functional = true; 94 | } else { 95 | functional = false; 96 | } 97 | } 98 | } 99 | } thread_; 100 | 101 | } 102 | 103 | int init() 104 | { 105 | temperature = try_read(); 106 | if (temperature < 0) { 107 | return -1; 108 | } 109 | thread_.start(LOWPRIO); 110 | return 0; 111 | } 112 | 113 | bool is_ok() 114 | { 115 | return functional; 116 | } 117 | 118 | std::int16_t get_temperature_K() 119 | { 120 | return temperature; 121 | } 122 | 123 | } 124 | -------------------------------------------------------------------------------- /firmware/src/temperature_sensor.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace temperature_sensor 40 | { 41 | 42 | int init(); 43 | 44 | /** 45 | * True if the sensor is OK, false if it's not. 46 | */ 47 | bool is_ok(); 48 | 49 | /** 50 | * Returns temperature in Kelvin; returns a negative value if temperature is not available. 51 | */ 52 | std::int16_t get_temperature_K(); 53 | 54 | } 55 | -------------------------------------------------------------------------------- /firmware/src/uavcan_node/bootloader_interface.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2016 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | #include 39 | 40 | namespace uavcan_node 41 | { 42 | /** 43 | * Returns version info of the currently running firmware image. 44 | * This function relies on the firmware image post-processing; refer to the build system docs for details. 45 | */ 46 | uavcan::protocol::SoftwareVersion get_uavcan_software_version(); 47 | 48 | /** 49 | * If known, returns the bit rate value inherited from the bootloader. 50 | * If unknown, return zero. 51 | */ 52 | std::uint32_t get_inherited_can_bus_bit_rate(); 53 | 54 | /** 55 | * If known, returns the node ID value inherited from the bootloader. 56 | * If unknown, returns an invalid node ID. 57 | */ 58 | uavcan::NodeID get_inherited_node_id(); 59 | 60 | /** 61 | * Initializes the shared data structure with given values. 62 | */ 63 | void pass_parameters_to_bootloader(std::uint32_t can_bus_bit_rate, uavcan::NodeID node_id); 64 | 65 | } 66 | -------------------------------------------------------------------------------- /firmware/src/uavcan_node/esc_controller.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace uavcan_node 40 | { 41 | 42 | int init_esc_controller(uavcan::INode& node); 43 | 44 | } 45 | -------------------------------------------------------------------------------- /firmware/src/uavcan_node/indication_controller.cpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #include "indication_controller.hpp" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | namespace uavcan_node 43 | { 44 | namespace 45 | { 46 | 47 | unsigned self_light_index = 0; 48 | 49 | os::config::Param param_light_index("light_index", 0, 0, 255); 50 | 51 | 52 | void cb_light_command(const uavcan::ReceivedDataStructure& msg) 53 | { 54 | static board::LEDOverlay led_ctl; 55 | 56 | for (auto& cmd : msg.commands) 57 | { 58 | if (cmd.light_id == self_light_index) 59 | { 60 | using uavcan::equipment::indication::RGB565; 61 | 62 | uavcan::uint32_t red = uavcan::uint32_t(float(cmd.color.red) * 63 | (255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F); 64 | 65 | uavcan::uint32_t green = uavcan::uint32_t(float(cmd.color.green) * 66 | (255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F); 67 | 68 | uavcan::uint32_t blue = uavcan::uint32_t(float(cmd.color.blue) * 69 | (255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F); 70 | 71 | red = uavcan::min(red, 0xFFU); 72 | green = uavcan::min(green, 0xFFU); 73 | blue = uavcan::min(blue, 0xFFU); 74 | 75 | led_ctl.set_hex_rgb((red << 16) | (green << 8) | (blue)); 76 | break; 77 | } 78 | } 79 | } 80 | 81 | void cb_beep_command(const uavcan::ReceivedDataStructure& msg) 82 | { 83 | motor_beep(static_cast(msg.frequency), static_cast(msg.duration * 1000)); 84 | } 85 | 86 | } 87 | 88 | int init_indication_controller(uavcan::INode& node) 89 | { 90 | static uavcan::Subscriber sub_light(node); 91 | static uavcan::Subscriber sub_beep(node); 92 | 93 | self_light_index = param_light_index.get(); 94 | 95 | int res = 0; 96 | 97 | res = sub_light.start(cb_light_command); 98 | if (res != 0) { 99 | return res; 100 | } 101 | 102 | res = sub_beep.start(cb_beep_command); 103 | if (res != 0) { 104 | return res; 105 | } 106 | 107 | return 0; 108 | } 109 | 110 | } 111 | -------------------------------------------------------------------------------- /firmware/src/uavcan_node/indication_controller.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace uavcan_node 40 | { 41 | 42 | int init_indication_controller(uavcan::INode& node); 43 | 44 | } 45 | -------------------------------------------------------------------------------- /firmware/src/uavcan_node/uavcan_node.hpp: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2014 PX4 Development Team. All rights reserved. 4 | * Author: Pavel Kirienko 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #pragma once 36 | 37 | #include 38 | 39 | namespace uavcan_node 40 | { 41 | 42 | void set_node_status_ok(); 43 | void set_node_status_warning(); 44 | void set_node_status_critical(); 45 | 46 | bool is_passive_mode(); 47 | 48 | void print_status(); 49 | 50 | int init(); 51 | 52 | } 53 | -------------------------------------------------------------------------------- /openocd.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/stm32f1x.cfg 7 | -------------------------------------------------------------------------------- /tools/ardubenchmark/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # Pavel Kirienko 3 | # 4 | 5 | SRC = ardubenchmark.c 6 | 7 | MCU = atmega328p 8 | DEF = -DF_CPU=16000000 9 | 10 | FLAGS = -O3 -mmcu=$(MCU) -Wl,-u,vfprintf -lprintf_flt -Wl,-u,vfscanf -lscanf_flt -lm 11 | CFLAGS = $(FLAGS) -ffunction-sections -fdata-sections -Wall -Wextra -Werror -pedantic -Wno-unused-parameter -std=c99 12 | 13 | LDFLAGS = $(FLAGS) -lgcc -lc 14 | 15 | # --------------- 16 | 17 | AVRDUDE_PORT ?= /dev/ttyUSB0 18 | 19 | # --------------- 20 | 21 | COBJ = $(SRC:.c=.o) 22 | SOBJ = $(ASMFILES:.s=.o) 23 | OBJ = $(COBJ) $(SOBJ) 24 | 25 | CROSS_COMPILE = avr- 26 | CC = $(CROSS_COMPILE)gcc 27 | AS = $(CROSS_COMPILE)as 28 | LD = $(CROSS_COMPILE)gcc 29 | CP = ${CROSS_COMPILE}objcopy 30 | SIZE = ${CROSS_COMPILE}size 31 | 32 | # --------------- 33 | 34 | all: output.elf output.hex size 35 | 36 | %.elf: $(OBJ) 37 | $(LD) $(LDFLAGS) $(OBJ) -o $@ 38 | 39 | %.hex: %.elf 40 | $(CP) -O ihex -R .eeprom $< $@ 41 | 42 | $(COBJ): %.o: %.c 43 | $(CC) -c $(DEF) $(INC) $(CFLAGS) $< -o $@ 44 | 45 | clean: 46 | rm -f output.elf output.hex $(OBJ) 47 | 48 | size: 49 | @echo $(MAKEFILE_LIST) 50 | @if [ -f output.elf ]; then echo; $(SIZE) $(SOBJ) $(COBJ) -t; echo; fi; 51 | 52 | sizex: 53 | $(SIZE) $(SOBJ) $(COBJ) -t 54 | @echo 55 | $(SIZE) output.elf -A 56 | 57 | dude: 58 | avrdude -p$(MCU) -carduino -P$(AVRDUDE_PORT) -b57600 -Uflash:w:output.hex:i 59 | 60 | .PHONY: all clean size sizex dude 61 | -------------------------------------------------------------------------------- /tools/blackmagic.gdbinit: -------------------------------------------------------------------------------- 1 | # 2 | # Template for .gdbinit 3 | # Copy the file to .gdbinit in your project root, and adjust the path below to match your system 4 | # 5 | 6 | target extended /dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE578CC-if00 7 | # target extended /dev/ttyACM0 8 | 9 | monitor swdp_scan 10 | attach 1 11 | monitor vector_catch disable hard 12 | -------------------------------------------------------------------------------- /tools/blackmagic_flash.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | PORT=${1:-'/dev/ttyACM0'} 4 | #/dev/serial/by-id/usb-Black_Sphere_Technologies_Black_Magic_Probe_DDE578CC-if00 5 | 6 | # Go to the build outputs directory 7 | cd $(dirname $(readlink -f $0))/../firmware/build 8 | 9 | # Find the firmware ELF 10 | elf=$(ls -1 *.elf) 11 | if [ -z "$elf" ]; then 12 | echo "No firmware found" 13 | exit 1 14 | fi 15 | 16 | arm-none-eabi-size $elf || exit 1 17 | 18 | tmpfile=fwupload.tempfile 19 | cat > $tmpfile <`), 13 | execute `sudo ./setup.sh`, and you're ready to get started: 14 | 15 | ```bash 16 | ./run.sh --help 17 | ``` 18 | 19 | ## Other documentation 20 | 21 | Refer to to find more documentation about anything. 22 | -------------------------------------------------------------------------------- /tools/drwatson/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | git pull > /dev/null 4 | git submodule update --init --recursive 5 | sudo ./drwatson_sapog.py $@ 6 | -------------------------------------------------------------------------------- /tools/drwatson/setup.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [[ $EUID -ne 0 ]]; then 4 | echo "You are not root. Why $USER, why?!" 1>&2 5 | exit 1 6 | fi 7 | 8 | set -o xtrace 9 | 10 | if ! which arm-none-eabi-gdb; then 11 | # https://bugs.launchpad.net/ubuntu/+source/gdb-arm-none-eabi/+bug/1267680 12 | apt-get -o Dpkg::Options::="--force-overwrite" install -y gcc-arm-none-eabi gdb-arm-none-eabi 13 | fi 14 | 15 | apt-get install -y python3 python3-pip can-utils 16 | 17 | pip3 install colorama easywebdav pyserial numpy pyyaml 18 | --------------------------------------------------------------------------------