├── .gitignore ├── LICENSE ├── README.md ├── attic ├── clients │ ├── hc05-client.py │ └── setpoint.py ├── helper-arduino │ ├── .gitignore │ ├── arm_math │ │ ├── Makefile │ │ └── arm_math.ino │ ├── bmi088 │ │ ├── Makefile │ │ └── bmi088.ino │ ├── flow │ │ ├── Makefile │ │ └── flow.ino │ ├── freetos │ │ ├── Makefile │ │ └── freetos.ino │ ├── leds │ │ ├── Makefile │ │ └── leds.ino │ ├── motors │ │ ├── Makefile │ │ └── motors.ino │ ├── serial │ │ ├── Makefile │ │ └── serial.ino │ └── vl53l1x │ │ ├── Makefile │ │ └── vl53l1x.ino └── src │ ├── hal │ ├── Wire.cpp │ └── Wire.h │ └── tasks │ └── receiver.hpp ├── bolt ├── .gitignore ├── Kconfig ├── Makefile ├── README.md ├── build │ ├── .config │ └── .gitignore ├── cf2loader-1.0.bin ├── scripts │ ├── Kbuild.include │ ├── Makefile │ ├── Makefile.build │ ├── Makefile.clean │ ├── Makefile.extrawarn │ ├── Makefile.host │ ├── Makefile.lib │ ├── basic │ │ ├── .gitignore │ │ ├── Makefile │ │ ├── bin2c.c │ │ └── fixdep.c │ ├── checkincludes.pl │ ├── gcc-goto.sh │ ├── headerdep.pl │ ├── kconfig │ │ ├── .gitignore │ │ ├── Makefile │ │ ├── POTFILES.in │ │ ├── check.sh │ │ ├── conf.c │ │ ├── confdata.c │ │ ├── expr.c │ │ ├── expr.h │ │ ├── gconf.c │ │ ├── gconf.glade │ │ ├── images.c │ │ ├── kxgettext.c │ │ ├── list.h │ │ ├── lkc.h │ │ ├── lkc_proto.h │ │ ├── lxdialog │ │ │ ├── .gitignore │ │ │ ├── BIG.FAT.WARNING │ │ │ ├── check-lxdialog.sh │ │ │ ├── checklist.c │ │ │ ├── dialog.h │ │ │ ├── inputbox.c │ │ │ ├── menubox.c │ │ │ ├── textbox.c │ │ │ ├── util.c │ │ │ └── yesno.c │ │ ├── mconf.c │ │ ├── menu.c │ │ ├── merge_config.sh │ │ ├── nconf.c │ │ ├── nconf.gui.c │ │ ├── nconf.h │ │ ├── qconf.cc │ │ ├── qconf.h │ │ ├── streamline_config.pl │ │ ├── symbol.c │ │ ├── util.c │ │ ├── zconf.gperf │ │ ├── zconf.hash.c_shipped │ │ ├── zconf.l │ │ ├── zconf.lex.c_shipped │ │ ├── zconf.tab.c_shipped │ │ └── zconf.y │ └── mkmakefile ├── src │ ├── .gitignore │ ├── FreeRTOSConfig.h │ ├── Kbuild.haskell │ ├── Kbuild.standard │ ├── Makefile │ ├── bosch │ │ ├── Kbuild │ │ ├── bmi088.h │ │ ├── bmi088_accel.c │ │ ├── bmi088_defs.h │ │ ├── bmi088_fifo.c │ │ ├── bmi088_fifo.h │ │ ├── bmi088_gyro.c │ │ ├── bstdr_comm_support.h │ │ └── bstdr_types.h │ ├── free_rtos │ │ ├── .gitignore │ │ ├── FreeRTOS.h │ │ ├── Kbuild │ │ ├── StackMacros.h │ │ ├── atomic.h │ │ ├── croutine.h │ │ ├── event_groups.c │ │ ├── event_groups.h │ │ ├── heap_4.c │ │ ├── list.c │ │ ├── list.h │ │ ├── message_buffer.h │ │ ├── mpu_prototypes.h │ │ ├── mpu_wrappers.h │ │ ├── port_system_dependent.c │ │ ├── portable.h │ │ ├── portmacro_system_dependent.h │ │ ├── projdefs.h │ │ ├── queue.c │ │ ├── queue.h │ │ ├── semphr.h │ │ ├── stack_macros.h │ │ ├── stdint.readme │ │ ├── stream_buffer.c │ │ ├── stream_buffer.h │ │ ├── task.h │ │ ├── tasks.c │ │ ├── timers.c │ │ └── timers.h │ ├── hal │ │ ├── Arduino.h │ │ ├── Kbuild │ │ ├── SPI.cpp │ │ ├── SPI.h │ │ ├── digital.cpp │ │ ├── digital.h │ │ ├── exti.c │ │ ├── exti.h │ │ ├── gpio.h │ │ ├── i2cdev.c │ │ ├── i2cdev.h │ │ ├── interrupt.c │ │ ├── interrupt.h │ │ ├── motors.cpp │ │ ├── nvic.c │ │ ├── nvic.h │ │ ├── nvicconf.h │ │ ├── spi2.cpp │ │ ├── spi2.h │ │ ├── static_mem.h │ │ ├── time.c │ │ ├── time.h │ │ ├── uart.c │ │ ├── uart.h │ │ ├── usb.cpp │ │ ├── usb.h │ │ ├── usb_bsp.c │ │ └── usbd_desc.c │ ├── main.cpp │ ├── nvicconf.h │ ├── stm32f4 │ │ ├── Kbuild │ │ ├── STM32F4xx_StdPeriph_Driver │ │ │ ├── Release_Notes.html │ │ │ ├── inc │ │ │ │ ├── misc.h │ │ │ │ ├── stm32f4xx_adc.h │ │ │ │ ├── stm32f4xx_can.h │ │ │ │ ├── stm32f4xx_cec.h │ │ │ │ ├── stm32f4xx_crc.h │ │ │ │ ├── stm32f4xx_cryp.h │ │ │ │ ├── stm32f4xx_dac.h │ │ │ │ ├── stm32f4xx_dbgmcu.h │ │ │ │ ├── stm32f4xx_dcmi.h │ │ │ │ ├── stm32f4xx_dfsdm.h │ │ │ │ ├── stm32f4xx_dma.h │ │ │ │ ├── stm32f4xx_dma2d.h │ │ │ │ ├── stm32f4xx_dsi.h │ │ │ │ ├── stm32f4xx_exti.h │ │ │ │ ├── stm32f4xx_flash.h │ │ │ │ ├── stm32f4xx_flash_ramfunc.h │ │ │ │ ├── stm32f4xx_fmc.h │ │ │ │ ├── stm32f4xx_fmpi2c.h │ │ │ │ ├── stm32f4xx_fsmc.h │ │ │ │ ├── stm32f4xx_gpio.h │ │ │ │ ├── stm32f4xx_hash.h │ │ │ │ ├── stm32f4xx_i2c.h │ │ │ │ ├── stm32f4xx_iwdg.h │ │ │ │ ├── stm32f4xx_lptim.h │ │ │ │ ├── stm32f4xx_ltdc.h │ │ │ │ ├── stm32f4xx_misc.h │ │ │ │ ├── stm32f4xx_pwr.h │ │ │ │ ├── stm32f4xx_qspi.h │ │ │ │ ├── stm32f4xx_rcc.h │ │ │ │ ├── stm32f4xx_rng.h │ │ │ │ ├── stm32f4xx_rtc.h │ │ │ │ ├── stm32f4xx_sai.h │ │ │ │ ├── stm32f4xx_sdio.h │ │ │ │ ├── stm32f4xx_spdifrx.h │ │ │ │ ├── stm32f4xx_spi.h │ │ │ │ ├── stm32f4xx_syscfg.h │ │ │ │ ├── stm32f4xx_tim.h │ │ │ │ ├── stm32f4xx_usart.h │ │ │ │ └── stm32f4xx_wwdg.h │ │ │ └── src │ │ │ │ ├── misc.c │ │ │ │ ├── stm32f4xx_adc.c │ │ │ │ ├── stm32f4xx_can.c │ │ │ │ ├── stm32f4xx_cec.c │ │ │ │ ├── stm32f4xx_crc.c │ │ │ │ ├── stm32f4xx_cryp.c │ │ │ │ ├── stm32f4xx_cryp_aes.c │ │ │ │ ├── stm32f4xx_cryp_des.c │ │ │ │ ├── stm32f4xx_cryp_tdes.c │ │ │ │ ├── stm32f4xx_dac.c │ │ │ │ ├── stm32f4xx_dbgmcu.c │ │ │ │ ├── stm32f4xx_dcmi.c │ │ │ │ ├── stm32f4xx_dfsdm.c │ │ │ │ ├── stm32f4xx_dma.c │ │ │ │ ├── stm32f4xx_dma2d.c │ │ │ │ ├── stm32f4xx_dsi.c │ │ │ │ ├── stm32f4xx_exti.c │ │ │ │ ├── stm32f4xx_flash.c │ │ │ │ ├── stm32f4xx_flash_ramfunc.c │ │ │ │ ├── stm32f4xx_fmc.c │ │ │ │ ├── stm32f4xx_fmpi2c.c │ │ │ │ ├── stm32f4xx_fsmc.c │ │ │ │ ├── stm32f4xx_gpio.c │ │ │ │ ├── stm32f4xx_hash.c │ │ │ │ ├── stm32f4xx_hash_md5.c │ │ │ │ ├── stm32f4xx_hash_sha1.c │ │ │ │ ├── stm32f4xx_i2c.c │ │ │ │ ├── stm32f4xx_iwdg.c │ │ │ │ ├── stm32f4xx_lptim.c │ │ │ │ ├── stm32f4xx_ltdc.c │ │ │ │ ├── stm32f4xx_misc.c │ │ │ │ ├── stm32f4xx_pwr.c │ │ │ │ ├── stm32f4xx_qspi.c │ │ │ │ ├── stm32f4xx_rcc.c │ │ │ │ ├── stm32f4xx_rng.c │ │ │ │ ├── stm32f4xx_rtc.c │ │ │ │ ├── stm32f4xx_sai.c │ │ │ │ ├── stm32f4xx_sdio.c │ │ │ │ ├── stm32f4xx_spdifrx.c │ │ │ │ ├── stm32f4xx_spi.c │ │ │ │ ├── stm32f4xx_syscfg.c │ │ │ │ ├── stm32f4xx_tim.c │ │ │ │ ├── stm32f4xx_usart.c │ │ │ │ └── stm32f4xx_wwdg.c │ │ ├── STM32_USB_Device_Library │ │ │ ├── Core │ │ │ │ ├── inc │ │ │ │ │ ├── usbd_conf_template.h │ │ │ │ │ ├── usbd_core.h │ │ │ │ │ ├── usbd_def.h │ │ │ │ │ ├── usbd_ioreq.h │ │ │ │ │ ├── usbd_req.h │ │ │ │ │ └── usbd_usr.h │ │ │ │ └── src │ │ │ │ │ ├── usbd_core.c │ │ │ │ │ ├── usbd_ioreq.c │ │ │ │ │ └── usbd_req.c │ │ │ └── Release_Notes.html │ │ ├── STM32_USB_OTG_Driver │ │ │ ├── Release_Notes.html │ │ │ ├── inc │ │ │ │ ├── usb_bsp.h │ │ │ │ ├── usb_conf_template.h │ │ │ │ ├── usb_core.h │ │ │ │ ├── usb_dcd.h │ │ │ │ ├── usb_dcd_int.h │ │ │ │ ├── usb_defines.h │ │ │ │ ├── usb_hcd.h │ │ │ │ ├── usb_hcd_int.h │ │ │ │ ├── usb_otg.h │ │ │ │ └── usb_regs.h │ │ │ └── src │ │ │ │ ├── usb_bsp_template.c │ │ │ │ ├── usb_core.c │ │ │ │ ├── usb_dcd.c │ │ │ │ ├── usb_dcd_int.c │ │ │ │ ├── usb_hcd.c │ │ │ │ ├── usb_hcd_int.c │ │ │ │ └── usb_otg.c │ │ ├── startup_stm32f40xx.S │ │ ├── stm32f4xx.h │ │ ├── stm32f4xx_conf.h │ │ ├── stm32fxxx.h │ │ ├── system_stm32f4xx.c │ │ ├── system_stm32f4xx.h │ │ ├── usb_conf.h │ │ ├── usbd_conf.h │ │ └── usbd_desc.h │ ├── system.cpp │ └── vapplication.c ├── tools │ ├── kbuild │ │ └── Makefile.kbuild │ └── make │ │ ├── F405 │ │ ├── linker │ │ │ ├── COMMON.ld │ │ │ ├── DEF.ld │ │ │ ├── DEF_CLOAD.ld │ │ │ ├── FLASH.ld │ │ │ ├── FLASH_CLOAD.ld │ │ │ └── sections_FLASH.ld │ │ ├── st_obj.mk │ │ ├── stm32f4discovery.cfg │ │ └── stm32f4x_stlink.cfg │ │ ├── check-for-submodules.py │ │ ├── cmsis_dsp │ │ ├── Makefile │ │ └── obj.mk │ │ ├── dfu-convert.py │ │ ├── oot.mk │ │ ├── size.py │ │ ├── targets.mk │ │ ├── usb-bootloader.py │ │ └── versionTemplate.py └── vendor │ ├── .gitignore │ └── Kbuild ├── clients ├── .gitignore ├── btsupport.py ├── gamepad.py ├── spikes.sh └── state.py ├── examples ├── .gitignore ├── bmi088 │ ├── FreeRTOSConfig.h │ ├── Makefile │ └── bmi088.ino └── standard │ ├── .gitignore │ ├── FreeRTOSConfig.h │ ├── Makefile │ └── standard.ino ├── haskell ├── .gitignore ├── Num.hs ├── Pids.hs ├── README.md └── pids │ ├── AltitudeController.hs │ ├── ClimbRateController.hs │ ├── PitchRollAngleController.hs │ ├── PitchRollRateController.hs │ ├── PositionController.hs │ ├── YawAngleController.hs │ └── YawRateController.hs ├── library.properties ├── media ├── boltquad.jpg ├── cage.odp ├── dataflow.odp ├── dataflow2.png ├── haskell.png ├── icon.png ├── lambda.png ├── logo.png ├── msppg.png ├── rust.png ├── sim.odp ├── sim.png ├── tennflight.png ├── tinyape.jpg ├── webots.png └── webots2.png ├── msppg ├── .gitignore ├── Makefile ├── README.md ├── messages.json └── msppg.py ├── rpi ├── .gitignore ├── Makefile └── btserver.cpp ├── src ├── .gitignore ├── cfassert.h ├── clock.hpp ├── control │ ├── haskell.hpp │ ├── pids │ │ ├── altitude.hpp │ │ ├── climbrate.hpp │ │ ├── pitchroll_angle.hpp │ │ ├── pitchroll_rate.hpp │ │ ├── position.hpp │ │ ├── yaw_angle.hpp │ │ └── yaw_rate.hpp │ └── standard.hpp ├── datatypes.h ├── dynamics.hpp ├── hackflight.h ├── kalman.hpp ├── linalg.h ├── lpf.hpp ├── m_pi.h ├── math3d.h ├── mixers │ └── crazyflie.hpp ├── motors.h ├── msp │ ├── .gitignore │ ├── parser.hpp │ └── serializer.hpp ├── num.hpp ├── outlierFilterTdoa.hpp ├── rateSupervisor.hpp ├── safety.hpp ├── system.cpp ├── system.h ├── task.hpp ├── tasks │ ├── core.hpp │ ├── debug.hpp │ ├── estimator.hpp │ ├── flowdeck.hpp │ ├── imu.hpp │ ├── led.hpp │ ├── logger.hpp │ ├── setpoint.hpp │ └── zranger.hpp └── vehicles │ └── diyquad.hpp └── webots ├── .gitignore ├── README.md ├── controllers └── controller │ ├── .gitignore │ ├── Makefile │ ├── controller.cpp │ ├── newplot.py │ └── plot.py ├── meshes ├── collada_files │ └── diyquad.dae ├── stl_files │ ├── prop_ccw.stl │ └── prop_cw.stl └── textures │ └── fast_helix.png ├── plugins └── physics │ ├── .gitignore │ ├── custom │ └── .gitignore │ ├── haskell │ ├── .gitignore │ ├── Makefile │ └── sim_control.hpp │ ├── snn │ ├── .gitignore │ ├── Makefile │ └── sim_control.hpp │ ├── standard │ ├── .gitignore │ └── Makefile │ └── support.cpp ├── protos └── DiyQuad.proto └── worlds ├── .gitignore └── simple.wbt /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.swp 3 | -------------------------------------------------------------------------------- /attic/clients/hc05-client.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import socket 4 | from sys import stdout 5 | 6 | import msp 7 | 8 | BT_ADDRESS = 'B8:27:EB:3F:AB:47' 9 | BT_PORT = 2 10 | 11 | 12 | class StateParser(msp.Parser): 13 | 14 | def __init__(self): 15 | 16 | msp.Parser.__init__(self) 17 | 18 | def handle_STATE(self, x, dx, y, dy, z, dz, phi, dphi, theta, dtheta, 19 | psi, dpsi): 20 | 21 | print(('dx=%+03.2f dy=%+03.2f z=%+03.2f dz=%+03.2f ' + 22 | 'phi=%+5.1f dphi=%+6.1f theta=%+5.1f dtheta=%+6.1f ' + 23 | 'psi=%+5.1f dpsi=%+5.1f') % 24 | (dx, dy, z, dz, phi, dphi, theta, dtheta, psi, dpsi)) 25 | 26 | 27 | def main(): 28 | 29 | print('Connecting to server %s:%d ... ' % (BT_ADDRESS, BT_PORT), end='') 30 | stdout.flush() 31 | 32 | # Create a Bluetooth or IP socket depending on address format 33 | client = (socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, 34 | socket.BTPROTO_RFCOMM)) 35 | 36 | try: 37 | client.connect((BT_ADDRESS, BT_PORT)) 38 | 39 | except Exception as e: 40 | print(str(e) + ': is server running?') 41 | exit(0) 42 | 43 | byte = None 44 | 45 | parser = StateParser() 46 | 47 | count = 0 48 | 49 | while True: 50 | 51 | try: 52 | 53 | msg = client.recv(48) 54 | 55 | print(count) 56 | 57 | count += 1 58 | 59 | # byte = client.recv(1) 60 | 61 | except KeyboardInterrupt: 62 | break 63 | 64 | # if byte is not None: 65 | # parser.parse(byte) 66 | 67 | 68 | main() 69 | -------------------------------------------------------------------------------- /attic/helper-arduino/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /attic/helper-arduino/arm_math/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = arm_math 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | 26 | -------------------------------------------------------------------------------- /attic/helper-arduino/arm_math/arm_math.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void setup() 4 | { 5 | Serial.begin(115200); 6 | } 7 | 8 | void loop() 9 | { 10 | float32_t A_f32[4] = {1.0, 2.0, 3.0, 4.0}; 11 | float32_t B_f32[4] = {5.0, 6.0, 7.0, 8.0}; 12 | 13 | const uint16_t numRowsA = 2; 14 | const uint16_t numColsA = 2; 15 | const uint16_t numRowsB = 2; 16 | const uint16_t numColsB = 2; 17 | 18 | arm_matrix_instance_f32 A, B, C; 19 | 20 | arm_mat_init_f32(&A, numRowsA, numColsA, A_f32); 21 | arm_mat_init_f32(&B, numRowsB, numColsB, B_f32); 22 | arm_mat_init_f32(&C, numRowsA, numColsB, NULL); // C will be the result matrix 23 | 24 | float32_t C_f32[numRowsA * numColsB]; 25 | C.pData = C_f32; 26 | 27 | arm_status status = arm_mat_mult_f32(&A, &B, &C); 28 | 29 | if (status == ARM_MATH_SUCCESS) { 30 | 31 | Serial.println("Matrix C (A * B):"); 32 | for (int i = 0; i < numRowsA; i++) { 33 | for (int j = 0; j < numColsB; j++) { 34 | Serial.print(C.pData[i * numColsB + j]); 35 | Serial.print(" "); 36 | } 37 | Serial.println(); 38 | } 39 | Serial.println(); 40 | } else { 41 | Serial.println("Matrix multiplication failed!"); 42 | } 43 | 44 | delay(1000); 45 | } 46 | -------------------------------------------------------------------------------- /attic/helper-arduino/bmi088/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = bmi088 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino Makefile 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | 26 | -------------------------------------------------------------------------------- /attic/helper-arduino/bmi088/bmi088.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static SPIClass myspi = SPIClass (PB15, PB14, PB13); // MOSI, MISO, SCLK 4 | static Bmi088Accel accel(myspi, PB1); // CS 5 | static Bmi088Gyro gyro(myspi, PB0); // CS 6 | 7 | static bool gotGyroInterrupt; 8 | 9 | static void gyroDrdy() 10 | { 11 | gotGyroInterrupt = true; 12 | } 13 | 14 | void setup() 15 | { 16 | Serial.begin(115200); 17 | 18 | int status = 0; 19 | 20 | status = accel.begin(); 21 | while (status < 0) { 22 | Serial.println("Accel Initialization Error"); 23 | Serial.println(status); 24 | delay(500); 25 | } 26 | 27 | accel.setOdr(Bmi088Accel::ODR_1600HZ_BW_145HZ); 28 | accel.setRange(Bmi088Accel::RANGE_24G); 29 | 30 | status = gyro.begin(); 31 | while (status < 0) { 32 | Serial.println("Gyro Initialization Error"); 33 | Serial.println(status); 34 | delay(500); 35 | } 36 | 37 | gyro.setOdr(Bmi088Gyro::ODR_1000HZ_BW_116HZ); 38 | gyro.setRange(Bmi088Gyro::RANGE_2000DPS); 39 | 40 | gyro.pinModeInt3(Bmi088Gyro::PUSH_PULL, Bmi088Gyro::ACTIVE_HIGH); 41 | gyro.mapDrdyInt3(true); 42 | 43 | pinMode(PC14, INPUT); 44 | attachInterrupt(PC14, gyroDrdy, RISING); 45 | } 46 | 47 | void loop() 48 | { 49 | if (gotGyroInterrupt) { 50 | 51 | gotGyroInterrupt = false; 52 | 53 | gyro.readSensor(); 54 | 55 | Serial.print(accel.getAccelX_mss()); 56 | Serial.print("\t"); 57 | Serial.print(accel.getAccelY_mss()); 58 | Serial.print("\t"); 59 | Serial.print(accel.getAccelZ_mss()); 60 | Serial.print("\t"); 61 | 62 | accel.readSensor(); 63 | 64 | Serial.print(gyro.getGyroX_rads()); 65 | Serial.print("\t"); 66 | Serial.print(gyro.getGyroY_rads()); 67 | Serial.print("\t"); 68 | Serial.print(gyro.getGyroZ_rads()); 69 | Serial.print("\t"); 70 | 71 | Serial.print(accel.getTemperature_C()); 72 | 73 | Serial.print("\n"); 74 | } 75 | 76 | } 77 | -------------------------------------------------------------------------------- /attic/helper-arduino/flow/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = flow 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | 26 | -------------------------------------------------------------------------------- /attic/helper-arduino/flow/flow.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static PMW3901 pmw3901; 4 | 5 | static const uint8_t CS_PIN = 20; 6 | 7 | void setup() 8 | { 9 | Serial.begin(115200); 10 | 11 | if (!pmw3901.begin(CS_PIN)) { 12 | 13 | while (true) { 14 | Serial.println("Initialization of the flow sensor failed"); 15 | delay(500); 16 | } 17 | } 18 | } 19 | 20 | void loop() 21 | { 22 | int16_t deltaX=0, deltaY=0; 23 | 24 | // Get motion count since last call 25 | pmw3901.readMotion(deltaX, deltaY); 26 | 27 | Serial.print("X: "); 28 | Serial.print(deltaX); 29 | Serial.print(", Y: "); 30 | Serial.print(deltaY); 31 | Serial.print("\n"); 32 | 33 | delay(100); 34 | } 35 | -------------------------------------------------------------------------------- /attic/helper-arduino/freetos/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = freetos 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | -------------------------------------------------------------------------------- /attic/helper-arduino/freetos/freetos.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static const uint8_t LED_RED_L = PC0; 4 | static const uint8_t LED_GREEN_L = PC1; 5 | static const uint8_t LED_GREEN_R = PC2; 6 | static const uint8_t LED_RED_R = PC3; 7 | static const uint8_t LED_BLUE_L = PD2; 8 | 9 | static const uint8_t LED_PIN = LED_GREEN_R; 10 | 11 | static SemaphoreHandle_t sem; 12 | 13 | static void Thread1(void* arg) 14 | { 15 | (void)arg; 16 | 17 | while (true) { 18 | 19 | // Wait for signal from thread 2. 20 | xSemaphoreTake(sem, portMAX_DELAY); 21 | 22 | // Turn LED off. 23 | digitalWrite(LED_PIN, LOW); 24 | } 25 | } 26 | 27 | 28 | static void Thread2(void* arg) 29 | { 30 | 31 | (void)arg; 32 | 33 | pinMode(LED_PIN, OUTPUT); 34 | 35 | while (true) { 36 | 37 | digitalWrite(LED_PIN, HIGH); 38 | 39 | // Sleep for 200 milliseconds. 40 | vTaskDelay((200L * configTICK_RATE_HZ) / 1000L); 41 | 42 | // Signal thread 1 to turn LED off. 43 | xSemaphoreGive(sem); 44 | 45 | // Sleep for 200 milliseconds. 46 | vTaskDelay((200L * configTICK_RATE_HZ) / 1000L); 47 | 48 | Serial.println(millis()); 49 | } 50 | } 51 | //------------------------------------------------------------------------------ 52 | void setup() 53 | { 54 | Serial.begin(115200); 55 | 56 | // initialize semaphore 57 | sem = xSemaphoreCreateCounting(1, 0); 58 | 59 | // create task at priority two 60 | portBASE_TYPE s1 = xTaskCreate(Thread1, NULL, configMINIMAL_STACK_SIZE, NULL, 2, NULL); 61 | 62 | // create task at priority one 63 | portBASE_TYPE s2 = xTaskCreate(Thread2, NULL, configMINIMAL_STACK_SIZE, NULL, 1, NULL); 64 | 65 | // check for creation errors 66 | if (sem== NULL || s1 != pdPASS || s2 != pdPASS ) { 67 | Serial.println(F("Creation problem")); 68 | while(1); 69 | } 70 | 71 | // start scheduler 72 | vTaskStartScheduler(); 73 | Serial.println("Insufficient RAM"); 74 | 75 | while (true) { 76 | } 77 | } 78 | 79 | void loop() 80 | { 81 | } 82 | -------------------------------------------------------------------------------- /attic/helper-arduino/leds/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = leds 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino Makefile 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | 26 | -------------------------------------------------------------------------------- /attic/helper-arduino/leds/leds.ino: -------------------------------------------------------------------------------- 1 | static const uint8_t NLEDS = 5; 2 | 3 | static const uint8_t BLUE_L = PD2; 4 | static const uint8_t GREEN_L = PC1; 5 | static const uint8_t RED_L = PC0; 6 | static const uint8_t GREEN_R = PC2; 7 | static const uint8_t RED_R = PC3; 8 | 9 | static const uint8_t pins[NLEDS] = {BLUE_L, GREEN_L, RED_L, GREEN_R, RED_R}; 10 | static const uint8_t polarities[NLEDS] = {1, 0, 0, 0, 0}; 11 | static const char * names[NLEDS] = {"BLUE_L", "GREEN_L", "RED_L", "GREEN_R", "RED_R"}; 12 | 13 | static uint8_t active; 14 | 15 | void setup() 16 | { 17 | Serial.begin(115200); 18 | 19 | for (uint8_t k=0; k 2 | #include 3 | 4 | static VL53L1X sensor; 5 | 6 | void setup() 7 | { 8 | Serial.begin(115200); 9 | 10 | static TwoWire wire = TwoWire(PB7, PB6); 11 | 12 | wire.begin(); 13 | wire.setClock(400000); // use 400 kHz I2C 14 | 15 | sensor.setBus(&wire); 16 | 17 | sensor.setTimeout(500); 18 | 19 | if (!sensor.init()) { 20 | while (true) { 21 | Serial.printf("Failed to detect and initialize sensor!\n"); 22 | delay(500); 23 | } 24 | } 25 | 26 | // Use long distance mode and allow up to 50000 us (50 ms) for a measurement. 27 | // You can change these settings to adjust the performance of the sensor, but 28 | // the minimum timing budget is 20 ms for short distance mode and 33 ms for 29 | // medium and long distance modes. See the VL53L1X datasheet for more 30 | // information on range and timing limits. 31 | sensor.setDistanceMode(VL53L1X::Long); 32 | sensor.setMeasurementTimingBudget(50000); 33 | 34 | // Start continuous readings at a rate of one measurement every 50 ms (the 35 | // inter-measurement period). This period should be at least as long as the 36 | // timing budget. 37 | sensor.startContinuous(50); 38 | } 39 | 40 | void loop() 41 | { 42 | Serial.print(sensor.read()); 43 | 44 | if (sensor.timeoutOccurred()) { 45 | Serial.print(" TIMEOUT"); 46 | } 47 | 48 | Serial.println(); 49 | } 50 | -------------------------------------------------------------------------------- /attic/src/hal/Wire.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | 9 | class TwoWire { 10 | 11 | public: 12 | 13 | void begin(); 14 | 15 | void beginTransmission(const uint8_t addr); 16 | 17 | uint8_t endTransmission(const uint8_t stop=0); 18 | 19 | uint8_t read(); 20 | 21 | size_t requestFrom( 22 | const uint8_t address, const size_t size, bool sendStop=0); 23 | 24 | void write(const uint8_t value); 25 | 26 | private: 27 | 28 | uint8_t _buffer[256]; 29 | 30 | uint8_t _bufidx; 31 | 32 | uint8_t _addr; 33 | }; 34 | -------------------------------------------------------------------------------- /attic/src/tasks/receiver.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class ReceiverTask : public FreeRTOSTask { 7 | 8 | public: 9 | 10 | 11 | void begin(void) 12 | { 13 | pinMode(LED_BUILTIN, OUTPUT); 14 | 15 | FreeRTOSTask::begin(run, "receiever", this, 4); 16 | } 17 | 18 | void getRawChannelValues(int16_t chanvals[6]) 19 | { 20 | static int16_t throttle; 21 | static int8_t dir; 22 | 23 | dir = 24 | dir == 0 ? +1 : 25 | throttle == 2000 ? -1 : 26 | throttle == 1000 ? +1 : 27 | dir; 28 | 29 | throttle = throttle == 0 ? 1000 : throttle + dir; 30 | 31 | chanvals[0] = throttle; 32 | chanvals[1] = 1500; 33 | chanvals[2] = 1500; 34 | chanvals[3] = 1500; 35 | chanvals[4] = 1500; 36 | chanvals[5] = 1500; 37 | } 38 | 39 | private: 40 | 41 | static void run(void * obj) 42 | { 43 | ((ReceiverTask *)obj)->run(); 44 | } 45 | 46 | void run(void) 47 | { 48 | consolePrintf("RECEIVER: running\n"); 49 | 50 | while (true) { 51 | 52 | static uint32_t prev; 53 | 54 | auto msec = millis(); 55 | 56 | if (msec - prev > 500) { 57 | 58 | static bool toggle; 59 | 60 | digitalWriteFast(LED_BUILTIN, toggle); 61 | 62 | toggle = !toggle; 63 | 64 | prev = msec; 65 | } 66 | 67 | vTaskDelay(1); 68 | 69 | } 70 | } 71 | }; 72 | -------------------------------------------------------------------------------- /bolt/.gitignore: -------------------------------------------------------------------------------- 1 | src/Kbuild 2 | bolt.* 3 | copilot* 4 | -------------------------------------------------------------------------------- /bolt/README.md: -------------------------------------------------------------------------------- 1 | Additional Arduino libraries needed: 2 | 3 | * https://github.com/simondlevy/Arduino\_CMSIS-DSP 4 | * https://github.com/simondlevy/BoschSensors 5 | * https://github.com/simondlevy/PMW3901 6 | * https://github.com/simondlevy/VL53L1 7 | 8 | In ~/.arduino15/packages, put STMicroelectronics, with tools/CMSIS/5.9.0/CMSIS/ removed 9 | -------------------------------------------------------------------------------- /bolt/build/.config: -------------------------------------------------------------------------------- 1 | # 2 | # Automatically generated file; DO NOT EDIT. 3 | # Crazyflie Platform Configuration 4 | # 5 | 6 | # 7 | # Build and debug options 8 | # 9 | CONFIG_CROSS_COMPILE="arm-none-eabi-" 10 | # CONFIG_DEBUG is not set 11 | # CONFIG_DEBUG_ENABLE_LED_MORSE is not set 12 | # CONFIG_DEBUG_PRINT_ON_UART1 is not set 13 | # CONFIG_DEBUG_DECK_IGNORE_OW is not set 14 | 15 | # 16 | # Platform configuration 17 | # 18 | # CONFIG_PLATFORM_CF2 is not set 19 | CONFIG_PLATFORM_BOLT=y 20 | # CONFIG_PLATFORM_TAG is not set 21 | # CONFIG_PLATFORM_FLAPPER is not set 22 | 23 | # 24 | # IMU configuration 25 | # 26 | # CONFIG_IMU_MADGWICK_QUATERNION is not set 27 | CONFIG_IMU_MAHONY_QUATERNION=y 28 | 29 | # 30 | # Sensor configuration 31 | # 32 | # CONFIG_SENSORS_MPU9250_LPS25H is not set 33 | CONFIG_SENSORS_BMI088_BMP388=y 34 | # CONFIG_SENSORS_BOSCH is not set 35 | # CONFIG_SENSORS_IGNORE_BAROMETER_FAIL is not set 36 | CONFIG_SENSORS_BMI088_SPI=y 37 | CONFIG_SENSORS_BMI088_I2C=y 38 | 39 | # 40 | # App layer configuration 41 | # 42 | # CONFIG_APP_ENABLE is not set 43 | 44 | # 45 | # Expansion deck configuration 46 | # 47 | CONFIG_DECK_FORCE="none" 48 | -------------------------------------------------------------------------------- /bolt/build/.gitignore: -------------------------------------------------------------------------------- 1 | *.bin 2 | *.elf 3 | *.hex 4 | *.map 5 | .firmware.cmd 6 | copilot* 7 | include 8 | arduino 9 | Makefile 10 | scripts 11 | source 12 | src 13 | vendor 14 | -------------------------------------------------------------------------------- /bolt/cf2loader-1.0.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/bolt/cf2loader-1.0.bin -------------------------------------------------------------------------------- /bolt/scripts/Makefile: -------------------------------------------------------------------------------- 1 | # Let clean descend into subdirs 2 | subdir- += basic kconfig 3 | -------------------------------------------------------------------------------- /bolt/scripts/Makefile.clean: -------------------------------------------------------------------------------- 1 | # ========================================================================== 2 | # Cleaning up 3 | # ========================================================================== 4 | 5 | src := $(obj) 6 | 7 | PHONY := __clean 8 | __clean: 9 | 10 | include scripts/Kbuild.include 11 | 12 | # The filename Kbuild has precedence over Makefile 13 | kbuild-dir := $(if $(filter /%,$(src)),$(src),$(srctree)/$(src)) 14 | include $(if $(wildcard $(kbuild-dir)/Kbuild), $(kbuild-dir)/Kbuild, $(kbuild-dir)/Makefile) 15 | 16 | # Figure out what we need to build from the various variables 17 | # ========================================================================== 18 | 19 | __subdir-y := $(patsubst %/,%,$(filter %/, $(obj-y))) 20 | subdir-y += $(__subdir-y) 21 | __subdir-m := $(patsubst %/,%,$(filter %/, $(obj-m))) 22 | subdir-m += $(__subdir-m) 23 | __subdir- := $(patsubst %/,%,$(filter %/, $(obj-))) 24 | subdir- += $(__subdir-) 25 | 26 | # Subdirectories we need to descend into 27 | 28 | subdir-ym := $(sort $(subdir-y) $(subdir-m)) 29 | subdir-ymn := $(sort $(subdir-ym) $(subdir-)) 30 | 31 | # Add subdir path 32 | 33 | subdir-ymn := $(addprefix $(obj)/,$(subdir-ymn)) 34 | 35 | # build a list of files to remove, usually relative to the current 36 | # directory 37 | 38 | __clean-files := $(extra-y) $(extra-m) $(extra-) \ 39 | $(always) $(targets) $(clean-files) \ 40 | $(host-progs) \ 41 | $(hostprogs-y) $(hostprogs-m) $(hostprogs-) 42 | 43 | __clean-files := $(filter-out $(no-clean-files), $(__clean-files)) 44 | 45 | # clean-files is given relative to the current directory, unless it 46 | # starts with $(objtree)/ (which means "./", so do not add "./" unless 47 | # you want to delete a file from the toplevel object directory). 48 | 49 | __clean-files := $(wildcard \ 50 | $(addprefix $(obj)/, $(filter-out $(objtree)/%, $(__clean-files))) \ 51 | $(filter $(objtree)/%, $(__clean-files))) 52 | 53 | # same as clean-files 54 | 55 | __clean-dirs := $(wildcard \ 56 | $(addprefix $(obj)/, $(filter-out $(objtree)/%, $(clean-dirs))) \ 57 | $(filter $(objtree)/%, $(clean-dirs))) 58 | 59 | # ========================================================================== 60 | 61 | quiet_cmd_clean = CLEAN $(obj) 62 | cmd_clean = rm -f $(__clean-files) 63 | quiet_cmd_cleandir = CLEAN $(__clean-dirs) 64 | cmd_cleandir = rm -rf $(__clean-dirs) 65 | 66 | 67 | __clean: $(subdir-ymn) 68 | ifneq ($(strip $(__clean-files)),) 69 | +$(call cmd,clean) 70 | endif 71 | ifneq ($(strip $(__clean-dirs)),) 72 | +$(call cmd,cleandir) 73 | endif 74 | @: 75 | 76 | 77 | # =========================================================================== 78 | # Generic stuff 79 | # =========================================================================== 80 | 81 | # Descending 82 | # --------------------------------------------------------------------------- 83 | 84 | PHONY += $(subdir-ymn) 85 | $(subdir-ymn): 86 | $(Q)$(MAKE) $(clean)=$@ 87 | 88 | # Declare the contents of the .PHONY variable as phony. We keep that 89 | # information in a variable se we can use it in if_changed and friends. 90 | 91 | .PHONY: $(PHONY) 92 | -------------------------------------------------------------------------------- /bolt/scripts/Makefile.extrawarn: -------------------------------------------------------------------------------- 1 | # ========================================================================== 2 | # 3 | # make W=... settings 4 | # 5 | # W=1 - warnings that may be relevant and does not occur too often 6 | # W=2 - warnings that occur quite often but may still be relevant 7 | # W=3 - the more obscure warnings, can most likely be ignored 8 | # 9 | # $(call cc-option, -W...) handles gcc -W.. options which 10 | # are not supported by all versions of the compiler 11 | # ========================================================================== 12 | 13 | ifeq ("$(origin W)", "command line") 14 | export KBUILD_ENABLE_EXTRA_GCC_CHECKS := $(W) 15 | endif 16 | 17 | ifdef KBUILD_ENABLE_EXTRA_GCC_CHECKS 18 | warning- := $(empty) 19 | 20 | warning-1 := -Wextra -Wunused -Wno-unused-parameter 21 | warning-1 += -Wmissing-declarations 22 | warning-1 += -Wmissing-format-attribute 23 | warning-1 += -Wold-style-definition 24 | warning-1 += $(call cc-option, -Wmissing-include-dirs) 25 | warning-1 += $(call cc-option, -Wunused-but-set-variable) 26 | warning-1 += $(call cc-option, -Wunused-const-variable) 27 | warning-1 += $(call cc-disable-warning, missing-field-initializers) 28 | warning-1 += $(call cc-disable-warning, sign-compare) 29 | 30 | warning-2 := -Waggregate-return 31 | warning-2 += -Wcast-align 32 | warning-2 += -Wdisabled-optimization 33 | warning-2 += -Wnested-externs 34 | warning-2 += -Wshadow 35 | warning-2 += $(call cc-option, -Wlogical-op) 36 | warning-2 += $(call cc-option, -Wmissing-field-initializers) 37 | warning-2 += $(call cc-option, -Wsign-compare) 38 | 39 | warning-3 := -Wbad-function-cast 40 | warning-3 += -Wcast-qual 41 | warning-3 += -Wconversion 42 | warning-3 += -Wpacked 43 | warning-3 += -Wpadded 44 | warning-3 += -Wpointer-arith 45 | warning-3 += -Wredundant-decls 46 | warning-3 += -Wswitch-default 47 | warning-3 += $(call cc-option, -Wpacked-bitfield-compat) 48 | warning-3 += $(call cc-option, -Wvla) 49 | 50 | warning := $(warning-$(findstring 1, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) 51 | warning += $(warning-$(findstring 2, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) 52 | warning += $(warning-$(findstring 3, $(KBUILD_ENABLE_EXTRA_GCC_CHECKS))) 53 | 54 | ifeq ("$(strip $(warning))","") 55 | $(error W=$(KBUILD_ENABLE_EXTRA_GCC_CHECKS) is unknown) 56 | endif 57 | 58 | KBUILD_CFLAGS += $(warning) 59 | else 60 | 61 | ifeq ($(cc-name),clang) 62 | KBUILD_CFLAGS += $(call cc-disable-warning, initializer-overrides) 63 | KBUILD_CFLAGS += $(call cc-disable-warning, unused-value) 64 | KBUILD_CFLAGS += $(call cc-disable-warning, format) 65 | KBUILD_CFLAGS += $(call cc-disable-warning, unknown-warning-option) 66 | KBUILD_CFLAGS += $(call cc-disable-warning, sign-compare) 67 | KBUILD_CFLAGS += $(call cc-disable-warning, format-zero-length) 68 | KBUILD_CFLAGS += $(call cc-disable-warning, uninitialized) 69 | endif 70 | endif 71 | -------------------------------------------------------------------------------- /bolt/scripts/basic/.gitignore: -------------------------------------------------------------------------------- 1 | fixdep 2 | bin2c 3 | -------------------------------------------------------------------------------- /bolt/scripts/basic/Makefile: -------------------------------------------------------------------------------- 1 | ### 2 | # Makefile.basic lists the most basic programs used during the build process. 3 | # The programs listed herein are what are needed to do the basic stuff, 4 | # such as fix file dependencies. 5 | # This initial step is needed to avoid files to be recompiled 6 | # when kernel configuration changes (which is what happens when 7 | # .config is included by main Makefile. 8 | # --------------------------------------------------------------------------- 9 | # fixdep: Used to generate dependency information during build process 10 | 11 | hostprogs-y := fixdep 12 | hostprogs-$(CONFIG_BUILD_BIN2C) += bin2c 13 | always := $(hostprogs-y) 14 | 15 | # fixdep is needed to compile other host programs 16 | $(addprefix $(obj)/,$(filter-out fixdep,$(always))): $(obj)/fixdep 17 | -------------------------------------------------------------------------------- /bolt/scripts/basic/bin2c.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Unloved program to convert a binary on stdin to a C include on stdout 3 | * 4 | * Jan 1999 Matt Mackall 5 | * 6 | * This software may be used and distributed according to the terms 7 | * of the GNU General Public License, incorporated herein by reference. 8 | */ 9 | 10 | #include 11 | 12 | int main(int argc, char *argv[]) 13 | { 14 | int ch, total = 0; 15 | 16 | if (argc > 1) 17 | printf("const char %s[] %s=\n", 18 | argv[1], argc > 2 ? argv[2] : ""); 19 | 20 | do { 21 | printf("\t\""); 22 | while ((ch = getchar()) != EOF) { 23 | total++; 24 | printf("\\x%02x", ch); 25 | if (total % 16 == 0) 26 | break; 27 | } 28 | printf("\"\n"); 29 | } while (ch != EOF); 30 | 31 | if (argc > 1) 32 | printf("\t;\n\nconst int %s_size = %d;\n", argv[1], total); 33 | 34 | return 0; 35 | } 36 | -------------------------------------------------------------------------------- /bolt/scripts/checkincludes.pl: -------------------------------------------------------------------------------- 1 | #!/usr/bin/perl 2 | # 3 | # checkincludes: find/remove files included more than once 4 | # 5 | # Copyright abandoned, 2000, Niels Kristian Bech Jensen . 6 | # Copyright 2009 Luis R. Rodriguez 7 | # 8 | # This script checks for duplicate includes. It also has support 9 | # to remove them in place. Note that this will not take into 10 | # consideration macros so you should run this only if you know 11 | # you do have real dups and do not have them under #ifdef's. You 12 | # could also just review the results. 13 | 14 | use strict; 15 | 16 | sub usage { 17 | print "Usage: checkincludes.pl [-r]\n"; 18 | print "By default we just warn of duplicates\n"; 19 | print "To remove duplicated includes in place use -r\n"; 20 | exit 1; 21 | } 22 | 23 | my $remove = 0; 24 | 25 | if ($#ARGV < 0) { 26 | usage(); 27 | } 28 | 29 | if ($#ARGV >= 1) { 30 | if ($ARGV[0] =~ /^-/) { 31 | if ($ARGV[0] eq "-r") { 32 | $remove = 1; 33 | shift; 34 | } else { 35 | usage(); 36 | } 37 | } 38 | } 39 | 40 | foreach my $file (@ARGV) { 41 | open(my $f, '<', $file) 42 | or die "Cannot open $file: $!.\n"; 43 | 44 | my %includedfiles = (); 45 | my @file_lines = (); 46 | 47 | while (<$f>) { 48 | if (m/^\s*#\s*include\s*[<"](\S*)[>"]/o) { 49 | ++$includedfiles{$1}; 50 | } 51 | push(@file_lines, $_); 52 | } 53 | 54 | close($f); 55 | 56 | if (!$remove) { 57 | foreach my $filename (keys %includedfiles) { 58 | if ($includedfiles{$filename} > 1) { 59 | print "$file: $filename is included more than once.\n"; 60 | } 61 | } 62 | next; 63 | } 64 | 65 | open($f, '>', $file) 66 | or die("Cannot write to $file: $!"); 67 | 68 | my $dups = 0; 69 | foreach (@file_lines) { 70 | if (m/^\s*#\s*include\s*[<"](\S*)[>"]/o) { 71 | foreach my $filename (keys %includedfiles) { 72 | if ($1 eq $filename) { 73 | if ($includedfiles{$filename} > 1) { 74 | $includedfiles{$filename}--; 75 | $dups++; 76 | } else { 77 | print {$f} $_; 78 | } 79 | } 80 | } 81 | } else { 82 | print {$f} $_; 83 | } 84 | } 85 | if ($dups > 0) { 86 | print "$file: removed $dups duplicate includes\n"; 87 | } 88 | close($f); 89 | } 90 | -------------------------------------------------------------------------------- /bolt/scripts/gcc-goto.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Test for gcc 'asm goto' support 3 | # Copyright (C) 2010, Jason Baron 4 | 5 | cat << "END" | $@ -x c - -c -o /dev/null >/dev/null 2>&1 && echo "y" 6 | int main(void) 7 | { 8 | #if defined(__arm__) || defined(__aarch64__) 9 | /* 10 | * Not related to asm goto, but used by jump label 11 | * and broken on some ARM GCC versions (see GCC Bug 48637). 12 | */ 13 | static struct { int dummy; int state; } tp; 14 | asm (".long %c0" :: "i" (&tp.state)); 15 | #endif 16 | 17 | entry: 18 | asm goto ("" :::: entry); 19 | return 0; 20 | } 21 | END 22 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/.gitignore: -------------------------------------------------------------------------------- 1 | # 2 | # Generated files 3 | # 4 | config* 5 | *.lex.c 6 | *.tab.c 7 | *.tab.h 8 | zconf.hash.c 9 | *.moc 10 | gconf.glade.h 11 | *.pot 12 | *.mo 13 | 14 | # 15 | # configuration programs 16 | # 17 | conf 18 | mconf 19 | nconf 20 | qconf 21 | gconf 22 | kxgettext 23 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/POTFILES.in: -------------------------------------------------------------------------------- 1 | scripts/kconfig/lxdialog/checklist.c 2 | scripts/kconfig/lxdialog/inputbox.c 3 | scripts/kconfig/lxdialog/menubox.c 4 | scripts/kconfig/lxdialog/textbox.c 5 | scripts/kconfig/lxdialog/util.c 6 | scripts/kconfig/lxdialog/yesno.c 7 | scripts/kconfig/mconf.c 8 | scripts/kconfig/conf.c 9 | scripts/kconfig/confdata.c 10 | scripts/kconfig/gconf.c 11 | scripts/kconfig/gconf.glade.h 12 | scripts/kconfig/qconf.cc 13 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/check.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Needed for systems without gettext 3 | $* -x c -o /dev/null - > /dev/null 2>&1 << EOF 4 | #include 5 | int main() 6 | { 7 | gettext(""); 8 | return 0; 9 | } 10 | EOF 11 | if [ ! "$?" -eq "0" ]; then 12 | echo -DKBUILD_NO_NLS; 13 | fi 14 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/lkc_proto.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /* confdata.c */ 4 | void conf_parse(const char *name); 5 | int conf_read(const char *name); 6 | int conf_read_simple(const char *name, int); 7 | int conf_write_defconfig(const char *name); 8 | int conf_write(const char *name); 9 | int conf_write_autoconf(void); 10 | bool conf_get_changed(void); 11 | void conf_set_changed_callback(void (*fn)(void)); 12 | void conf_set_message_callback(void (*fn)(const char *fmt, va_list ap)); 13 | 14 | /* menu.c */ 15 | extern struct menu rootmenu; 16 | 17 | bool menu_is_empty(struct menu *menu); 18 | bool menu_is_visible(struct menu *menu); 19 | bool menu_has_prompt(struct menu *menu); 20 | const char * menu_get_prompt(struct menu *menu); 21 | struct menu * menu_get_root_menu(struct menu *menu); 22 | struct menu * menu_get_parent_menu(struct menu *menu); 23 | bool menu_has_help(struct menu *menu); 24 | const char * menu_get_help(struct menu *menu); 25 | struct gstr get_relations_str(struct symbol **sym_arr, struct list_head *head); 26 | void menu_get_ext_help(struct menu *menu, struct gstr *help); 27 | 28 | /* symbol.c */ 29 | extern struct symbol * symbol_hash[SYMBOL_HASHSIZE]; 30 | 31 | struct symbol * sym_lookup(const char *name, int flags); 32 | struct symbol * sym_find(const char *name); 33 | const char * sym_expand_string_value(const char *in); 34 | const char * sym_escape_string_value(const char *in); 35 | struct symbol ** sym_re_search(const char *pattern); 36 | const char * sym_type_name(enum symbol_type type); 37 | void sym_calc_value(struct symbol *sym); 38 | enum symbol_type sym_get_type(struct symbol *sym); 39 | bool sym_tristate_within_range(struct symbol *sym,tristate tri); 40 | bool sym_set_tristate_value(struct symbol *sym,tristate tri); 41 | tristate sym_toggle_tristate_value(struct symbol *sym); 42 | bool sym_string_valid(struct symbol *sym, const char *newval); 43 | bool sym_string_within_range(struct symbol *sym, const char *str); 44 | bool sym_set_string_value(struct symbol *sym, const char *newval); 45 | bool sym_is_changable(struct symbol *sym); 46 | struct property * sym_get_choice_prop(struct symbol *sym); 47 | const char * sym_get_string_value(struct symbol *sym); 48 | 49 | const char * prop_get_type_name(enum prop_type type); 50 | 51 | /* expr.c */ 52 | void expr_print(struct expr *e, void (*fn)(void *, struct symbol *, const char *), void *data, int prevtoken); 53 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/lxdialog/.gitignore: -------------------------------------------------------------------------------- 1 | # 2 | # Generated files 3 | # 4 | lxdialog 5 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/lxdialog/BIG.FAT.WARNING: -------------------------------------------------------------------------------- 1 | This is NOT the official version of dialog. This version has been 2 | significantly modified from the original. It is for use by the Linux 3 | kernel configuration script. Please do not bother Savio Lam with 4 | questions about this program. 5 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/lxdialog/check-lxdialog.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Check ncurses compatibility 3 | 4 | # What library to link 5 | ldflags() 6 | { 7 | pkg-config --libs ncursesw 2>/dev/null && exit 8 | pkg-config --libs ncurses 2>/dev/null && exit 9 | for ext in so a dll.a dylib ; do 10 | for lib in ncursesw ncurses curses ; do 11 | $cc -print-file-name=lib${lib}.${ext} | grep -q / 12 | if [ $? -eq 0 ]; then 13 | echo "-l${lib}" 14 | exit 15 | fi 16 | done 17 | done 18 | exit 1 19 | } 20 | 21 | # Where is ncurses.h? 22 | ccflags() 23 | { 24 | if pkg-config --cflags ncursesw 2>/dev/null; then 25 | echo '-DCURSES_LOC="" -DNCURSES_WIDECHAR=1' 26 | elif pkg-config --cflags ncurses 2>/dev/null; then 27 | echo '-DCURSES_LOC=""' 28 | elif [ -f /usr/include/ncursesw/curses.h ]; then 29 | echo '-I/usr/include/ncursesw -DCURSES_LOC=""' 30 | echo ' -DNCURSES_WIDECHAR=1' 31 | elif [ -f /usr/include/ncurses/ncurses.h ]; then 32 | echo '-I/usr/include/ncurses -DCURSES_LOC=""' 33 | elif [ -f /usr/include/ncurses/curses.h ]; then 34 | echo '-I/usr/include/ncurses -DCURSES_LOC=""' 35 | elif [ -f /usr/include/ncurses.h ]; then 36 | echo '-DCURSES_LOC=""' 37 | else 38 | echo '-DCURSES_LOC=""' 39 | fi 40 | } 41 | 42 | # Temp file, try to clean up after us 43 | tmp=.lxdialog.tmp 44 | trap "rm -f $tmp" 0 1 2 3 15 45 | 46 | # Check if we can link to ncurses 47 | check() { 48 | $cc -x c - -o $tmp 2>/dev/null <<'EOF' 49 | #include CURSES_LOC 50 | main() {} 51 | EOF 52 | if [ $? != 0 ]; then 53 | echo " *** Unable to find the ncurses libraries or the" 1>&2 54 | echo " *** required header files." 1>&2 55 | echo " *** 'make menuconfig' requires the ncurses libraries." 1>&2 56 | echo " *** " 1>&2 57 | echo " *** Install ncurses (ncurses-devel) and try again." 1>&2 58 | echo " *** " 1>&2 59 | exit 1 60 | fi 61 | } 62 | 63 | usage() { 64 | printf "Usage: $0 [-check compiler options|-ccflags|-ldflags compiler options]\n" 65 | } 66 | 67 | if [ $# -eq 0 ]; then 68 | usage 69 | exit 1 70 | fi 71 | 72 | cc="" 73 | case "$1" in 74 | "-check") 75 | shift 76 | cc="$@" 77 | check 78 | ;; 79 | "-ccflags") 80 | ccflags 81 | ;; 82 | "-ldflags") 83 | shift 84 | cc="$@" 85 | ldflags 86 | ;; 87 | "*") 88 | usage 89 | exit 1 90 | ;; 91 | esac 92 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/nconf.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2008 Nir Tzachar 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include "ncurses.h" 28 | 29 | #define max(a, b) ({\ 30 | typeof(a) _a = a;\ 31 | typeof(b) _b = b;\ 32 | _a > _b ? _a : _b; }) 33 | 34 | #define min(a, b) ({\ 35 | typeof(a) _a = a;\ 36 | typeof(b) _b = b;\ 37 | _a < _b ? _a : _b; }) 38 | 39 | typedef enum { 40 | NORMAL = 1, 41 | MAIN_HEADING, 42 | MAIN_MENU_BOX, 43 | MAIN_MENU_FORE, 44 | MAIN_MENU_BACK, 45 | MAIN_MENU_GREY, 46 | MAIN_MENU_HEADING, 47 | SCROLLWIN_TEXT, 48 | SCROLLWIN_HEADING, 49 | SCROLLWIN_BOX, 50 | DIALOG_TEXT, 51 | DIALOG_MENU_FORE, 52 | DIALOG_MENU_BACK, 53 | DIALOG_BOX, 54 | INPUT_BOX, 55 | INPUT_HEADING, 56 | INPUT_TEXT, 57 | INPUT_FIELD, 58 | FUNCTION_TEXT, 59 | FUNCTION_HIGHLIGHT, 60 | ATTR_MAX 61 | } attributes_t; 62 | extern attributes_t attributes[]; 63 | 64 | typedef enum { 65 | F_HELP = 1, 66 | F_SYMBOL = 2, 67 | F_INSTS = 3, 68 | F_CONF = 4, 69 | F_BACK = 5, 70 | F_SAVE = 6, 71 | F_LOAD = 7, 72 | F_SEARCH = 8, 73 | F_EXIT = 9, 74 | } function_key; 75 | 76 | void set_colors(void); 77 | 78 | /* this changes the windows attributes !!! */ 79 | void print_in_middle(WINDOW *win, 80 | int starty, 81 | int startx, 82 | int width, 83 | const char *string, 84 | chtype color); 85 | int get_line_length(const char *line); 86 | int get_line_no(const char *text); 87 | const char *get_line(const char *text, int line_no); 88 | void fill_window(WINDOW *win, const char *text); 89 | int btn_dialog(WINDOW *main_window, const char *msg, int btn_num, ...); 90 | int dialog_inputbox(WINDOW *main_window, 91 | const char *title, const char *prompt, 92 | const char *init, char **resultp, int *result_len); 93 | void refresh_all_windows(WINDOW *main_window); 94 | void show_scroll_win(WINDOW *main_window, 95 | const char *title, 96 | const char *text); 97 | -------------------------------------------------------------------------------- /bolt/scripts/kconfig/zconf.gperf: -------------------------------------------------------------------------------- 1 | %language=ANSI-C 2 | %define hash-function-name kconf_id_hash 3 | %define lookup-function-name kconf_id_lookup 4 | %define string-pool-name kconf_id_strings 5 | %compare-strncmp 6 | %enum 7 | %pic 8 | %struct-type 9 | 10 | struct kconf_id; 11 | 12 | static const struct kconf_id *kconf_id_lookup(const char *str, unsigned int len); 13 | 14 | %% 15 | mainmenu, T_MAINMENU, TF_COMMAND 16 | menu, T_MENU, TF_COMMAND 17 | endmenu, T_ENDMENU, TF_COMMAND 18 | source, T_SOURCE, TF_COMMAND 19 | choice, T_CHOICE, TF_COMMAND 20 | endchoice, T_ENDCHOICE, TF_COMMAND 21 | comment, T_COMMENT, TF_COMMAND 22 | config, T_CONFIG, TF_COMMAND 23 | menuconfig, T_MENUCONFIG, TF_COMMAND 24 | help, T_HELP, TF_COMMAND 25 | ---help---, T_HELP, TF_COMMAND 26 | if, T_IF, TF_COMMAND|TF_PARAM 27 | endif, T_ENDIF, TF_COMMAND 28 | depends, T_DEPENDS, TF_COMMAND 29 | optional, T_OPTIONAL, TF_COMMAND 30 | default, T_DEFAULT, TF_COMMAND, S_UNKNOWN 31 | prompt, T_PROMPT, TF_COMMAND 32 | tristate, T_TYPE, TF_COMMAND, S_TRISTATE 33 | def_tristate, T_DEFAULT, TF_COMMAND, S_TRISTATE 34 | bool, T_TYPE, TF_COMMAND, S_BOOLEAN 35 | boolean, T_TYPE, TF_COMMAND, S_BOOLEAN 36 | def_bool, T_DEFAULT, TF_COMMAND, S_BOOLEAN 37 | int, T_TYPE, TF_COMMAND, S_INT 38 | hex, T_TYPE, TF_COMMAND, S_HEX 39 | string, T_TYPE, TF_COMMAND, S_STRING 40 | select, T_SELECT, TF_COMMAND 41 | range, T_RANGE, TF_COMMAND 42 | visible, T_VISIBLE, TF_COMMAND 43 | option, T_OPTION, TF_COMMAND 44 | on, T_ON, TF_PARAM 45 | modules, T_OPT_MODULES, TF_OPTION 46 | defconfig_list, T_OPT_DEFCONFIG_LIST,TF_OPTION 47 | env, T_OPT_ENV, TF_OPTION 48 | allnoconfig_y, T_OPT_ALLNOCONFIG_Y,TF_OPTION 49 | %% 50 | -------------------------------------------------------------------------------- /bolt/scripts/mkmakefile: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # Generates a small Makefile used in the root of the output 3 | # directory, to allow make to be started from there. 4 | # The Makefile also allow for more convinient build of external modules 5 | 6 | # Usage 7 | # $1 - Kernel src directory 8 | # $2 - Output directory 9 | # $3 - version 10 | # $4 - patchlevel 11 | 12 | 13 | test ! -r $2/Makefile -o -O $2/Makefile || exit 0 14 | # Only overwrite automatically generated Makefiles 15 | # (so we do not overwrite kernel Makefile) 16 | if test -e $2/Makefile && ! grep -q Automatically $2/Makefile 17 | then 18 | exit 0 19 | fi 20 | if [ "${quiet}" != "silent_" ]; then 21 | echo " GEN $2/Makefile" 22 | fi 23 | 24 | cat << EOF > $2/Makefile 25 | # Automatically generated by $0: don't edit 26 | 27 | VERSION = $3 28 | PATCHLEVEL = $4 29 | 30 | lastword = \$(word \$(words \$(1)),\$(1)) 31 | makedir := \$(dir \$(call lastword,\$(MAKEFILE_LIST))) 32 | 33 | ifeq ("\$(origin V)", "command line") 34 | VERBOSE := \$(V) 35 | endif 36 | ifneq (\$(VERBOSE),1) 37 | Q := @ 38 | endif 39 | 40 | MAKEARGS := -C $1 41 | MAKEARGS += O=\$(if \$(patsubst /%,,\$(makedir)),\$(CURDIR)/)\$(patsubst %/,%,\$(makedir)) 42 | 43 | MAKEFLAGS += --no-print-directory 44 | 45 | .PHONY: __sub-make \$(MAKECMDGOALS) 46 | 47 | __sub-make: 48 | \$(Q)\$(MAKE) \$(MAKEARGS) \$(MAKECMDGOALS) 49 | 50 | \$(filter-out __sub-make, \$(MAKECMDGOALS)): __sub-make 51 | @: 52 | EOF 53 | -------------------------------------------------------------------------------- /bolt/src/.gitignore: -------------------------------------------------------------------------------- 1 | system.cpp 2 | -------------------------------------------------------------------------------- /bolt/src/Kbuild.haskell: -------------------------------------------------------------------------------- 1 | obj-y += bosch/ 2 | obj-y += hal/ 3 | obj-y += free_rtos/ 4 | obj-y += stm32f4/ 5 | 6 | obj-y += main.o 7 | obj-y += system.o 8 | obj-y += vapplication.o 9 | 10 | obj-y += copilot_core.o 11 | -------------------------------------------------------------------------------- /bolt/src/Kbuild.standard: -------------------------------------------------------------------------------- 1 | obj-y += bosch/ 2 | obj-y += hal/ 3 | obj-y += free_rtos/ 4 | obj-y += stm32f4/ 5 | 6 | obj-y += main.o 7 | obj-y += system.o 8 | obj-y += vapplication.o 9 | -------------------------------------------------------------------------------- /bolt/src/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = arduino 2 | 3 | FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | BIN = build/$(SKETCH).ino.bin 10 | 11 | $(BIN): $(SKETCH).ino *.h *.hpp *.c *.cpp */*.h */*.hpp Makefile 12 | arduino-cli compile --libraries $(LIBS) --fqbn $(FQBN) --output-dir=./build $(SKETCH).ino 13 | 14 | flash: $(BIN) 15 | dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 16 | 17 | clean: 18 | rm -rf build 19 | 20 | edit: 21 | vim $(SKETCH).ino 22 | 23 | listen: 24 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 25 | 26 | -------------------------------------------------------------------------------- /bolt/src/bosch/Kbuild: -------------------------------------------------------------------------------- 1 | obj-y += bmi088_accel.o 2 | obj-y += bmi088_fifo.o 3 | obj-y += bmi088_gyro.o 4 | #obj-y += bmp3.o 5 | -------------------------------------------------------------------------------- /bolt/src/bosch/bstdr_comm_support.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "bstdr_types.h" 4 | 5 | bstdr_ret_t bstdr_burst_read(uint8_t dev_id, uint8_t reg_addr, 6 | uint8_t *reg_data, uint32_t len); 7 | 8 | bstdr_ret_t bstdr_burst_write(uint8_t dev_id, uint8_t reg_addr, 9 | uint8_t *reg_data, uint32_t len); 10 | -------------------------------------------------------------------------------- /bolt/src/free_rtos/.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | .*cmd 3 | -------------------------------------------------------------------------------- /bolt/src/free_rtos/Kbuild: -------------------------------------------------------------------------------- 1 | obj-y += event_groups.o 2 | obj-y += list.o 3 | obj-y += port_system_dependent.o 4 | obj-y += heap_4.o 5 | obj-y += queue.o 6 | obj-y += tasks.o 7 | obj-y += timers.o 8 | obj-y += stream_buffer.o 9 | -------------------------------------------------------------------------------- /bolt/src/free_rtos/StackMacros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FreeRTOS Kernel V10.4.1 3 | * Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | * this software and associated documentation files (the "Software"), to deal in 7 | * the Software without restriction, including without limitation the rights to 8 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | * the Software, and to permit persons to whom the Software is furnished to do so, 10 | * subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | * 22 | * https://www.FreeRTOS.org 23 | * https://github.com/FreeRTOS 24 | * 25 | */ 26 | 27 | 28 | #ifndef _MSC_VER /* Visual Studio doesn't support #warning. */ 29 | #warning The name of this file has changed to stack_macros.h. Please update your code accordingly. This source file (which has the original name) will be removed in future released. 30 | #endif 31 | 32 | #include "stack_macros.h" 33 | -------------------------------------------------------------------------------- /bolt/src/free_rtos/stdint.readme: -------------------------------------------------------------------------------- 1 | 2 | #ifndef FREERTOS_STDINT 3 | #define FREERTOS_STDINT 4 | 5 | /******************************************************************************* 6 | * THIS IS NOT A FULL stdint.h IMPLEMENTATION - It only contains the definitions 7 | * necessary to build the FreeRTOS code. It is provided to allow FreeRTOS to be 8 | * built using compilers that do not provide their own stdint.h definition. 9 | * 10 | * To use this file: 11 | * 12 | * 1) Copy this file into the directory that contains your FreeRTOSConfig.h 13 | * header file, as that directory will already be in the compilers include 14 | * path. 15 | * 16 | * 2) Rename the copied file stdint.h. 17 | * 18 | */ 19 | 20 | typedef signed char int8_t; 21 | typedef unsigned char uint8_t; 22 | typedef short int16_t; 23 | typedef unsigned short uint16_t; 24 | typedef long int32_t; 25 | typedef unsigned long uint32_t; 26 | 27 | #endif /* FREERTOS_STDINT */ 28 | -------------------------------------------------------------------------------- /bolt/src/hal/Arduino.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This header helps us move toward using the Arduino API. 3 | */ 4 | 5 | // NB: Putting #pragma once here will cause errors 6 | 7 | #pragma once 8 | 9 | #include 10 | #include 11 | 12 | #if !defined(__main) 13 | extern 14 | #endif 15 | SPIClass SPI; 16 | -------------------------------------------------------------------------------- /bolt/src/hal/Kbuild: -------------------------------------------------------------------------------- 1 | obj-y += digital.o 2 | obj-y += exti.o 3 | obj-y += i2cdev.o 4 | obj-y += motors.o 5 | obj-y += nvic.o 6 | obj-y += SPI.o 7 | obj-y += spi2.o 8 | obj-y += time.o 9 | obj-y += uart.o 10 | obj-y += usb.o 11 | obj-y += usb_bsp.o 12 | obj-y += usbd_desc.o 13 | -------------------------------------------------------------------------------- /bolt/src/hal/SPI.h: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2015 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #pragma once 26 | 27 | #include 28 | #include 29 | #include 30 | 31 | /** 32 | * Initialize the SPI. 33 | */ 34 | void spiBeginTransaction(uint16_t baudRatePrescaler); 35 | /* Send the data_tx buffer and receive into the data_rx buffer */ 36 | bool spiExchange(size_t length, const uint8_t *data); 37 | 38 | typedef enum { 39 | 40 | MSBFIRST, 41 | LSBFIRST, 42 | 43 | } spi_endianness_t; 44 | 45 | typedef enum { 46 | 47 | SPI_MODE0, 48 | SPI_MODE1, 49 | SPI_MODE2, 50 | SPI_MODE3, 51 | 52 | } spi_mode_t; 53 | 54 | class SPISettings { 55 | 56 | friend class SPIClass; 57 | 58 | public: 59 | 60 | SPISettings( 61 | const uint32_t rate, 62 | const spi_endianness_t endianness, 63 | const spi_mode_t mode) 64 | { 65 | _rate = rate; 66 | _endianness = endianness; 67 | _mode = mode; 68 | } 69 | 70 | private: 71 | 72 | uint32_t _rate; 73 | spi_endianness_t _endianness; 74 | spi_mode_t _mode; 75 | 76 | }; 77 | 78 | class SPIClass { 79 | 80 | public: 81 | 82 | void begin(void); 83 | 84 | void beginTransaction(const SPISettings & settings); 85 | 86 | void transfer(uint8_t * data, size_t size); 87 | 88 | void endTransaction(void); 89 | }; 90 | -------------------------------------------------------------------------------- /bolt/src/hal/digital.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2015 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * digital.c - Deck-API digital IO implementation 25 | */ 26 | 27 | #include 28 | 29 | #include "gpio.h" 30 | #include "digital.h" 31 | 32 | void pinMode(const uint8_t pin, const uint32_t mode) 33 | { 34 | const auto mapping = GPIOMappings[pin]; 35 | 36 | RCC_AHB1PeriphClockCmd(mapping.periph, ENABLE); 37 | 38 | GPIO_InitTypeDef GPIO_InitStructure = { 39 | 40 | mapping.pin, 41 | mode == OUTPUT ? GPIO_Mode_OUT:GPIO_Mode_IN, 42 | GPIO_Speed_25MHz, 43 | mode == OUTPUT ? GPIO_OType_PP : GPIO_OType_OD, 44 | mode == INPUT_PULLUP ? GPIO_PuPd_UP : GPIO_PuPd_DOWN 45 | }; 46 | 47 | GPIO_Init(mapping.port, &GPIO_InitStructure); 48 | } 49 | 50 | void digitalWrite(const uint8_t pin, const uint32_t val) 51 | { 52 | const GPIO_Mapping_t mapping = GPIOMappings[pin]; 53 | 54 | GPIO_WriteBit(mapping.port, mapping.pin, val ? Bit_SET : Bit_RESET); 55 | } 56 | 57 | int digitalRead(const uint8_t pin) 58 | { 59 | const auto mapping = GPIOMappings[pin]; 60 | 61 | const auto val = GPIO_ReadInputDataBit(mapping.port, mapping.pin); 62 | 63 | return (val == Bit_SET) ? HIGH : LOW; 64 | } 65 | -------------------------------------------------------------------------------- /bolt/src/hal/digital.h: -------------------------------------------------------------------------------- 1 | /* 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2015 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | */ 25 | 26 | #pragma once 27 | 28 | #include 29 | 30 | enum { 31 | LOW, 32 | HIGH 33 | }; 34 | 35 | enum { 36 | INPUT, 37 | OUTPUT, 38 | INPUT_PULLUP, 39 | INPUT_PULLDOWN, 40 | }; 41 | 42 | void pinMode(const uint8_t pin, const uint32_t mode); 43 | 44 | void digitalWrite(const uint8_t pin, const uint32_t val); 45 | 46 | int digitalRead(const uint8_t pin); 47 | -------------------------------------------------------------------------------- /bolt/src/hal/exti.c: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2011-2012 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * exti.c - Unified implementation of the exti interrupts 25 | */ 26 | #include 27 | 28 | #include 29 | 30 | #include "exti.h" 31 | #include "nvicconf.h" 32 | 33 | static bool didInit; 34 | 35 | /* Interruption initialisation */ 36 | void extiInit() 37 | { 38 | static NVIC_InitTypeDef NVIC_InitStructure; 39 | 40 | if (didInit) 41 | return; 42 | 43 | // This is required for the EXTI interrupt configuration since EXTI 44 | // lines are set via the SYSCFG peripheral; eg. 45 | // SYSCFG_EXTILineConfig(EXTI_PortSourceGPIOC, EXTI_PinSource13); 46 | RCC_AHB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 47 | 48 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; 49 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 50 | 51 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI4_PRI; 52 | NVIC_InitStructure.NVIC_IRQChannel = EXTI4_IRQn; 53 | NVIC_Init(&NVIC_InitStructure); 54 | 55 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = EXTI15_10_PRI; 56 | NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn; 57 | NVIC_Init(&NVIC_InitStructure); 58 | 59 | didInit = true; 60 | } 61 | 62 | bool extiTest(void) 63 | { 64 | return didInit; 65 | } 66 | 67 | // NRF interrupt 68 | void __attribute__((used)) EXTI4_IRQHandler(void) 69 | { 70 | NVIC_ClearPendingIRQ(EXTI4_IRQn); 71 | EXTI_ClearITPendingBit(EXTI_Line4); 72 | EXTI4_Callback(); 73 | } 74 | 75 | void __attribute__((weak)) EXTI4_Callback(void) { } 76 | 77 | 78 | // Deck/sensor interrupts 79 | void __attribute__((used)) EXTI15_10_IRQHandler(void) 80 | { 81 | NVIC_ClearPendingIRQ(EXTI15_10_IRQn); 82 | 83 | // Gyro 84 | if (EXTI_GetITStatus(EXTI_Line14) == SET) { 85 | EXTI_ClearITPendingBit(EXTI_Line14); 86 | EXTI14_Callback(); 87 | } 88 | } 89 | 90 | void __attribute__((weak)) EXTI14_Callback(void) { } 91 | -------------------------------------------------------------------------------- /bolt/src/hal/exti.h: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2011-2012 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * exti.h - Unified implementation of the exti interrupts 25 | */ 26 | 27 | #pragma once 28 | 29 | #include 30 | 31 | #ifdef __cplusplus 32 | extern "C" { 33 | #endif 34 | 35 | void extiInit(); 36 | bool extiTest(); 37 | 38 | void EXTI0_Callback(void); 39 | void EXTI1_Callback(void); 40 | void EXTI2_Callback(void); 41 | void EXTI3_Callback(void); 42 | void EXTI4_Callback(void); 43 | void EXTI5_Callback(void); 44 | void EXTI6_Callback(void); 45 | void EXTI7_Callback(void); 46 | void EXTI8_Callback(void); 47 | void EXTI9_Callback(void); 48 | void EXTI10_Callback(void); 49 | void EXTI11_Callback(void); 50 | void EXTI12_Callback(void); 51 | void EXTI13_Callback(void); 52 | void EXTI14_Callback(void); 53 | void EXTI15_Callback(void); 54 | 55 | #ifdef __cplusplus 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /bolt/src/hal/gpio.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | typedef struct { 6 | uint32_t periph; 7 | GPIO_TypeDef* port; 8 | uint16_t pin; 9 | int8_t adcCh; // -1 means no ADC available for this pin 10 | } GPIO_Mapping_t; 11 | 12 | 13 | static const uint32_t LED_PERIPH = RCC_AHB1Periph_GPIOC | RCC_AHB1Periph_GPIOD; 14 | 15 | // Mapping between pin number, real GPIO and ADC channel 16 | static GPIO_Mapping_t GPIOMappings[] = { 17 | 18 | // LEDs 19 | {.periph= LED_PERIPH, .port= GPIOD, .pin=GPIO_Pin_2, 20 | .adcCh=-1}, // LED_BLUE_L 21 | {.periph= LED_PERIPH, .port= GPIOC, .pin=GPIO_Pin_1, 22 | .adcCh=-1}, // LED_GREEN_L 23 | {.periph= LED_PERIPH, .port= GPIOC, .pin=GPIO_Pin_0, 24 | .adcCh=-1}, // LED_RED_L 25 | {.periph= LED_PERIPH, .port= GPIOC, .pin=GPIO_Pin_2, 26 | .adcCh=-1}, // LED_GREEN_R 27 | {.periph= LED_PERIPH, .port= GPIOC, .pin=GPIO_Pin_3, 28 | .adcCh=-1}, // LED_RED_R 29 | 30 | // Decks 31 | {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_11, 32 | .adcCh=-1}, // Deck RX1 33 | {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_10, 34 | .adcCh=-1}, // Deck TX1 35 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_7, 36 | .adcCh=-1}, // Deck SDA 37 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_6, 38 | .adcCh=-1}, // Deck SCL 39 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_8, 40 | .adcCh=-1}, // Deck IO1 41 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_5, 42 | .adcCh=-1}, // Deck IO2 43 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_4, 44 | .adcCh=-1}, // Deck IO3 45 | {.periph= RCC_AHB1Periph_GPIOC, .port= GPIOC, .pin=GPIO_Pin_12, 46 | .adcCh=-1}, // Deck IO4 47 | {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_2, 48 | .adcCh=ADC_Channel_2}, // Deck TX2 49 | {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_3, 50 | .adcCh=ADC_Channel_3}, // Deck RX2 51 | {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_5, 52 | .adcCh=ADC_Channel_5}, // Deck SCK 53 | {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_6, 54 | .adcCh=ADC_Channel_6}, // Deck MISO 55 | {.periph= RCC_AHB1Periph_GPIOA, .port= GPIOA, .pin=GPIO_Pin_7, 56 | .adcCh=ADC_Channel_7}, // Deck MOSI 57 | 58 | // Bolt IMU SPI 59 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_0, 60 | .adcCh=-1}, // Bolt gyro CS 61 | {.periph= RCC_AHB1Periph_GPIOB, .port= GPIOB, .pin=GPIO_Pin_1, 62 | .adcCh=-1}, // Bolt accel CS 63 | 64 | }; 65 | 66 | -------------------------------------------------------------------------------- /bolt/src/hal/i2cdev.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | typedef enum 13 | { 14 | i2cAck, 15 | i2cNack 16 | } I2cStatus; 17 | 18 | typedef enum 19 | { 20 | i2cWrite, 21 | i2cRead 22 | } I2cDirection; 23 | 24 | 25 | typedef struct _I2cMessage 26 | { 27 | uint32_t messageLength; 28 | uint8_t slaveAddress; 29 | uint8_t nbrOfRetries; 30 | I2cDirection direction; 31 | I2cStatus status; 32 | QueueHandle_t clientQueue; 33 | bool isInternal16bit; 34 | uint16_t internalAddress; 35 | uint8_t *buffer; 36 | } I2cMessage; 37 | 38 | typedef struct 39 | { 40 | I2C_TypeDef* i2cPort; 41 | uint32_t i2cPerif; 42 | uint32_t i2cEVIRQn; 43 | uint32_t i2cERIRQn; 44 | uint32_t i2cClockSpeed; 45 | uint32_t gpioSCLPerif; 46 | GPIO_TypeDef* gpioSCLPort; 47 | uint32_t gpioSCLPin; 48 | uint32_t gpioSCLPinSource; 49 | uint32_t gpioSDAPerif; 50 | GPIO_TypeDef* gpioSDAPort; 51 | uint32_t gpioSDAPin; 52 | uint32_t gpioSDAPinSource; 53 | uint32_t gpioAF; 54 | uint32_t dmaPerif; 55 | uint32_t dmaChannel; 56 | DMA_Stream_TypeDef* dmaRxStream; 57 | uint32_t dmaRxIRQ; 58 | uint32_t dmaRxTCFlag; 59 | uint32_t dmaRxTEFlag; 60 | 61 | } I2cDef; 62 | 63 | typedef struct 64 | { 65 | const I2cDef *def; 66 | I2cMessage txMessage; 67 | uint32_t messageIndex; 68 | uint32_t nbrOfretries; 69 | SemaphoreHandle_t isBusFreeSemaphore; 70 | StaticSemaphore_t isBusFreeSemaphoreBuffer; 71 | SemaphoreHandle_t isBusFreeMutex; 72 | StaticSemaphore_t isBusFreeMutexBuffer; 73 | DMA_InitTypeDef DMAStruct; 74 | 75 | } I2C_Dev; 76 | 77 | extern I2C_Dev deckBus; 78 | 79 | #ifdef __cplusplus 80 | extern "C" { 81 | #endif 82 | 83 | int i2cdevInit(); 84 | 85 | bool i2cdevReadReg16(I2C_Dev *dev, uint8_t devAddress, uint16_t memAddress, 86 | uint16_t len, uint8_t *data); 87 | 88 | bool i2cdevWriteReg16(I2C_Dev *dev, uint8_t devAddress, uint16_t memAddress, 89 | uint16_t len, const uint8_t *data); 90 | 91 | #ifdef __cplusplus 92 | } 93 | #endif 94 | -------------------------------------------------------------------------------- /bolt/src/hal/interrupt.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | 5 | void resetInterrupts(void) 6 | { 7 | NVIC_SystemReset(); 8 | } 9 | -------------------------------------------------------------------------------- /bolt/src/hal/interrupt.h: -------------------------------------------------------------------------------- 1 | 2 | #ifdef __cplusplus 3 | extern "C" { 4 | #endif 5 | 6 | void resetInterrupts(void); 7 | 8 | #ifdef __cplusplus 9 | } 10 | #endif 11 | -------------------------------------------------------------------------------- /bolt/src/hal/nvic.h: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2011-2012 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * nvic.h - Interrupt vector header file 25 | */ 26 | 27 | #pragma once 28 | 29 | #ifdef __cplusplus 30 | extern "C" { 31 | #endif 32 | 33 | /** 34 | * Setup and initialize the NVIC 35 | */ 36 | void nvicInit(void); 37 | 38 | #ifdef __cplusplus 39 | } 40 | #endif 41 | -------------------------------------------------------------------------------- /bolt/src/hal/spi2.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | void spi2_send_byte(uint8_t byte); 6 | 7 | void spi2_dma_read(uint8_t reg_addr, uint8_t *reg_data, uint16_t len); 8 | 9 | void spi2_begin(void); 10 | 11 | -------------------------------------------------------------------------------- /bolt/src/hal/time.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "time.h" 12 | 13 | static bool didInit = false; 14 | static uint32_t usecTimerHighCount; 15 | 16 | void usecTimerInit(void) 17 | { 18 | if (didInit) { 19 | return; 20 | } 21 | 22 | usecTimerHighCount = 0; 23 | 24 | TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; 25 | NVIC_InitTypeDef NVIC_InitStructure; 26 | 27 | //Enable the Timer 28 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM7, ENABLE); 29 | 30 | //Timer configuration 31 | TIM_TimeBaseStructure.TIM_Period = 0xFFFF; 32 | // APB1 clock is /4, but APB clock inputs to timers are doubled when 33 | // the APB clock runs slower than /1, so APB1 timers see a /2 clock 34 | TIM_TimeBaseStructure.TIM_Prescaler = SystemCoreClock / (1000 * 1000) / 2; 35 | TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 36 | TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; 37 | TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; 38 | TIM_TimeBaseInit(TIM7, &TIM_TimeBaseStructure); 39 | 40 | NVIC_InitStructure.NVIC_IRQChannel = TIM7_IRQn; 41 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_TRACE_TIM_PRI; 42 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; 43 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 44 | NVIC_Init(&NVIC_InitStructure); 45 | 46 | DBGMCU_APB1PeriphConfig(DBGMCU_TIM7_STOP, ENABLE); 47 | TIM_ITConfig(TIM7, TIM_IT_Update, ENABLE); 48 | TIM_Cmd(TIM7, ENABLE); 49 | 50 | didInit = true; 51 | } 52 | 53 | uint64_t micros(void) 54 | { 55 | uint32_t high0; 56 | __atomic_load(&usecTimerHighCount, &high0, __ATOMIC_SEQ_CST); 57 | uint32_t low = TIM7->CNT; 58 | uint32_t high; 59 | __atomic_load(&usecTimerHighCount, &high, __ATOMIC_SEQ_CST); 60 | 61 | // There was no increment in between 62 | if (high == high0) 63 | { 64 | return (((uint64_t)high) << 16) + low; 65 | } 66 | // There was an increment, but we don't expect another one soon 67 | return (((uint64_t)high) << 16) + TIM7->CNT; 68 | } 69 | 70 | uint32_t millis() 71 | { 72 | return micros() / 1000; 73 | } 74 | 75 | void delay(const uint32_t msec) 76 | { 77 | vTaskDelay(M2T(msec)); 78 | } 79 | 80 | void delayMicroseconds(const uint32_t usec) 81 | { 82 | uint64_t start = micros(); 83 | 84 | while ((start+usec) > micros()) { 85 | } 86 | } 87 | void __attribute__((used)) TIM7_IRQHandler(void) 88 | { 89 | TIM_ClearITPendingBit(TIM7, TIM_IT_Update); 90 | 91 | __sync_fetch_and_add(&usecTimerHighCount, 1); 92 | } 93 | -------------------------------------------------------------------------------- /bolt/src/hal/time.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | void usecTimerInit(void); 10 | 11 | void delay(const uint32_t msec); 12 | 13 | void delayMicroseconds(const uint32_t usec); 14 | 15 | uint64_t micros(void); 16 | 17 | uint32_t millis(void); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | -------------------------------------------------------------------------------- /bolt/src/hal/usb.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | static const uint8_t USB_RX_TX_PACKET_SIZE = 64; 4 | 5 | /* Structure used for in/out data via USB */ 6 | typedef struct 7 | { 8 | uint8_t size; 9 | uint8_t data[USB_RX_TX_PACKET_SIZE]; 10 | } USBPacket; 11 | 12 | 13 | #ifdef __cplusplus 14 | extern "C" { 15 | #endif 16 | 17 | void usbInit(void); 18 | 19 | #ifdef __cplusplus 20 | } 21 | #endif 22 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/Kbuild: -------------------------------------------------------------------------------- 1 | obj-y += startup_stm32f40xx.o 2 | obj-y += system_stm32f4xx.o 3 | 4 | # STM32F4xx_StdPeriph_Driver 5 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_adc.o 6 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dbgmcu.o 7 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma.o 8 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_exti.o 9 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.o 10 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_gpio.o 11 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_i2c.o 12 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_iwdg.o 13 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_misc.o 14 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.o 15 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_spi.o 16 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_syscfg.o 17 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.o 18 | obj-y += STM32F4xx_StdPeriph_Driver/src/stm32f4xx_usart.o 19 | 20 | # STM32_USB_Device_Library 21 | obj-y += STM32_USB_Device_Library/Core/src/usbd_core.o 22 | obj-y += STM32_USB_Device_Library/Core/src/usbd_ioreq.o 23 | obj-y += STM32_USB_Device_Library/Core/src/usbd_req.o 24 | 25 | # STM32_USB_OTG_Driver 26 | obj-y += STM32_USB_OTG_Driver/src/usb_core.o 27 | obj-y += STM32_USB_OTG_Driver/src/usb_dcd_int.o 28 | obj-y += STM32_USB_OTG_Driver/src/usb_dcd.o 29 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32F4xx_StdPeriph_Driver/Release_Notes.html: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/bolt/src/stm32f4/STM32F4xx_StdPeriph_Driver/Release_Notes.html -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_crc.h 4 | * @author MCD Application Team 5 | * @version V1.8.0 6 | * @date 04-November-2016 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2016 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_CRC_H 31 | #define __STM32F4xx_CRC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup CRC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup CRC_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /* Exported macro ------------------------------------------------------------*/ 60 | /* Exported functions --------------------------------------------------------*/ 61 | 62 | void CRC_ResetDR(void); 63 | uint32_t CRC_CalcCRC(uint32_t Data); 64 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 65 | uint32_t CRC_GetCRC(void); 66 | void CRC_SetIDRegister(uint8_t IDValue); 67 | uint8_t CRC_GetIDRegister(void); 68 | 69 | #ifdef __cplusplus 70 | } 71 | #endif 72 | 73 | #endif /* __STM32F4xx_CRC_H */ 74 | 75 | /** 76 | * @} 77 | */ 78 | 79 | /** 80 | * @} 81 | */ 82 | 83 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 84 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_qspi.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/bolt/src/stm32f4/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_qspi.c -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_Device_Library/Core/inc/usbd_conf_template.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_conf_template.h 4 | * @author MCD Application Team 5 | * @version V1.2.1 6 | * @date 30-June-2015 7 | * @brief usb device configuration template file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USBD_CONF__H__ 24 | #define __USBD_CONF__H__ 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | #include "usb_conf.h" 28 | 29 | /** @defgroup USB_CONF_Exported_Defines 30 | * @{ 31 | */ 32 | #define USE_USB_OTG_HS 33 | 34 | #define USBD_CFG_MAX_NUM 1 35 | #define USB_MAX_STR_DESC_SIZ 64 36 | #define USBD_EP0_MAX_PACKET_SIZE 64 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | 43 | /** @defgroup USB_CONF_Exported_Types 44 | * @{ 45 | */ 46 | /** 47 | * @} 48 | */ 49 | 50 | 51 | /** @defgroup USB_CONF_Exported_Macros 52 | * @{ 53 | */ 54 | /** 55 | * @} 56 | */ 57 | 58 | /** @defgroup USB_CONF_Exported_Variables 59 | * @{ 60 | */ 61 | /** 62 | * @} 63 | */ 64 | 65 | /** @defgroup USB_CONF_Exported_FunctionsPrototype 66 | * @{ 67 | */ 68 | /** 69 | * @} 70 | */ 71 | 72 | 73 | #endif //__USBD_CONF__H__ 74 | 75 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 76 | 77 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_Device_Library/Core/inc/usbd_core.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_core.h 4 | * @author MCD Application Team 5 | * @version V1.2.1 6 | * @date 17-March-2018 7 | * @brief Header file for usbd_core.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USBD_CORE_H 24 | #define __USBD_CORE_H 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | #include "usb_dcd.h" 28 | #include "usbd_def.h" 29 | #include "usbd_conf.h" 30 | 31 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 32 | * @{ 33 | */ 34 | 35 | /** @defgroup USBD_CORE 36 | * @brief This file is the Header file for usbd_core.c file 37 | * @{ 38 | */ 39 | 40 | 41 | /** @defgroup USBD_CORE_Exported_Defines 42 | * @{ 43 | */ 44 | 45 | typedef enum { 46 | USBD_OK = 0, 47 | USBD_BUSY, 48 | USBD_FAIL, 49 | }USBD_Status; 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @defgroup USBD_CORE_Exported_TypesDefinitions 56 | * @{ 57 | */ 58 | 59 | 60 | /** 61 | * @} 62 | */ 63 | 64 | 65 | 66 | /** @defgroup USBD_CORE_Exported_Macros 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup USBD_CORE_Exported_Variables 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @defgroup USBD_CORE_Exported_FunctionsPrototype 83 | * @{ 84 | */ 85 | void USBD_Init(USB_OTG_CORE_HANDLE *pdev, 86 | USB_OTG_CORE_ID_TypeDef coreID, 87 | USBD_DEVICE *pDevice, 88 | USBD_Class_cb_TypeDef *class_cb, 89 | USBD_Usr_cb_TypeDef *usr_cb); 90 | 91 | USBD_Status USBD_DeInit(USB_OTG_CORE_HANDLE *pdev); 92 | 93 | USBD_Status USBD_ClrCfg(USB_OTG_CORE_HANDLE *pdev, uint8_t cfgidx); 94 | 95 | USBD_Status USBD_SetCfg(USB_OTG_CORE_HANDLE *pdev, uint8_t cfgidx); 96 | 97 | /** 98 | * @} 99 | */ 100 | 101 | #endif /* __USBD_CORE_H */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** 108 | * @} 109 | */ 110 | 111 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_Device_Library/Core/inc/usbd_req.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_req.h 4 | * @author MCD Application Team 5 | * @version V1.2.1 6 | * @date 17-March-2018 7 | * @brief header file for the usbd_req.c file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | 24 | #ifndef __USB_REQUEST_H_ 25 | #define __USB_REQUEST_H_ 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "usbd_def.h" 29 | #include "usbd_core.h" 30 | #include "usbd_conf.h" 31 | 32 | 33 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 34 | * @{ 35 | */ 36 | 37 | /** @defgroup USBD_REQ 38 | * @brief header file for the usbd_ioreq.c file 39 | * @{ 40 | */ 41 | 42 | /** @defgroup USBD_REQ_Exported_Defines 43 | * @{ 44 | */ 45 | /** 46 | * @} 47 | */ 48 | 49 | 50 | /** @defgroup USBD_REQ_Exported_Types 51 | * @{ 52 | */ 53 | /** 54 | * @} 55 | */ 56 | 57 | 58 | 59 | /** @defgroup USBD_REQ_Exported_Macros 60 | * @{ 61 | */ 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup USBD_REQ_Exported_Variables 67 | * @{ 68 | */ 69 | /** 70 | * @} 71 | */ 72 | 73 | /** @defgroup USBD_REQ_Exported_FunctionsPrototype 74 | * @{ 75 | */ 76 | 77 | USBD_Status USBD_StdDevReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 78 | USBD_Status USBD_StdItfReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 79 | USBD_Status USBD_StdEPReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 80 | void USBD_ParseSetupRequest( USB_OTG_CORE_HANDLE *pdev, 81 | USB_SETUP_REQ *req); 82 | 83 | void USBD_CtlError( USB_OTG_CORE_HANDLE *pdev, 84 | USB_SETUP_REQ *req); 85 | 86 | void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len); 87 | /** 88 | * @} 89 | */ 90 | 91 | #endif /* __USB_REQUEST_H_ */ 92 | 93 | /** 94 | * @} 95 | */ 96 | 97 | /** 98 | * @} 99 | */ 100 | 101 | 102 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 103 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_OTG_Driver/inc/usb_bsp.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_bsp.h 4 | * @author MCD Application Team 5 | * @version V2.2.1 6 | * @date 17-March-2018 7 | * @brief Specific api's relative to the used hardware platform 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USB_BSP__H__ 24 | #define __USB_BSP__H__ 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | #include "usb_core.h" 28 | 29 | /** @addtogroup USB_OTG_DRIVER 30 | * @{ 31 | */ 32 | 33 | /** @defgroup USB_BSP 34 | * @brief This file is the 35 | * @{ 36 | */ 37 | 38 | 39 | /** @defgroup USB_BSP_Exported_Defines 40 | * @{ 41 | */ 42 | /** 43 | * @} 44 | */ 45 | 46 | 47 | /** @defgroup USB_BSP_Exported_Types 48 | * @{ 49 | */ 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @defgroup USB_BSP_Exported_Macros 56 | * @{ 57 | */ 58 | /** 59 | * @} 60 | */ 61 | 62 | /** @defgroup USB_BSP_Exported_Variables 63 | * @{ 64 | */ 65 | /** 66 | * @} 67 | */ 68 | 69 | /** @defgroup USB_BSP_Exported_FunctionsPrototype 70 | * @{ 71 | */ 72 | void BSP_Init(void); 73 | 74 | void USB_OTG_BSP_Init (USB_OTG_CORE_HANDLE *pdev); 75 | void USB_OTG_BSP_uDelay (const uint32_t usec); 76 | void USB_OTG_BSP_mDelay (const uint32_t msec); 77 | void USB_OTG_BSP_EnableInterrupt (USB_OTG_CORE_HANDLE *pdev); 78 | void USB_OTG_BSP_TimerIRQ (void); 79 | #ifdef USE_HOST_MODE 80 | void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev); 81 | void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state); 82 | void USB_OTG_BSP_Resume(USB_OTG_CORE_HANDLE *pdev) ; 83 | void USB_OTG_BSP_Suspend(USB_OTG_CORE_HANDLE *pdev); 84 | 85 | #endif /* USE_HOST_MODE */ 86 | /** 87 | * @} 88 | */ 89 | 90 | #endif /* __USB_BSP__H__ */ 91 | 92 | /** 93 | * @} 94 | */ 95 | 96 | /** 97 | * @} 98 | */ 99 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 100 | 101 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_OTG_Driver/inc/usb_hcd.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_hcd.h 4 | * @author MCD Application Team 5 | * @version V2.2.1 6 | * @date 17-March-2018 7 | * @brief Host layer Header file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USB_HCD_H__ 24 | #define __USB_HCD_H__ 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | #include "usb_regs.h" 28 | #include "usb_core.h" 29 | 30 | 31 | /** @addtogroup USB_OTG_DRIVER 32 | * @{ 33 | */ 34 | 35 | /** @defgroup USB_HCD 36 | * @brief This file is the 37 | * @{ 38 | */ 39 | 40 | 41 | /** @defgroup USB_HCD_Exported_Defines 42 | * @{ 43 | */ 44 | /** 45 | * @} 46 | */ 47 | 48 | 49 | /** @defgroup USB_HCD_Exported_Types 50 | * @{ 51 | */ 52 | /** 53 | * @} 54 | */ 55 | 56 | 57 | /** @defgroup USB_HCD_Exported_Macros 58 | * @{ 59 | */ 60 | /** 61 | * @} 62 | */ 63 | 64 | /** @defgroup USB_HCD_Exported_Variables 65 | * @{ 66 | */ 67 | /** 68 | * @} 69 | */ 70 | 71 | /** @defgroup USB_HCD_Exported_FunctionsPrototype 72 | * @{ 73 | */ 74 | uint32_t HCD_Init (USB_OTG_CORE_HANDLE *pdev , 75 | USB_OTG_CORE_ID_TypeDef coreID); 76 | uint32_t HCD_HC_Init (USB_OTG_CORE_HANDLE *pdev , 77 | uint8_t hc_num); 78 | uint32_t HCD_SubmitRequest (USB_OTG_CORE_HANDLE *pdev , 79 | uint8_t hc_num) ; 80 | uint32_t HCD_GetCurrentSpeed (USB_OTG_CORE_HANDLE *pdev); 81 | uint32_t HCD_ResetPort (USB_OTG_CORE_HANDLE *pdev); 82 | uint32_t HCD_IsDeviceConnected (USB_OTG_CORE_HANDLE *pdev); 83 | uint32_t HCD_IsPortEnabled (USB_OTG_CORE_HANDLE *pdev); 84 | 85 | uint32_t HCD_GetCurrentFrame (USB_OTG_CORE_HANDLE *pdev) ; 86 | URB_STATE HCD_GetURB_State (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num); 87 | uint32_t HCD_GetXferCnt (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num); 88 | HC_STATUS HCD_GetHCState (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num) ; 89 | /** 90 | * @} 91 | */ 92 | 93 | #endif //__USB_HCD_H__ 94 | 95 | 96 | /** 97 | * @} 98 | */ 99 | 100 | /** 101 | * @} 102 | */ 103 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 104 | 105 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/STM32_USB_OTG_Driver/inc/usb_otg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_otg.h 4 | * @author MCD Application Team 5 | * @version V2.2.1 6 | * @date 17-March-2018 7 | * @brief OTG Core Header 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© Copyright (c) 2015 STMicroelectronics. 12 | * All rights reserved.

13 | * 14 | * This software component is licensed by ST under Ultimate Liberty license 15 | * SLA0044, the "License"; You may not use this file except in compliance with 16 | * the License. You may obtain a copy of the License at: 17 | * 18 | * 19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USB_OTG__ 24 | #define __USB_OTG__ 25 | 26 | 27 | /** @addtogroup USB_OTG_DRIVER 28 | * @{ 29 | */ 30 | 31 | /** @defgroup USB_OTG 32 | * @brief This file is the 33 | * @{ 34 | */ 35 | 36 | 37 | /** @defgroup USB_OTG_Exported_Defines 38 | * @{ 39 | */ 40 | 41 | 42 | void USB_OTG_InitiateSRP(void); 43 | void USB_OTG_InitiateHNP(uint8_t state , uint8_t mode); 44 | void USB_OTG_Switchback (USB_OTG_CORE_DEVICE *pdev); 45 | uint32_t USB_OTG_GetCurrentState (USB_OTG_CORE_DEVICE *pdev); 46 | 47 | /** 48 | * @} 49 | */ 50 | 51 | 52 | /** @defgroup USB_OTG_Exported_Types 53 | * @{ 54 | */ 55 | /** 56 | * @} 57 | */ 58 | 59 | 60 | /** @defgroup USB_OTG_Exported_Macros 61 | * @{ 62 | */ 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @defgroup USB_OTG_Exported_Variables 68 | * @{ 69 | */ 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup USB_OTG_Exported_FunctionsPrototype 75 | * @{ 76 | */ 77 | /** 78 | * @} 79 | */ 80 | 81 | 82 | #endif //__USB_OTG__ 83 | 84 | 85 | /** 86 | * @} 87 | */ 88 | 89 | /** 90 | * @} 91 | */ 92 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 93 | 94 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/stm32f4xx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/bolt/src/stm32f4/stm32f4xx.h -------------------------------------------------------------------------------- /bolt/src/stm32f4/stm32fxxx.h: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2011-2012 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * stm32fxxx.h - Includes correct stm32 include file. 25 | */ 26 | 27 | #pragma once 28 | 29 | #if defined (STM32F40_41xxx) 30 | #include "stm32f4xx.h" 31 | #elif defined (STM32F10X_MD) 32 | #include "stm32f10x.h" 33 | #else 34 | #warning "Don't know which stm32fxxx header file to include" 35 | #endif 36 | -------------------------------------------------------------------------------- /bolt/src/stm32f4/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @version V1.8.0 6 | * @date 09-November-2016 7 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2016 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /** @addtogroup CMSIS 29 | * @{ 30 | */ 31 | 32 | /** @addtogroup stm32f4xx_system 33 | * @{ 34 | */ 35 | 36 | /** 37 | * @brief Define to prevent recursive inclusion 38 | */ 39 | #ifndef __SYSTEM_STM32F4XX_H 40 | #define __SYSTEM_STM32F4XX_H 41 | 42 | #ifdef __cplusplus 43 | extern "C" { 44 | #endif 45 | 46 | /** @addtogroup STM32F4xx_System_Includes 47 | * @{ 48 | */ 49 | 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @addtogroup STM32F4xx_System_Exported_types 56 | * @{ 57 | */ 58 | 59 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 60 | 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @addtogroup STM32F4xx_System_Exported_Constants 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @addtogroup STM32F4xx_System_Exported_Macros 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @addtogroup STM32F4xx_System_Exported_Functions 83 | * @{ 84 | */ 85 | 86 | extern void SystemInit(void); 87 | extern void SystemCoreClockUpdate(void); 88 | /** 89 | * @} 90 | */ 91 | 92 | #ifdef __cplusplus 93 | } 94 | #endif 95 | 96 | #endif /*__SYSTEM_STM32F4XX_H */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** 103 | * @} 104 | */ 105 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 106 | -------------------------------------------------------------------------------- /bolt/src/vapplication.c: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | static void fail(const char * msg) 25 | { 26 | portDISABLE_INTERRUPTS(); 27 | motorsStop(); 28 | while (true); 29 | } 30 | 31 | void vApplicationIdleHook( void ) 32 | { 33 | // Enter sleep mode. Currently saves about 20mA STM32F405 current 34 | // consumption (~30%). 35 | { __asm volatile ("wfi"); } 36 | } 37 | 38 | void vApplicationMallocFailedHook( void ) 39 | { 40 | fail("Malloc failed"); 41 | } 42 | 43 | void vApplicationStackOverflowHook(TaskHandle_t xTask, char * pcTaskName) 44 | { 45 | fail("Stack overflow"); 46 | } 47 | 48 | /** 49 | * @brief configSUPPORT_STATIC_ALLOCATION is set to 1, so the application 50 | * must provide an implementation of vApplicationGetIdleTaskMemory() to 51 | * provide the memory that is 52 | * used by the Idle task. 53 | */ 54 | void vApplicationGetIdleTaskMemory( StaticTask_t **ppxIdleTaskTCBBuffer, 55 | StackType_t **ppxIdleTaskStackBuffer, 56 | uint32_t *pulIdleTaskStackSize ) 57 | { 58 | static StaticTask_t xIdleTaskTCB; 59 | static StackType_t uxIdleTaskStack[ configMINIMAL_STACK_SIZE ]; 60 | 61 | *ppxIdleTaskTCBBuffer = &xIdleTaskTCB; 62 | *ppxIdleTaskStackBuffer = uxIdleTaskStack; 63 | *pulIdleTaskStackSize = configMINIMAL_STACK_SIZE; 64 | } 65 | /*———————————————————–*/ 66 | 67 | /** 68 | * @brief configSUPPORT_STATIC_ALLOCATION and configUSE_TIMERS are both set to 1, so the 69 | * application must provide an implementation of vApplicationGetTimerTaskMemory() 70 | * to provide the memory that is used by the Timer service task. 71 | */ 72 | void vApplicationGetTimerTaskMemory( StaticTask_t **ppxTimerTaskTCBBuffer, 73 | StackType_t **ppxTimerTaskStackBuffer, 74 | uint32_t *pulTimerTaskStackSize ) 75 | { 76 | static StaticTask_t xTimerTaskTCB; 77 | static StackType_t uxTimerTaskStack[ configTIMER_TASK_STACK_DEPTH ]; 78 | 79 | *ppxTimerTaskTCBBuffer = &xTimerTaskTCB; 80 | *ppxTimerTaskStackBuffer = uxTimerTaskStack; 81 | *pulTimerTaskStackSize = configTIMER_TASK_STACK_DEPTH; 82 | } 83 | 84 | void debugSendTraceInfo(unsigned int taskNbr) 85 | { 86 | } 87 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/linker/DEF.ld: -------------------------------------------------------------------------------- 1 | /* 2 | Linker subscript for STM32F103 definitions with 32K Flash and 20K RAM 3 | Copyright RAISONANCE 2007-2009 4 | !!! This file is automatically generated by Ride !!! 5 | Do not modify it, as it will be erased at every link. 6 | You can use, copy and distribute this file freely, but without any waranty. 7 | */ 8 | 9 | /* Memory Spaces Definitions */ 10 | 11 | MEMORY 12 | { 13 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K 14 | CCMRAM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K 15 | FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K 16 | FLASHPATCH (r) : ORIGIN = 0x00000000, LENGTH = 0 17 | ENDFLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0 18 | FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 19 | EXTMEMB0 (rx) : ORIGIN = 0x00000000, LENGTH = 0 20 | EXTMEMB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 21 | EXTMEMB2 (rx) : ORIGIN = 0x00000000, LENGTH = 0 22 | EXTMEMB3 (rx) : ORIGIN = 0x00000000, LENGTH = 0 23 | } 24 | 25 | /* higher address of the user mode stack */ 26 | _estack = 0x2001FFFF; 27 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/linker/DEF_CLOAD.ld: -------------------------------------------------------------------------------- 1 | /* 2 | Linker subscript for STM32F405 definitions with 1M Flash and 128K RAM 3 | Copyright RAISONANCE 2007-2009 4 | !!! This file is automatically generated by Ride !!! 5 | Do not modify it, as it will be erased at every link. 6 | You can use, copy and distribute this file freely, but without any waranty. 7 | */ 8 | 9 | /* Memory Spaces Definitions */ 10 | 11 | MEMORY 12 | { 13 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 128K 14 | CCMRAM (xrw) : ORIGIN = 0x10000000, LENGTH = 64K 15 | FLASH (rx) : ORIGIN = 0x8004000, LENGTH = 1008K 16 | FLASHPATCH (r) : ORIGIN = 0x00000000, LENGTH = 0 17 | ENDFLASH (rx) : ORIGIN = 0x00000000, LENGTH = 0 18 | FLASHB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 19 | EXTMEMB0 (rx) : ORIGIN = 0x00000000, LENGTH = 0 20 | EXTMEMB1 (rx) : ORIGIN = 0x00000000, LENGTH = 0 21 | EXTMEMB2 (rx) : ORIGIN = 0x00000000, LENGTH = 0 22 | EXTMEMB3 (rx) : ORIGIN = 0x00000000, LENGTH = 0 23 | } 24 | 25 | /* higher address of the user mode stack */ 26 | _estack = 0x2001FFFF; 27 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/linker/FLASH.ld: -------------------------------------------------------------------------------- 1 | /* 2 | Default secondary/main linker script for STM32F103_32K_20K 3 | Copyright RAISONANCE S.A.S. 2007-2009 4 | 5 | !!! This file is automatically generated by Ride if the default linker script option is active !!! 6 | 7 | Do not modify it if the option is set, as it will be erased at every link. 8 | If you need to use your own script please configure your Ride7 project linker options to use your custom linker script 9 | You can take this one as example for wrting your custom linker script 10 | You can use, copy and distribute this file freely, but without any waranty.*/ 11 | 12 | SEARCH_DIR("./tools/make/F405/linker") 13 | 14 | /* include the common STMxxx sub-script */ 15 | INCLUDE "COMMON.ld" 16 | 17 | /* include the memory spaces definitions sub-script */ 18 | INCLUDE "DEF.ld" 19 | 20 | /* include the sections management sub-script for FLASH mode */ 21 | INCLUDE "sections_FLASH.ld" 22 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/linker/FLASH_CLOAD.ld: -------------------------------------------------------------------------------- 1 | /* 2 | Default secondary/main linker script for STM32F103_32K_20K 3 | Copyright RAISONANCE S.A.S. 2007-2009 4 | 5 | !!! This file is automatically generated by Ride if the default linker script option is active !!! 6 | 7 | Do not modify it if the option is set, as it will be erased at every link. 8 | If you need to use your own script please configure your Ride7 project linker options to use your custom linker script 9 | You can take this one as example for wrting your custom linker script 10 | You can use, copy and distribute this file freely, but without any waranty.*/ 11 | 12 | SEARCH_DIR("./tools/make/F405/linker") 13 | 14 | /* include the common STMxxx sub-script */ 15 | INCLUDE "COMMON.ld" 16 | 17 | /* include the memory spaces definitions sub-script */ 18 | INCLUDE "DEF_CLOAD.ld" 19 | 20 | /* include the sections management sub-script for FLASH mode */ 21 | INCLUDE "sections_FLASH.ld" 22 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/st_obj.mk: -------------------------------------------------------------------------------- 1 | # st_obj.mk - Selection of the ST library objects to compile 2 | # This file is part of the Crazy Flie control program 3 | # Copyright (c) 2009, EAT-IT 4 | 5 | VPATH+=$(LIB)/STM32F4xx_StdPeriph_Driver/src/ 6 | ST_OBJ= 7 | #ST_OBJ+=misc.o 8 | ST_OBJ+=stm32f4xx_adc.o 9 | #ST_OBJ+=stm32f4xx_bkp.o 10 | #ST_OBJ+=stm32f4xx_can.o 11 | #ST_OBJ+=stm32f4xx_crc.o 12 | #ST_OBJ+=stm32f4xx_dac.o 13 | ST_OBJ+=stm32f4xx_dbgmcu.o 14 | ST_OBJ+=stm32f4xx_dma.o 15 | ST_OBJ+=stm32f4xx_exti.o 16 | ST_OBJ+=stm32f4xx_flash.o 17 | #ST_OBJ+=stm32f4xx_fsmc.o 18 | ST_OBJ+=stm32f4xx_gpio.o 19 | ST_OBJ+=stm32f4xx_i2c.o 20 | ST_OBJ+=stm32f4xx_iwdg.o 21 | #ST_OBJ+=stm32f4xx_pwr.o 22 | ST_OBJ+=stm32f4xx_rcc.o 23 | #ST_OBJ+=stm32f4xx_rtc.o 24 | #ST_OBJ+=stm32f4xx_sdio.o 25 | ST_OBJ+=stm32f4xx_spi.o 26 | ST_OBJ+=stm32f4xx_tim.o 27 | ST_OBJ+=stm32f4xx_usart.o 28 | ST_OBJ+=stm32f4xx_misc.o 29 | #ST_OBJ+=stm32f4xx_wwdg.o 30 | ST_OBJ+=stm32f4xx_syscfg.o 31 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/stm32f4discovery.cfg: -------------------------------------------------------------------------------- 1 | # This is an STM32F4 discovery board with a single STM32F407VGT6 chip. 2 | # http://www.st.com/internet/evalboard/product/252419.jsp 3 | 4 | source [find interface/stlink-v2.cfg] 5 | 6 | source [find target/stm32f4x_stlink.cfg] 7 | 8 | # use hardware reset, connect under reset 9 | reset_config srst_only srst_nogate 10 | -------------------------------------------------------------------------------- /bolt/tools/make/F405/stm32f4x_stlink.cfg: -------------------------------------------------------------------------------- 1 | # 2 | # STM32f4x stlink pseudo target 3 | # 4 | 5 | if { [info exists CHIPNAME] == 0 } { 6 | set CHIPNAME stm32f4x 7 | } 8 | 9 | if { [info exists CPUTAPID] == 0 } { 10 | set CPUTAPID 0x2ba01477 11 | } 12 | 13 | if { [info exists WORKAREASIZE] == 0 } { 14 | set WORKAREASIZE 0x10000 15 | } 16 | 17 | source [find target/stm32_stlink.cfg] 18 | 19 | # stm32f4x family uses stm32f2x driver 20 | set _FLASHNAME $_CHIPNAME.flash 21 | flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME 22 | -------------------------------------------------------------------------------- /bolt/tools/make/check-for-submodules.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | from subprocess import check_output 4 | import sys 5 | 6 | # Get the status of the submodules from git. If any of the submodules start 7 | # with "-" then the module is not initalized. Issue a warning. 8 | 9 | result = str(check_output(["git", "submodule", "status"])) 10 | lines = result.split("\n") 11 | 12 | initialized = True 13 | for line in lines: 14 | if len(line) > 0 and line[0] == '-': 15 | initialized = False 16 | 17 | if not initialized: 18 | print('ERROR: One or more of the git submodules are not initialized. Try') 19 | print(' git submodule init') 20 | print(' git submodule update') 21 | sys.exit(-1) 22 | -------------------------------------------------------------------------------- /bolt/tools/make/cmsis_dsp/Makefile: -------------------------------------------------------------------------------- 1 | include obj.mk 2 | 3 | BIN=$(PROJ_ROOT)/bin/vendor 4 | PROJ_BIN=$(PROJ_ROOT)/bin 5 | CMSIS_INC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/Core/Include 6 | DSP_INC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Include 7 | DSP_SRC=$(CRAZYFLIE_BASE)/vendor/CMSIS/CMSIS/DSP/Source 8 | 9 | VPATH += $(BIN) 10 | VPATH += $(DSP_SRC)/BasicMathFunctions/ 11 | VPATH += $(DSP_SRC)/CommonTables/ 12 | VPATH += $(DSP_SRC)/ComplexMathFunctions/ 13 | VPATH += $(DSP_SRC)/ControllerFunctions/ 14 | VPATH += $(DSP_SRC)/FastMathFunctions/ 15 | VPATH += $(DSP_SRC)/FilteringFunctions/ 16 | VPATH += $(DSP_SRC)/MatrixFunctions/ 17 | VPATH += $(DSP_SRC)/StatisticsFunctions/ 18 | VPATH += $(DSP_SRC)/SupportFunctions/ 19 | VPATH += $(DSP_SRC)/TransformFunctions/ 20 | 21 | 22 | LIB=$(PROJ_BIN)/libarm_math.a 23 | 24 | CFLAGS = -O2 -I$(CMSIS_INC) -I$(DSP_INC) -std=c11 25 | CFLAGS += -mthumb -mcpu=cortex-m4 -mfloat-abi=hard -mfpu=fpv4-sp-d16 26 | CFLAGS += -DARM_MATH_CM4 -D__FPU_PRESENT=1 -ffunction-sections -fdata-sections 27 | 28 | CROSS_COMPILE ?= arm-none-eabi- 29 | CC = $(CROSS_COMPILE)gcc 30 | AR = $(CROSS_COMPILE)ar 31 | 32 | all: $(LIB) 33 | 34 | $(LIB): $(DSP_OBJ) #$(foreach o,$(DSP_OBJ),$(BIN)/$(o)) 35 | @$(AR) rcs $@ $(foreach o,$(DSP_OBJ),$(BIN)/$(o)) 36 | @echo " AR libarm_math.a" 37 | 38 | include ../targets.mk 39 | -------------------------------------------------------------------------------- /bolt/tools/make/oot.mk: -------------------------------------------------------------------------------- 1 | # 2 | # We tell the firmware that this is an "Out-of-Tree" build so look for 3 | # kbuild files in this directory. 4 | # 5 | OOT ?= $(PWD) 6 | 7 | # 8 | # We need to tell the firmware where to find extra config options for this 9 | # build. 10 | # 11 | OOT_CONFIG ?= $(OOT)/oot-config 12 | 13 | OOT_ARGS ?= -C $(CRAZYFLIE_BASE) OOT=$(OOT) EXTRA_CFLAGS="$(EXTRA_CFLAGS)" 14 | 15 | ifneq ($(OOT_USES_CXX),) 16 | OOT_ARGS += OOT_USES_CXX=1 17 | endif 18 | 19 | .PHONY: all clean 20 | 21 | all: 22 | $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) KCONFIG_ALLCONFIG=$(OOT_CONFIG) alldefconfig 23 | $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) -j 12 24 | 25 | clean: 26 | $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) clean 27 | 28 | cload: 29 | $(MAKE) KBUILD_OUTPUT=$(OOT)/build $(OOT_ARGS) cload 30 | -------------------------------------------------------------------------------- /bolt/tools/make/size.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import argparse 4 | import subprocess 5 | 6 | # Calls the size progeam and prints pretty memory usage 7 | 8 | def check_output(*args): 9 | """A wrapper for subprocess.check_output() to handle differences in python 2 and 3. 10 | Returns a string. 11 | """ 12 | result = subprocess.check_output(*args) 13 | 14 | if isinstance(result, bytes): 15 | return result.decode('utf-8') 16 | else: 17 | return result 18 | 19 | 20 | if __name__ == "__main__": 21 | 22 | parser = argparse.ArgumentParser() 23 | parser.add_argument("size_app", help="path to the size program") 24 | parser.add_argument("source", help=".elf file to use") 25 | parser.add_argument("flash_size_k", help="size of the flash, in k bytes", type=int) 26 | parser.add_argument("ram_size_k", help="size of the RAM, in k bytes", type=int) 27 | parser.add_argument("ccm_size_k", help="size of the CCM, in k bytes", type=int) 28 | args = parser.parse_args() 29 | 30 | output = check_output([args.size_app, '-A', args.source]) 31 | sizes = {} 32 | for line in output.splitlines(): 33 | parts = line.split() 34 | if len(parts) == 3 and parts[0] != 'section': 35 | sizes[parts[0]] = int(parts[1]) 36 | 37 | flash_available = args.flash_size_k * 1024 38 | flash_used = sizes['.text'] + sizes['.data'] + sizes['.ccmdata'] 39 | flash_free = flash_available - flash_used 40 | flash_fill = 100 * flash_used / flash_available 41 | 42 | ram_available = args.ram_size_k * 1024 43 | ram_used = sizes['.bss'] + sizes['.data'] 44 | ram_free = ram_available - ram_used 45 | ram_fill = 100 * ram_used / ram_available 46 | 47 | ccm_available = args.ccm_size_k * 1024 48 | ccm_used = sizes['.ccmbss'] + sizes['.ccmdata'] 49 | ccm_free = ccm_available - ccm_used 50 | ccm_fill = 100 * ccm_used / ccm_available 51 | 52 | print("Flash | {:7d}/{:<7d} ({:2.0f}%), {:7d} free | text: {}, data: {}, ccmdata: {}".format(flash_used, flash_available, flash_fill, flash_free, sizes['.text'], sizes['.data'], sizes['.ccmdata'])) 53 | print("RAM | {:7d}/{:<7d} ({:2.0f}%), {:7d} free | bss: {}, data: {}".format(ram_used, ram_available, ram_fill, ram_free, sizes['.bss'], sizes['.data'])) 54 | print("CCM | {:7d}/{:<7d} ({:2.0f}%), {:7d} free | ccmbss: {}, ccmdata: {}".format(ccm_used, ccm_available, ccm_fill, ccm_free, sizes['.ccmbss'], sizes['.ccmdata'])) 55 | -------------------------------------------------------------------------------- /bolt/tools/make/targets.mk: -------------------------------------------------------------------------------- 1 | # Part of CrazyFlie's Makefile 2 | # Copyright (c) 2009, Arnaud Taffanel 3 | # Common targets with verbose support 4 | 5 | 6 | ifeq ($(V),) 7 | VERBOSE=_SILENT 8 | endif 9 | 10 | ifeq ($(V),0) 11 | QUIET=1 12 | endif 13 | 14 | target = @$(if $(QUIET), ,echo $($1_COMMAND$(VERBOSE)) ); @$($1_COMMAND) 15 | 16 | VTMPL_COMMAND=$(PYTHON) $(CRAZYFLIE_BASE)/tools/make/versionTemplate.py --crazyflie-base $(CRAZYFLIE_BASE) $< $@ 17 | #$(BIN)/$(lastword $(subst /, ,$@)) 18 | VTMPL_COMMAND_SILENT=" VTMPL $@" 19 | %.c: %.vtpl 20 | @$(if $(QUIET), ,echo $(VTMPL_COMMAND$(VERBOSE)) ) 21 | @$(VTMPL_COMMAND) 22 | 23 | CC_COMMAND=$(CC) $(CFLAGS) -c $< -o $(BIN)/$@ 24 | CC_COMMAND_SILENT=" CC $@" 25 | .c.o: 26 | @$(if $(QUIET), ,echo $(CC_COMMAND$(VERBOSE)) ) 27 | @$(CC_COMMAND) 28 | 29 | CCS_COMMAND=$(CC) $(CSFLAGS) -c $< -o $(BIN)/$@ 30 | CCS_COMMAND_SILENT=" CCS $@" 31 | .S.o: 32 | @$(if $(QUIET), ,echo $(CCS_COMMAND$(VERBOSE)) ) 33 | @$(CCS_COMMAND) 34 | 35 | HEX_COMMAND=$(OBJCOPY) $< -O ihex $@ 36 | HEX_COMMAND_SILENT=" COPY $@" 37 | $(PROG).hex: $(PROG).elf 38 | @$(if $(QUIET), ,echo $(HEX_COMMAND$(VERBOSE)) ) 39 | @$(HEX_COMMAND) 40 | 41 | BIN_COMMAND=$(OBJCOPY) $< -O binary --pad-to 0 --remove-section=.bss --remove-section=.nzds --remove-section=._usrstack $@ 42 | BIN_COMMAND_SILENT=" COPY $@" 43 | $(PROG).bin: $(PROG).elf 44 | @$(if $(QUIET), ,echo $(BIN_COMMAND$(VERBOSE)) ) 45 | @$(BIN_COMMAND) 46 | 47 | DFU_COMMAND=$(PYTHON) $(CRAZYFLIE_BASE)/tools/make/dfu-convert.py -b $(LOAD_ADDRESS):$< $@ 48 | DFU_COMMAND_SILENT=" DFUse $@" 49 | $(PROG).dfu: $(PROG).bin 50 | @$(if $(QUIET), ,echo $(DFU_COMMAND$(VERBOSE)) ) 51 | @$(DFU_COMMAND) 52 | 53 | AS_COMMAND=$(AS) $(ASFLAGS) $< -o $(BIN)/$@ 54 | AS_COMMAND_SILENT=" AS $@" 55 | .s.o: 56 | @$(if $(QUIET), ,echo $(AS_COMMAND$(VERBOSE)) ) 57 | @$(AS_COMMAND) 58 | 59 | CLEAN_O_COMMAND=rm -f $(foreach o,$(OBJ),$(BIN)/$(o)) 60 | CLEAN_O_COMMAND_SILENT=" CLEAN_O" 61 | clean_o: clean_version 62 | @$(if $(QUIET), ,echo $(CLEAN_O_COMMAND$(VERBOSE)) ) 63 | @$(CLEAN_O_COMMAND) 64 | 65 | clean_cf: 66 | @rm -f $(srctree)/$(PROG).* 67 | @rm -f $(srctree)/cffirmware.py 68 | @rm -f $(srctree)/_cffirmware*.so 69 | @rm -f $(srctree)/build/cffirmware_wrap.c 70 | -------------------------------------------------------------------------------- /bolt/tools/make/usb-bootloader.py: -------------------------------------------------------------------------------- 1 | #send a USB request to the crazyflie, causing it to enter DFU mode (bootloader) automatically 2 | #flash new firmware using 'make flash_dfu' 3 | 4 | from time import sleep 5 | import usb 6 | import usb.core 7 | 8 | #find connected crazyflie, usb vendor and product id = 0483:5740 9 | dev = usb.core.find(idVendor=0x0483, idProduct=0x5740) 10 | if dev is None: 11 | raise ValueError('Could not find any USB connected crazyflie') 12 | 13 | #send signal to enter bootloader 14 | try: 15 | dev.ctrl_transfer(bmRequestType=usb.TYPE_VENDOR, 16 | bRequest=0x01, wValue=0x01, wIndex=2) 17 | except IOError: 18 | #io error expected because the crazyflie will not respond to USB request as it resets into the bootloader 19 | #TODO usbd_cf_Setup function in firmware needs to return USBD_OK before rebooting to fix this 20 | #sleep to allow time for crazyflie to get into DFU mode 21 | sleep(0.5) -------------------------------------------------------------------------------- /bolt/vendor/.gitignore: -------------------------------------------------------------------------------- 1 | STMicroelectronics 2 | vl53l1x-arduino 3 | free_rtos 4 | -------------------------------------------------------------------------------- /clients/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | msp.py 3 | -------------------------------------------------------------------------------- /clients/btsupport.py: -------------------------------------------------------------------------------- 1 | ''' 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | ''' 16 | 17 | import socket 18 | import sys 19 | import time 20 | 21 | 22 | #RPI_ADDRESS = 'B8:27:EB:E0:1D:07' # Onboard 23 | RPI_ADDRESS = 'B8:27:EB:3F:AB:47' # PiHat 24 | 25 | def connect_to_server(port): 26 | 27 | while True: 28 | 29 | try: 30 | 31 | print('Connecting to server %s:%d ... ' % (RPI_ADDRESS, port), end='') 32 | sys.stdout.flush() 33 | 34 | # Create a Bluetooth or IP socket depending on address format 35 | client = (socket.socket(socket.AF_BLUETOOTH, socket.SOCK_STREAM, 36 | socket.BTPROTO_RFCOMM) 37 | if ':' in RPI_ADDRESS 38 | else socket.socket(socket.AF_INET, socket.SOCK_STREAM)) 39 | 40 | try: 41 | client.connect((RPI_ADDRESS, port)) 42 | print(' connected') 43 | break 44 | 45 | except Exception as e: 46 | print(str(e) + ': is server running?') 47 | time.sleep(1) 48 | 49 | except KeyboardInterrupt: 50 | break 51 | 52 | return client 53 | -------------------------------------------------------------------------------- /clients/spikes.sh: -------------------------------------------------------------------------------- 1 | ~/Desktop/spikeplotter/spikeplot.py -n 2 -a B8:27:EB:3F:AB:47 -p 3 2 | -------------------------------------------------------------------------------- /clients/state.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | ''' 4 | State message logger 5 | 6 | Copyright (C) 2025 Simon D. Levy 7 | 8 | This program is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU General Public License as published by 10 | the Free Software Foundation, in version 3. 11 | 12 | This program is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU General Public License for more details. 16 | 17 | You should have received a copy of the GNU General Public License 18 | along with this program. If not, see . 19 | ''' 20 | 21 | import struct 22 | import argparse 23 | 24 | from btsupport import connect_to_server 25 | 26 | RPI_LOGGING_PORT = 2 27 | 28 | 29 | def logging_fun(client, status): 30 | 31 | while status['running']: 32 | 33 | try: 34 | msg = client.recv(48) 35 | 36 | except Exception as e: 37 | print('Failed to receiving logging data: ' + str(e)) 38 | status['running'] = False 39 | return 40 | 41 | x, dx, y, dy, z, dz, phi, dphi, theta, dtheta, psi, dpsi = ( 42 | struct.unpack('ffffffffffff', msg)) 43 | 44 | print(('dx=%+03.2f dy=%+03.2f z=%+03.2f dz=%+03.2f ' + 45 | 'phi=%+5.1f dphi=%+6.1f theta=%+5.1f dtheta=%+6.1f ' + 46 | 'psi=%+5.1f dpsi=%+5.1f') % 47 | (dx, dy, z, dz, phi, dphi, theta, dtheta, psi, dpsi)) 48 | 49 | 50 | def main(): 51 | 52 | client = connect_to_server(RPI_LOGGING_PORT) 53 | 54 | status = {'running': True} 55 | 56 | while status['running']: 57 | 58 | try: 59 | 60 | logging_fun(client, status) 61 | 62 | except KeyboardInterrupt: 63 | 64 | break 65 | 66 | 67 | main() 68 | -------------------------------------------------------------------------------- /examples/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /examples/bmi088/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = bmi088 2 | 3 | # FQBN = STMicroelectronics:stm32:GenF4:pnum=GENERIC_F405RGTX,usb=CDCgen 4 | FQBN = teensy:avr:teensy40 5 | 6 | # PORT = /dev/ttyACM0 7 | PORT = COM3 8 | 9 | LIBS = $(HOME)/Documents/Arduino/libraries 10 | 11 | SRC = ../../src/ 12 | 13 | build: 14 | cp FreeRTOSConfig.h $(SRC) 15 | arduino-cli compile --libraries ../../.. --libraries $(HOME)/Documents/Arduino/libraries \ 16 | --fqbn $(FQBN) $(SKETCH).ino 17 | rm ../../src/FreeRTOSConfig.h 18 | 19 | 20 | flash: 21 | #dfu-util -d 0483:5740 -a 0 -s 0x08000000:leave -D $(BIN) 22 | arduino-cli upload -p $(PORT) --fqbn $(FQBN) 23 | 24 | edit: 25 | vim $(SKETCH).ino 26 | 27 | listen: 28 | #miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 29 | plink -serial $(PORT) 30 | 31 | -------------------------------------------------------------------------------- /examples/bmi088/bmi088.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | static const uint8_t GYRO_INTERRUPT_PIN = 4; 4 | 5 | static Bmi088Accel accel(Wire,0x19); 6 | 7 | static Bmi088Gyro gyro(Wire,0x69); 8 | 9 | static volatile bool gyro_flag; 10 | 11 | void gyro_drdy() 12 | { 13 | gyro_flag = true; 14 | } 15 | 16 | static void reportForever(const char * msg) 17 | { 18 | while (true) { 19 | Serial.println(msg); 20 | } 21 | } 22 | 23 | void setup() 24 | { 25 | 26 | Serial.begin(0); 27 | 28 | if (accel.begin() < 0) { 29 | reportForever("Accel Initialization Error"); 30 | } 31 | 32 | if (gyro.begin() < 0) { 33 | reportForever("Gyro Initialization Error"); 34 | } 35 | 36 | gyro.setOdr(Bmi088Gyro::ODR_100HZ_BW_12HZ); 37 | gyro.pinModeInt3(Bmi088Gyro::PUSH_PULL,Bmi088Gyro::ACTIVE_HIGH); 38 | gyro.mapDrdyInt3(true); 39 | 40 | pinMode(4,INPUT); 41 | attachInterrupt(GYRO_INTERRUPT_PIN,gyro_drdy,RISING); 42 | } 43 | 44 | void loop() 45 | { 46 | //if (gyro_flag) { 47 | 48 | gyro_flag = false; 49 | 50 | accel.readSensor(); 51 | gyro.readSensor(); 52 | 53 | Serial.print(accel.getAccelX_mss()); 54 | Serial.print("\t"); 55 | Serial.print(accel.getAccelY_mss()); 56 | Serial.print("\t"); 57 | Serial.print(accel.getAccelZ_mss()); 58 | Serial.print("\t"); 59 | Serial.print(gyro.getGyroX_rads()); 60 | Serial.print("\t"); 61 | Serial.print(gyro.getGyroY_rads()); 62 | Serial.print("\t"); 63 | Serial.print(gyro.getGyroZ_rads()); 64 | Serial.print("\t"); 65 | Serial.print(accel.getTemperature_C()); 66 | Serial.print("\n"); 67 | //} 68 | } 69 | -------------------------------------------------------------------------------- /examples/standard/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | -------------------------------------------------------------------------------- /examples/standard/Makefile: -------------------------------------------------------------------------------- 1 | SKETCH = standard 2 | 3 | FQBN = teensy:avr:teensy40 4 | 5 | PORT = /dev/ttyACM0 6 | 7 | LIBS = $(HOME)/Documents/Arduino/libraries 8 | 9 | SRC = ../../src/ 10 | 11 | build: 12 | cp FreeRTOSConfig.h $(SRC) 13 | arduino-cli compile --libraries ../../.. --libraries $(HOME)/Documents/Arduino/libraries \ 14 | --fqbn $(FQBN) $(SKETCH).ino 15 | rm ../../src/FreeRTOSConfig.h 16 | 17 | config: 18 | cp $(SRC)/control/standard.hpp $(SRC)/control.hpp 19 | cd ../../msppg; make install; cd .. 20 | 21 | flash: 22 | arduino-cli upload -p $(PORT) --fqbn $(FQBN) . 23 | 24 | edit: 25 | vim $(SKETCH).ino 26 | 27 | listen: 28 | miniterm.py $(PORT) 115200 --exit-char 3 # exit on CTRL-C 29 | 30 | -------------------------------------------------------------------------------- /haskell/.gitignore: -------------------------------------------------------------------------------- 1 | LinAlg 2 | Ekf 3 | *.hi 4 | *.o 5 | *.c* 6 | *.h 7 | -------------------------------------------------------------------------------- /haskell/Num.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Utility functions for real and simulated flight controllers 3 | 4 | Copyright (C) 2025 Simon D. Levy 5 | 6 | This program is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU General Public License as published by 8 | the Free Software Foundation, in version 3. 9 | 10 | This program is distributed in the hope that it will be useful, 11 | but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | GNU General Public License for more details. 14 | 15 | You should have received a copy of the GNU General Public License 16 | along with this program. If not, see . 17 | --} 18 | 19 | {-# LANGUAGE DataKinds #-} 20 | {-# LANGUAGE RebindableSyntax #-} 21 | 22 | module Num where 23 | 24 | import Language.Copilot 25 | import Copilot.Compile.C99 26 | 27 | type SFloat = Stream Float 28 | type SInt32 = Stream Int32 29 | type SInt8 = Stream Int8 30 | type SInt16 = Stream Int16 31 | type SBool = Stream Bool 32 | 33 | sqr :: SFloat -> SFloat 34 | sqr x = x * x 35 | 36 | rescale :: SFloat -> SFloat -> SFloat -> SFloat -> SFloat -> SFloat 37 | rescale value oldmin oldmax newmin newmax = 38 | (value - oldmin) / (oldmax - oldmin) * (newmax - newmin) + newmin 39 | 40 | constrain :: SFloat -> SFloat -> SFloat -> SFloat 41 | constrain val min max = if val < min then min else if val > max then max else val 42 | 43 | constrainabs :: SFloat -> SFloat -> SFloat 44 | constrainabs val max = constrain val (-max) max 45 | 46 | rad2deg :: SFloat -> SFloat 47 | rad2deg rad = 180 * rad / pi 48 | 49 | deg2rad :: SFloat -> SFloat 50 | deg2rad deg = deg * pi / 180 51 | 52 | atan2 :: SFloat -> SFloat -> SFloat 53 | atan2 y x = if x > 0 then atan (y / x) 54 | else if x < 0 && y >= 0 then atan (y / x) + pi 55 | else if x < 0 && y < 0 then atan (y / x) - pi 56 | else if x == 0 && y > 0 then pi 57 | else if x == 0 && y < 0 then (-pi) 58 | else 0 -- undefined 59 | -------------------------------------------------------------------------------- /haskell/Pids.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module Pids where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | -- PID controllers 28 | import AltitudeController 29 | import ClimbRateController 30 | import YawAngleController 31 | import YawRateController 32 | import PositionController 33 | import PitchRollAngleController 34 | import PitchRollRateController 35 | 36 | -- Constants ----------------------------------------------------------------- 37 | 38 | spec = do 39 | 40 | let climbrate = altitudeController 41 | 42 | let thrust = climbRateController climbrate 43 | 44 | let airborne = thrust > 0 45 | 46 | let yaw_rate = yawAngleController airborne 47 | 48 | let yaw' = yawRateController airborne yaw_rate 49 | 50 | let (roll', pitch') = positionController airborne 51 | 52 | let (roll'', pitch'') = pitchRollAngleController airborne (roll', pitch') 53 | 54 | let (roll''', pitch''') = pitchRollRateController airborne (roll'', pitch'') 55 | 56 | trigger "setDemands" true 57 | [arg thrust, arg roll''', arg pitch''', arg yaw'] 58 | 59 | -- Compile the spec 60 | main = reify spec >>= 61 | compileWith (CSettings "copilot_step_core" ".") "copilot_core" 62 | -------------------------------------------------------------------------------- /haskell/README.md: -------------------------------------------------------------------------------- 1 |

2 | 3 |

4 | 5 | # “LambdaFlight”: Hackflight meets Haskell 6 | 7 | To experiment with using Hackflight for flight control in Haskell, you'll first 8 | need to install [Haskell](https://www.haskell.org/) and [NASA 9 | Copilot](https://copilot-language.github.io) (the “secret sauce” 10 | that allows you to compile Haskell code to a form suitable for running on a 11 | flight controller.) I was able install Copilot via: 12 | 13 | ``` 14 | cabal install copilot 15 | cabal install copilot-c99 16 | ``` 17 | 18 | Next, follow these [directions](../webots) for installing Webots and flying a simulated quadcopter. 19 | 20 | Finally, from the hackflight main directory, do the following: 21 | 22 | ``` 23 | cd webots/plugins/physics/haskell 24 | make 25 | cd ../../../controllers/controller 26 | make 27 | make runhaskell 28 | ``` 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /haskell/pids/AltitudeController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module AltitudeController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 2 28 | ki = 0.5 29 | ilimit = 5000 30 | vel_max = 1 31 | vel_max_overhead = 1.10 32 | landing_speed_mps = 0.15 33 | 34 | dt :: SFloat 35 | dt = extern "stream_dt" Nothing 36 | 37 | demand_thrust :: SFloat 38 | demand_thrust = extern "stream_thrust" Nothing 39 | 40 | state_z :: SFloat 41 | state_z = extern "stream_z" Nothing 42 | 43 | hovering :: SBool 44 | hovering = extern "stream_hovering" Nothing 45 | 46 | {-- 47 | Demand is input as altitude target in meters and output as climb rate in meters 48 | per second. 49 | --} 50 | 51 | altitudeController :: SFloat 52 | 53 | altitudeController = climbrate where 54 | 55 | error = demand_thrust - state_z 56 | 57 | integral = if hovering 58 | then constrainabs (integral' + error * dt) ilimit 59 | else 0 60 | 61 | integral' = [0] ++ integral 62 | 63 | climbrate = if hovering 64 | then constrainabs (kp * error + ki * integral) 65 | ((max vel_max 0.5) * vel_max_overhead) 66 | else (-landing_speed_mps) 67 | -------------------------------------------------------------------------------- /haskell/pids/ClimbRateController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module ClimbRateController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | -- PID constants 28 | kp = 25 29 | ki = 15 30 | ilimit = 5000 31 | 32 | -- Appropriate thrust values for our DIY quadcopter 33 | thrust_base = 36000 34 | thrust_min = 20000 35 | thrust_max = 65535 36 | thrust_scale = 1000 37 | 38 | hovering :: SBool 39 | hovering = extern "stream_hovering" Nothing 40 | 41 | dt :: SFloat 42 | dt = extern "stream_dt" Nothing 43 | 44 | state_z :: SFloat 45 | state_z = extern "stream_z" Nothing 46 | 47 | state_dz :: SFloat 48 | state_dz = extern "stream_dz" Nothing 49 | 50 | landing_altitude_m :: SFloat 51 | landing_altitude_m = extern "stream_landing_altitude_m" Nothing 52 | 53 | {-- 54 | 55 | Demand is input as climb rate in meters per second and output as arbitrary 56 | positive value to be scaled according to motor characteristics. 57 | 58 | --} 59 | 60 | climbRateController :: SFloat -> SFloat 61 | 62 | climbRateController climbrate = spin where 63 | 64 | airborne = hovering || (state_z > landing_altitude_m) 65 | 66 | error = climbrate - state_dz 67 | 68 | integral = if airborne 69 | then constrainabs (integral' + error * dt) ilimit 70 | else 0 71 | 72 | integral' = [0] ++ integral 73 | 74 | thrust = kp * error + ki * integral 75 | 76 | spin = if airborne 77 | then constrain (thrust * thrust_scale + thrust_base) 78 | thrust_min thrust_max 79 | else 0 80 | -------------------------------------------------------------------------------- /haskell/pids/PitchRollAngleController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module PitchRollAngleController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 6 28 | ki = 3 29 | ilimit = 20 30 | 31 | dt :: SFloat 32 | dt = extern "stream_dt" Nothing 33 | 34 | state_phi :: SFloat 35 | state_phi = extern "stream_phi" Nothing 36 | 37 | state_theta :: SFloat 38 | state_theta = extern "stream_theta" Nothing 39 | 40 | {-- 41 | Demand is input as angles in degrees and output as angular 42 | velocities in degrees per second: 43 | 44 | roll: right-down positive 45 | 46 | pitch: nose-down positive 47 | --} 48 | 49 | pitchRollAngleController :: SBool -> (SFloat, SFloat) -> (SFloat, SFloat) 50 | 51 | pitchRollAngleController airborne (roll, pitch) = (roll', pitch') where 52 | 53 | -- Run PIDs on body-coordinate velocities 54 | (roll', roll_integral) = 55 | runAxis airborne dt roll state_phi roll_integral' 56 | 57 | (pitch', pitch_integral) = 58 | runAxis airborne dt pitch state_theta pitch_integral' 59 | 60 | roll_integral' = [0] ++ roll_integral 61 | pitch_integral' = [0] ++ pitch_integral 62 | 63 | runAxis airborne dt demand measured integral = (demand', integral') where 64 | 65 | error = demand - measured 66 | 67 | integral' = if airborne 68 | then constrainabs (integral + error * dt) ilimit 69 | else 0 70 | 71 | demand' = kp * error + ki * integral 72 | -------------------------------------------------------------------------------- /haskell/pids/PitchRollRateController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module PitchRollRateController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 125 28 | ki = 250 29 | kd = 1.25 30 | ilimit = 33 31 | 32 | dt :: SFloat 33 | dt = extern "stream_dt" Nothing 34 | 35 | state_dphi :: SFloat 36 | state_dphi = extern "stream_dphi" Nothing 37 | 38 | state_dtheta :: SFloat 39 | state_dtheta = extern "stream_dtheta" Nothing 40 | 41 | {-- 42 | Demands are input as angular velocities in degrees per second and 43 | output as as arbitrary values to be scaled according to motor 44 | characteristics: 45 | 46 | roll: input roll-right positive => output positive 47 | 48 | pitch: input nose-down positive => output positive 49 | --} 50 | 51 | pitchRollRateController :: SBool -> (SFloat, SFloat) -> (SFloat, SFloat) 52 | 53 | pitchRollRateController airborne (roll, pitch) = (roll', pitch') where 54 | 55 | (roll', roll_integral, roll_error) = 56 | runAxis airborne dt roll state_dphi roll_integral' roll_error' 57 | 58 | (pitch', pitch_integral, pitch_error) = 59 | runAxis airborne dt pitch state_dtheta pitch_integral' pitch_error' 60 | 61 | roll_integral' = [0] ++ roll_integral 62 | pitch_integral' = [0] ++ pitch_integral 63 | 64 | roll_error' = [0] ++ roll_error 65 | pitch_error' = [0] ++ pitch_error 66 | 67 | runAxis airborne dt demand measured integral error' = 68 | (demand', integral', error'') where 69 | 70 | error = demand - measured 71 | 72 | deriv = if dt > 0 then (error - error') / dt else 0 73 | 74 | demand' = kp * error + ki * integral + kd * deriv 75 | 76 | integral' = if airborne 77 | then constrainabs (integral + error * dt) ilimit 78 | else 0 79 | 80 | error'' = if airborne then error else 0 81 | -------------------------------------------------------------------------------- /haskell/pids/PositionController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module PositionController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 25 28 | ki = 1 29 | ilimit = 5000 30 | limit = 20 31 | limit_overhead = 1.10 32 | 33 | dt :: SFloat 34 | dt = extern "stream_dt" Nothing 35 | 36 | demand_roll :: SFloat 37 | demand_roll = extern "stream_roll" Nothing 38 | 39 | demand_pitch :: SFloat 40 | demand_pitch = extern "stream_pitch" Nothing 41 | 42 | state_dx :: SFloat 43 | state_dx = extern "stream_dx" Nothing 44 | 45 | state_dy :: SFloat 46 | state_dy = extern "stream_dy" Nothing 47 | 48 | state_psi :: SFloat 49 | state_psi = extern "stream_psi" Nothing 50 | 51 | 52 | {-- 53 | Demands are input as normalized interval [-1,+1] and output as 54 | angles in degrees. 55 | 56 | roll: input left positive => output negative 57 | 58 | pitch: input forward positive => output negative 59 | --} 60 | 61 | positionController :: SBool -> (SFloat, SFloat) 62 | 63 | positionController airborne = (roll, pitch) where 64 | 65 | psi = deg2rad state_psi 66 | 67 | -- Rotate world-coordinate velocities into body coordinates 68 | cospsi = cos psi 69 | sinpsi = sin psi 70 | dxb = state_dx * cospsi + state_dy * sinpsi 71 | dyb = -state_dx * sinpsi + state_dy * cospsi 72 | 73 | -- Run PIDs on body-coordinate velocities 74 | (roll, integralY) = runAxis airborne dt demand_roll dyb integralY' 75 | (pitch, integralX) = runAxis airborne dt demand_pitch dxb integralX' 76 | 77 | integralX' = [0] ++ integralX 78 | integralY' = [0] ++ integralY 79 | 80 | runAxis airborne dt demand measured integral = (demand', integral') where 81 | 82 | error = demand - measured 83 | 84 | integral' = if airborne 85 | then constrainabs (integral + error * dt) ilimit 86 | else 0 87 | 88 | demand' = constrainabs (kp * error + ki * integral) limit 89 | -------------------------------------------------------------------------------- /haskell/pids/YawAngleController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module YawAngleController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 6 28 | ki = 1 29 | kd = 0.35 30 | ilimit = 360 31 | demand_max = 200 32 | 33 | dt :: SFloat 34 | dt = extern "stream_dt" Nothing 35 | 36 | state_psi :: SFloat 37 | state_psi = extern "stream_psi" Nothing 38 | 39 | demand_yaw :: SFloat 40 | demand_yaw = extern "stream_yaw" Nothing 41 | 42 | {-- 43 | Demand is input as yawAngle target in meters and output as climb rate in 44 | meters per second. 45 | --} 46 | 47 | yawAngleController :: SBool -> SFloat 48 | 49 | yawAngleController airborne = demand where 50 | 51 | target = cap $ target' + demand_max * demand_yaw * dt 52 | 53 | error = cap $ target - state_psi 54 | 55 | integral = if airborne 56 | then constrainabs (integral' + error * dt) ilimit 57 | else 0 58 | 59 | deriv = if dt > 0 then (error - error') / dt else 0 60 | 61 | demand = kp * error + ki * integral + kd * deriv 62 | 63 | target' = [0] ++ if airborne then target else state_psi 64 | integral' = [0] ++ integral 65 | error' = [0] ++ if airborne then error else 0 66 | 67 | cap :: SFloat -> SFloat 68 | 69 | cap angle = if angle > 180 then angle - 360 70 | else if angle < (-180) then angle + 360 71 | else angle 72 | -------------------------------------------------------------------------------- /haskell/pids/YawRateController.hs: -------------------------------------------------------------------------------- 1 | {-- 2 | Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | 8 | This program is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU General Public License for more details. 12 | 13 | You should have received a copy of the GNU General Public License 14 | along with this program. If not, see . 15 | --} 16 | 17 | {-# LANGUAGE DataKinds #-} 18 | {-# LANGUAGE RebindableSyntax #-} 19 | 20 | module YawRateController where 21 | 22 | import Language.Copilot 23 | import Copilot.Compile.C99 24 | 25 | import Num 26 | 27 | kp = 120 28 | ki = 16.7 29 | ilimit = 166.7 30 | 31 | dt :: SFloat 32 | dt = extern "stream_dt" Nothing 33 | 34 | state_dpsi :: SFloat 35 | state_dpsi = extern "stream_dpsi" Nothing 36 | 37 | {-- 38 | Input is angular rate demand (deg/sec) and actual angular 39 | rate from gyro; ouputput is arbitrary units scaled for motors. 40 | --} 41 | 42 | yawRateController :: SBool -> SFloat -> SFloat 43 | 44 | yawRateController airborne demand = demand' where 45 | 46 | error = demand - state_dpsi 47 | 48 | integral = if airborne 49 | then constrainabs (integral' + error * dt) ilimit 50 | else 0 51 | 52 | demand' = kp * error + ki * integral 53 | 54 | integral' = [0] ++ integral 55 | -------------------------------------------------------------------------------- /library.properties: -------------------------------------------------------------------------------- 1 | name=Hackflight 2 | version=0.1 3 | author= Simon D. Levy 4 | maintainer=Simon D. Levy 5 | sentence=Minimalist flight control 6 | paragraph=Minimalist flight control 7 | category=Other 8 | url=https://github.com/simondlevy/Hackflight 9 | architectures=* 10 | includes=hackflight.h 11 | -------------------------------------------------------------------------------- /media/boltquad.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/boltquad.jpg -------------------------------------------------------------------------------- /media/cage.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/cage.odp -------------------------------------------------------------------------------- /media/dataflow.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/dataflow.odp -------------------------------------------------------------------------------- /media/dataflow2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/dataflow2.png -------------------------------------------------------------------------------- /media/haskell.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/haskell.png -------------------------------------------------------------------------------- /media/icon.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/icon.png -------------------------------------------------------------------------------- /media/lambda.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/lambda.png -------------------------------------------------------------------------------- /media/logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/logo.png -------------------------------------------------------------------------------- /media/msppg.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/msppg.png -------------------------------------------------------------------------------- /media/rust.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/rust.png -------------------------------------------------------------------------------- /media/sim.odp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/sim.odp -------------------------------------------------------------------------------- /media/sim.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/sim.png -------------------------------------------------------------------------------- /media/tennflight.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/tennflight.png -------------------------------------------------------------------------------- /media/tinyape.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/tinyape.jpg -------------------------------------------------------------------------------- /media/webots.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/webots.png -------------------------------------------------------------------------------- /media/webots2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/media/webots2.png -------------------------------------------------------------------------------- /msppg/.gitignore: -------------------------------------------------------------------------------- 1 | msp.py 2 | messages.h 3 | MspParser.java 4 | __pycache__/ 5 | -------------------------------------------------------------------------------- /msppg/Makefile: -------------------------------------------------------------------------------- 1 | install: messages.json msppg.py 2 | python3 msppg.py 3 | cp msp.py ../clients/ 4 | cp messages.h ../src/msp/ 5 | 6 | edit: 7 | vim msppg.py 8 | 9 | clean: 10 | rm -f msp.py messages.h MspParser.java 11 | 12 | flake: 13 | flake8 msppg.py 14 | -------------------------------------------------------------------------------- /msppg/README.md: -------------------------------------------------------------------------------- 1 | # MSPPG: Multiwii Serial Protocol Parser Generator 2 | 3 | 4 | 5 | **msppgy.py** is a standalone Python script that outputs code for parsing and generating 6 | [MSP](http://www.armazila.com/MultiwiiSerialProtocol(draft)v02.pdf) messages 7 | based on a simple JSON specification. By using this script you can avoid the 8 | lengthy and error-prone task of writing your own parsing code from scratch. 9 | Python and Java outputs are currently supported. (For C++ you can use the 10 | existing [Msp](https://github.com/simondlevy/Hackflight/blob/master/src/msp.h) class.) 11 | 12 | ## Usage 13 | 14 | Running **msppg.py** in a directory that contains a file **messages.json** will produce the following files: 15 | 16 | * **mspp.py**, a Python module containing a **Parser** class that you can subclass to implement your 17 | message-handling methods 18 | 19 | * **MspParser.java**, a Java module containing a **Parser** class that you can subclass to implement your 20 | message-handling methods 21 | 22 | ## Example 23 | 24 | The sample [messages.json](https://github.com/simondlevy/Hackflight/blob/master/parser/messages.json) 25 | file specifies 26 | 27 | ## Extending 28 | 29 | The messages.json file currently contains just a few message specifications, 30 | but you can easily add to it by specifying additional messages from the the MSP 31 | [standard](http://www.armazila.com/MultiwiiSerialProtocol(draft)v02.pdf), 32 | or add some of your own new message types. 33 | 34 | ## Caveats 35 | 36 | The Java code produced by msppg.py has not been tested recently and may not even compile. 37 | -------------------------------------------------------------------------------- /msppg/messages.json: -------------------------------------------------------------------------------- 1 | { 2 | "STATE": 3 | [{"ID": 100}, 4 | {"comment": "Vehicle state per Bouabdallah et al. 2004"}, 5 | {"x": "float"}, 6 | {"dx": "float"}, 7 | {"y": "float"}, 8 | {"dy": "float"}, 9 | {"z": "float"}, 10 | {"dz": "float"}, 11 | {"phi": "float"}, 12 | {"dphi": "float"}, 13 | {"theta": "float"}, 14 | {"dtheta": "float"}, 15 | {"psi": "float"}, 16 | {"dpsi": "float"} 17 | ], 18 | 19 | "SPIKES": 20 | [{"ID": 101}, 21 | {"comment": "Current spikes, eight per byte"}, 22 | {"byte1": "float"}, 23 | {"byte2": "float"}, 24 | {"byte3": "float"}, 25 | {"byte4": "float"}, 26 | {"byte5": "float"}, 27 | {"byte6": "float"}, 28 | {"byte7": "float"}, 29 | {"byte8": "float"} 30 | ], 31 | 32 | "SET_ARMING": 33 | [{"ID": 200}, 34 | {"comment": "Dis/arm"}, 35 | {"arm": "byte"} 36 | ], 37 | 38 | "SET_SETPOINT_RPYT": 39 | [{"ID": 201}, 40 | {"comment": "Thrust is ushort in original"}, 41 | {"roll": "float"}, 42 | {"pitch": "float"}, 43 | {"yaw": "float"}, 44 | {"thrust": "float"} 45 | ], 46 | 47 | "SET_SETPOINT_HOVER": 48 | [{"ID": 202}, 49 | {"comment": "Hover mode"}, 50 | {"vx": "float"}, 51 | {"vy": "float"}, 52 | {"yawrate": "float"}, 53 | {"zdistance": "float"} 54 | ] 55 | } 56 | -------------------------------------------------------------------------------- /rpi/.gitignore: -------------------------------------------------------------------------------- 1 | *.hpp 2 | btserver 3 | *.o 4 | -------------------------------------------------------------------------------- /rpi/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Bluetooth server running on Raspberry Pi 2 | # 3 | # Copyright (C) 2025 Simon D. Levy 4 | # 5 | # This program is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU General Public License as published by 7 | # the Free Software Foundation, in version 3. 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 | # TeNNLab framework stuff ---------------------------------------------------- 18 | 19 | ROOT = $(HOME) 20 | 21 | EDIR = $(ROOT)/embedded-neuromorphic/levy/encoder-decoder 22 | 23 | FDIR = $(ROOT)/framework 24 | 25 | NETWORK = difference_risp_train.txt 26 | 27 | INCLUDE = -I$(ROOT) -I../src 28 | 29 | standard: btserver.cpp $(ROOT)/posix-utils/*.hpp ../src/msp/*.hpp ../src/msp/messages.h 30 | g++ -Wall $(INCLUDE) -O3 -Wall btserver.cpp -o btserver 31 | 32 | # Just encoder for now (full monty would use libframework.a) 33 | snn: btserver.cpp $(ROOT)/posix-utils/*.hpp ../src/msp/*.hpp ../src/msp/messages.h difference_risp_train.hpp 34 | g++ -Wall $(INCLUDE) -I$(EDIR)/src -D_SNN -I../src -O3 -Wall btserver.cpp -o btserver 35 | 36 | difference_risp_train.hpp: 37 | $(EDIR)/compiler.py $(FDIR)/networks/$(NETWORK) 38 | 39 | ../src/msp/messages.h: ../msppg/messages.json 40 | cd ../msppg; make install; cd ../rpi 41 | 42 | run: btserver 43 | ./btserver 44 | 45 | clean: 46 | rm -f btserver *.o *.hpp 47 | 48 | edit: 49 | vim btserver.cpp 50 | -------------------------------------------------------------------------------- /src/.gitignore: -------------------------------------------------------------------------------- 1 | control.hpp 2 | -------------------------------------------------------------------------------- /src/cfassert.h: -------------------------------------------------------------------------------- 1 | /** 2 | * || ____ _ __ 3 | * +------+ / __ )(_) /_______________ _____ ___ 4 | * | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ 5 | * +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ 6 | * || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ 7 | * 8 | * Crazyflie control firmware 9 | * 10 | * Copyright (C) 2011-2012 Bitcraze AB 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | * 24 | * cfassert.h - Assert macro 25 | */ 26 | 27 | #pragma once 28 | 29 | #include 30 | 31 | #define ASSERT(e) if (e) ; else assertFail( (char *)#e, (char *)__FILE__, __LINE__ ) 32 | 33 | #define ASSERT_DMA_SAFE(PTR) if (((uint32_t)(PTR) >= 0x10000000) && ((uint32_t)(PTR) <= 0x1000FFFF)) assertFail( (char *)"", (char *)__FILE__, __LINE__ ) 34 | 35 | void assertFail(char *exp, char *file, int line); 36 | 37 | -------------------------------------------------------------------------------- /src/clock.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class Clock { 23 | 24 | public: 25 | 26 | // Permitted frequencies 27 | typedef enum { 28 | RATE_25_HZ = 25, 29 | RATE_30_HZ = 30, 30 | RATE_33_HZ = 33, 31 | RATE_50_HZ = 50, 32 | RATE_100_HZ = 100, 33 | RATE_250_HZ = 250, 34 | RATE_500_HZ = 500, 35 | RATE_1000_HZ = 1000, 36 | } rate_t ; 37 | 38 | static const rate_t RATE_MAIN_LOOP = RATE_1000_HZ; 39 | 40 | static bool rateDoExecute(const rate_t rate, const uint32_t tick) 41 | { 42 | return (tick % (RATE_MAIN_LOOP / rate)) == 0; 43 | } 44 | }; 45 | -------------------------------------------------------------------------------- /src/control/haskell.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | // Global data and routines shared with Haskell Copilot ---------------------- 25 | 26 | float stream_dt; 27 | 28 | bool stream_hovering; 29 | 30 | float stream_thrust; 31 | float stream_roll; 32 | float stream_pitch; 33 | float stream_yaw; 34 | 35 | float stream_dx; 36 | float stream_dy; 37 | float stream_z; 38 | float stream_dz; 39 | float stream_phi; 40 | float stream_dphi; 41 | float stream_theta; 42 | float stream_dtheta; 43 | float stream_psi; 44 | float stream_dpsi; 45 | 46 | float stream_landing_altitude_m; 47 | 48 | static demands_t _demands; 49 | 50 | void setDemands(float t, float r, float p, float y) 51 | { 52 | _demands.thrust = t; 53 | _demands.roll = r; 54 | _demands.pitch = p; 55 | _demands.yaw = y; 56 | } 57 | 58 | void copilot_step_core(); 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | static void runClosedLoopControl( 63 | const float dt, 64 | const bool inHoverMode, 65 | const vehicleState_t & vehicleState, 66 | const demands_t & openLoopDemands, 67 | const float landingAltitudeMeters, 68 | demands_t & demands) 69 | { 70 | stream_dt = dt; 71 | 72 | stream_hovering = inHoverMode; 73 | 74 | stream_thrust = openLoopDemands.thrust; 75 | stream_roll = openLoopDemands.roll; 76 | stream_pitch = openLoopDemands.pitch; 77 | stream_yaw = openLoopDemands.yaw; 78 | 79 | stream_dx = vehicleState.dx; 80 | stream_dy = vehicleState.dy; 81 | stream_z = vehicleState.z; 82 | stream_dz = vehicleState.dz; 83 | stream_phi = vehicleState.phi; 84 | stream_dphi = vehicleState.dphi; 85 | stream_theta = vehicleState.theta; 86 | stream_dtheta = vehicleState.dtheta; 87 | stream_psi = vehicleState.psi; 88 | stream_dpsi = vehicleState.dpsi; 89 | 90 | stream_landing_altitude_m = landingAltitudeMeters; 91 | 92 | // This will call setDemands() defined above 93 | copilot_step_core(); 94 | 95 | memcpy(&demands, &_demands, sizeof(demands_t)); 96 | } 97 | -------------------------------------------------------------------------------- /src/control/pids/altitude.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class AltitudeController { 23 | 24 | public: 25 | 26 | /** 27 | * Demand is input as altitude target in meters and output as 28 | * climb rate in meters per second. 29 | */ 30 | static float run( 31 | const bool hovering, 32 | const float dt, 33 | const float z, 34 | const float thrust) 35 | { 36 | static float _integral; 37 | 38 | const auto error = thrust - z; 39 | 40 | _integral = hovering ? 41 | Num::fconstrain(_integral + error * dt, ILIMIT) : 0; 42 | 43 | return hovering ? 44 | Num::fconstrain(KP * error + KI * _integral, 45 | fmaxf(VEL_MAX, 0.5f) * VEL_MAX_OVERHEAD) : 46 | -LANDING_SPEED_MPS; 47 | } 48 | 49 | private: 50 | 51 | static constexpr float KP = 2; 52 | static constexpr float KI = 0.5; 53 | static constexpr float ILIMIT = 5000; 54 | static constexpr float VEL_MAX = 1; 55 | static constexpr float VEL_MAX_OVERHEAD = 1.10; 56 | static constexpr float LANDING_SPEED_MPS = 0.15; 57 | }; 58 | -------------------------------------------------------------------------------- /src/control/pids/climbrate.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | #include 22 | 23 | class ClimbRateController { 24 | 25 | public: 26 | 27 | /** 28 | * Demand is input as altitude target in meters and output as 29 | * arbitrary positive value to be scaled according to motor 30 | * characteristics. 31 | */ 32 | static float run( 33 | const bool hovering, 34 | const float z0, 35 | const float dt, 36 | const float z, 37 | const float dz, 38 | const float demand) 39 | { 40 | static float _integral; 41 | 42 | const auto airborne = hovering || (z > z0); 43 | 44 | const auto error = demand - dz; 45 | 46 | _integral = airborne ? 47 | Num::fconstrain(_integral + error * dt, ILIMIT) : 0; 48 | 49 | const auto thrust = KP * error + KI * _integral; 50 | 51 | return airborne ? 52 | Num::fconstrain(thrust * THRUST_SCALE + THRUST_BASE, 53 | THRUST_MIN, THRUST_MAX) : 0; 54 | } 55 | 56 | private: 57 | 58 | static constexpr float KP = 25; 59 | static constexpr float KI = 15; 60 | static constexpr float ILIMIT = 5000; 61 | }; 62 | -------------------------------------------------------------------------------- /src/control/pids/pitchroll_angle.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class PitchRollAngleController { 23 | 24 | public: 25 | 26 | /** 27 | * Demand is input as angles in degrees and output as angular 28 | * velocities in degrees per second: 29 | * 30 | * roll: right-down positive 31 | * 32 | * pitch: nose-down positive 33 | */ 34 | static void run( 35 | const bool airborne, 36 | const float dt, 37 | const float state_phi, 38 | const float state_theta, 39 | const float demand_roll, 40 | const float demand_pitch, 41 | float & new_demand_roll, 42 | float & new_demand_pitch) 43 | { 44 | static float _roll_integral; 45 | 46 | static float _pitch_integral; 47 | 48 | new_demand_roll = runAxis(airborne, dt, demand_roll, state_phi, 49 | _roll_integral); 50 | 51 | new_demand_pitch = 52 | runAxis(airborne, dt, demand_pitch, state_theta, 53 | _pitch_integral); 54 | } 55 | 56 | private: 57 | 58 | static constexpr float KP = 6; 59 | static constexpr float KI = 3; 60 | static constexpr float ILIMIT = 20; 61 | 62 | static float runAxis( 63 | const bool airborne, 64 | const float dt, 65 | const float demand, 66 | const float measured, 67 | float & integral) 68 | { 69 | const auto error = demand - measured; 70 | 71 | integral = airborne ? 72 | Num::fconstrain(integral + error * dt, ILIMIT): 0; 73 | 74 | return airborne ? KP * error + KI * integral : 0; 75 | } 76 | }; 77 | -------------------------------------------------------------------------------- /src/control/pids/pitchroll_rate.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class PitchRollRateController { 23 | 24 | public: 25 | 26 | /** 27 | * Demands are input as angular velocities in degrees per second and 28 | * output as as arbitrary values to be scaled according to motor 29 | * characteristics: 30 | * 31 | * roll: input roll-right positive => output positive 32 | * 33 | * pitch: input nose-down positive => output positive 34 | */ 35 | static void run( 36 | const bool airborne, 37 | const float dt, 38 | const float state_dphi, const float state_dtheta, 39 | const float demand_roll,const float demand_pitch, 40 | float & new_demand_roll, float & new_demand_pitch) 41 | { 42 | static axis_t _roll; 43 | 44 | static axis_t _pitch; 45 | 46 | new_demand_roll = 47 | runAxis(airborne, dt, demand_roll, state_dphi, _roll); 48 | 49 | new_demand_pitch = 50 | runAxis(airborne, dt, demand_pitch, state_dtheta, _pitch); 51 | } 52 | 53 | private: 54 | 55 | static constexpr float KP = 125; 56 | static constexpr float KI = 250; 57 | static constexpr float KD = 1.25; 58 | static constexpr float ILIMIT = 33; 59 | 60 | typedef struct { 61 | float integral; 62 | float previous; 63 | } axis_t; 64 | 65 | static float runAxis( 66 | const bool airborne, 67 | const float dt, 68 | const float demand, 69 | const float measured, 70 | axis_t & axis) 71 | { 72 | const auto error = demand - measured; 73 | 74 | axis.integral = airborne ? 75 | Num::fconstrain( axis.integral + error * dt, ILIMIT) : 0; 76 | 77 | auto deriv = dt > 0 ? (error - axis.previous) / dt : 0; 78 | 79 | axis.previous = airborne ? error : 0; 80 | 81 | return airborne ? KP * error + KI * axis.integral + deriv : 0; 82 | } 83 | }; 84 | -------------------------------------------------------------------------------- /src/control/pids/position.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class PositionController { 23 | 24 | public: 25 | 26 | /** 27 | * Demands are input as normalized interval [-1,+1] and output as 28 | * angles in degrees. 29 | * 30 | * roll: input left positive => output negative 31 | * 32 | * pitch: input forward positive => output negative 33 | */ 34 | static void run( 35 | const bool airborne, 36 | const float dt, 37 | const float state_dx, 38 | const float state_dy, 39 | const float state_psi, 40 | const float demand_x, 41 | const float demand_y, 42 | float & roll, 43 | float & pitch) 44 | { 45 | static float _integralX; 46 | static float _integralY; 47 | 48 | // Rotate world-coordinate velocities into body coordinates 49 | const auto dxw = state_dx; 50 | const auto dyw = state_dy; 51 | const auto psi = Num::DEG2RAD * state_psi; 52 | const auto cospsi = cos(psi); 53 | const auto sinpsi = sin(psi); 54 | const auto dxb = dxw * cospsi + dyw * sinpsi; 55 | const auto dyb = -dxw * sinpsi + dyw * cospsi; 56 | 57 | // Run PIDs on body-coordinate velocities 58 | roll = runAxis(airborne, dt, demand_y, dyb, _integralY); 59 | pitch =runAxis(airborne, dt, demand_x, dxb, _integralX); 60 | } 61 | 62 | private: 63 | 64 | static constexpr float KP = 25; 65 | static constexpr float KI = 1; 66 | static constexpr float ILIMIT = 5000; 67 | static constexpr float LIMIT = 20; 68 | static constexpr float LIMIT_OVERHEAD = 1.10; 69 | 70 | static float runAxis( 71 | const bool airborne, 72 | const float dt, 73 | float demand, 74 | const float measured, 75 | float & integral) 76 | { 77 | const auto error = demand - measured; 78 | 79 | integral = airborne ? 80 | Num::fconstrain(integral + error * dt, ILIMIT) : 0; 81 | 82 | return airborne ? 83 | Num::fconstrain(KP * error + KI * integral, LIMIT) : 0; 84 | } 85 | }; 86 | -------------------------------------------------------------------------------- /src/control/pids/yaw_angle.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class YawAngleController { 23 | 24 | public: 25 | 26 | static float run( 27 | const bool airborne, 28 | const float dt, 29 | const float psi, 30 | const float yaw) 31 | { 32 | static float _target; 33 | static float _integral; 34 | static float _previous; 35 | 36 | _target = airborne ? 37 | cap(_target + DEMAND_MAX * yaw * dt) : psi; 38 | 39 | const auto error = cap(_target - psi); 40 | 41 | _integral = airborne ? 42 | Num::fconstrain(_integral + error * dt, ILIMIT) : 0; 43 | 44 | auto deriv = dt > 0 ? (error - _previous) / dt : 0; 45 | 46 | _previous = airborne ? error : 0; 47 | 48 | return airborne ? KP * error + KI * _integral + KD * deriv : 0; 49 | } 50 | 51 | private: 52 | 53 | static constexpr float KP = 6; 54 | static constexpr float KI = 1; 55 | static constexpr float KD = 0.35; 56 | static constexpr float ILIMIT = 360; 57 | static constexpr float DEMAND_MAX = 200; 58 | 59 | float _integral; 60 | float _previous; 61 | 62 | static float cap(float angle) 63 | { 64 | float result = angle; 65 | 66 | while (result > 180.0f) { 67 | result -= 360.0f; 68 | } 69 | 70 | while (result < -180.0f) { 71 | result += 360.0f; 72 | } 73 | 74 | return result; 75 | } 76 | }; 77 | -------------------------------------------------------------------------------- /src/control/pids/yaw_rate.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | class YawRateController { 23 | 24 | public: 25 | 26 | /** 27 | * Input is angular rate demand (deg/sec) and actual angular 28 | * rate from gyro; ouputput is arbitrary units scaled for motors. 29 | */ 30 | static float run( 31 | const bool airborne, 32 | const float dt, 33 | const float dpsi, 34 | const float yaw) 35 | { 36 | static float _integral; 37 | 38 | const auto error = yaw - dpsi; 39 | 40 | _integral = airborne ? 41 | Num::fconstrain(_integral + error * dt, ILIMIT) : 0; 42 | 43 | return airborne ? 44 | Num::fconstrain(KP * error + KI * _integral, OUTPUT_LIMIT) : 0; 45 | } 46 | 47 | private: 48 | 49 | static constexpr float KP = 120; 50 | static constexpr float KI = 16.7; 51 | static constexpr float ILIMIT = 166.7; 52 | static constexpr float OUTPUT_LIMIT = INT16_MAX; 53 | 54 | float _integral; 55 | }; 56 | -------------------------------------------------------------------------------- /src/control/standard.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | static void runClosedLoopControl( 35 | const float dt, 36 | const bool hovering, 37 | const vehicleState_t & vehicleState, 38 | const demands_t & openLoopDemands, 39 | const float landingAltitudeMeters, 40 | demands_t & demands) 41 | { 42 | const auto climbrate = AltitudeController::run(hovering, 43 | dt, vehicleState.z, openLoopDemands.thrust); 44 | 45 | demands.thrust = 46 | ClimbRateController::run( 47 | hovering, 48 | landingAltitudeMeters, 49 | dt, 50 | vehicleState.z, 51 | vehicleState.dz, 52 | climbrate); 53 | 54 | const auto airborne = demands.thrust > 0; 55 | 56 | const auto yaw = YawAngleController::run( 57 | airborne, dt, vehicleState.psi, openLoopDemands.yaw); 58 | 59 | demands.yaw = 60 | YawRateController::run(airborne, dt, vehicleState.dpsi, yaw); 61 | 62 | PositionController::run( 63 | airborne, 64 | dt, 65 | vehicleState.dx, vehicleState.dy, vehicleState.psi, 66 | hovering ? openLoopDemands.pitch : 0, 67 | hovering ? openLoopDemands.roll : 0, 68 | demands.roll, demands.pitch); 69 | 70 | PitchRollAngleController::run( 71 | airborne, 72 | dt, 73 | vehicleState.phi, vehicleState.theta, 74 | demands.roll, demands.pitch, 75 | demands.roll, demands.pitch); 76 | 77 | PitchRollRateController::run( 78 | airborne, 79 | dt, 80 | vehicleState.dphi, vehicleState.dtheta, 81 | demands.roll, demands.pitch, 82 | demands.roll, demands.pitch); 83 | } 84 | -------------------------------------------------------------------------------- /src/hackflight.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | -------------------------------------------------------------------------------- /src/linalg.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2018-2021 Bitcraze AB 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | * 16 | */ 17 | 18 | #pragma once 19 | 20 | #include 21 | #pragma GCC diagnostic pop 22 | 23 | static inline void mat_trans(const arm_matrix_instance_f32 * pSrc, 24 | arm_matrix_instance_f32 * pDst) 25 | { 26 | arm_mat_trans_f32(pSrc, pDst); 27 | } 28 | 29 | static inline void mat_inv(const arm_matrix_instance_f32 * pSrc, 30 | arm_matrix_instance_f32 * pDst) 31 | { 32 | arm_mat_inverse_f32(pSrc, pDst); 33 | } 34 | 35 | static inline void mat_mult(const arm_matrix_instance_f32 * pSrcA, 36 | const arm_matrix_instance_f32 * pSrcB, arm_matrix_instance_f32 * pDst) 37 | { 38 | arm_mat_mult_f32(pSrcA, pSrcB, pDst); 39 | } 40 | 41 | static inline float fast_sqrt(float32_t in) 42 | { 43 | float pOut = 0; 44 | arm_sqrt_f32(in, &pOut); 45 | return pOut; 46 | } 47 | 48 | static inline void mat_scale(const arm_matrix_instance_f32 * pSrcA, 49 | float32_t scale, arm_matrix_instance_f32 * pDst) 50 | { 51 | arm_mat_scale_f32(pSrcA, scale, pDst); 52 | } 53 | -------------------------------------------------------------------------------- /src/m_pi.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2019 - 2020 Bitcraze AB, 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | * 16 | */ 17 | 18 | #pragma once 19 | 20 | #ifndef M_PI 21 | static constexpr float M_PI = 3.14159265358979323846; 22 | #endif 23 | -------------------------------------------------------------------------------- /src/mixers/crazyflie.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Crazyflie motor mixer for Hackflight 3 | * 4 | * 4:cw 1:ccw 5 | * \ / 6 | * X 7 | * / \ 8 | * 3:ccw 2:cw 9 | * 10 | * Copyright (C) 2025 Simon D. Levy 11 | * 12 | * This program is free software: you can redistribute it and/or modify 13 | * it under the terms of the GNU General Public License as published by 14 | * the Free Software Foundation, in version 3. 15 | * 16 | * This program is distributed in the hope that it will be useful, 17 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 | * GNU General Public License for more details. 20 | * 21 | * You should have received a copy of the GNU General Public License 22 | * along with this program. If not, see . 23 | */ 24 | 25 | #pragma once 26 | 27 | #include 28 | 29 | class Mixer { 30 | 31 | public: 32 | 33 | static const uint8_t rotorCount = 4; 34 | 35 | static constexpr int8_t roll[4] = {-1, -1, +1, +1}; 36 | static constexpr int8_t pitch[4] = {-1, +1, +1, -1}; 37 | static constexpr int8_t yaw[4] = {-1, +1, -1, +1}; 38 | 39 | static void mix(const demands_t & demands, float motors[]) 40 | { 41 | auto t = demands.thrust; 42 | auto r = demands.roll; 43 | auto p = demands.pitch; 44 | auto y = demands.yaw; 45 | 46 | motors[0] = t - r - p + y; 47 | motors[1] = t - r + p - y; 48 | motors[2] = t + r + p + y; 49 | motors[3] = t + r - p - y; 50 | } 51 | }; 52 | 53 | -------------------------------------------------------------------------------- /src/motors.h: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | int motorsGetRatio(uint32_t id); 23 | void motorsInit(void); 24 | bool motorsTest(void); 25 | void motorsSetRatios(const uint16_t ratios[]); 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | // can be called from nvic.c 32 | void motorsStop(); 33 | 34 | #ifdef __cplusplus 35 | } 36 | #endif 37 | -------------------------------------------------------------------------------- /src/msp/.gitignore: -------------------------------------------------------------------------------- 1 | messages.h 2 | -------------------------------------------------------------------------------- /src/safety.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * 3 | * Copyright (C) 2011-2022 Bitcraze AB, 2025 Simon D. Levy 4 | * 5 | * This program is free software: you can redistribute it and/or modify 6 | * it under the terms of the GNU General Public License as published by 7 | * the Free Software Foundation, in version 3. 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 | 18 | #pragma once 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | class Safety { 27 | 28 | public: 29 | 30 | bool isFlying() 31 | { 32 | return _is_flying; 33 | } 34 | 35 | bool isArmed() 36 | { 37 | return _is_armed; 38 | } 39 | 40 | void requestArming(const bool doArm) 41 | { 42 | _is_armed = doArm ? _is_safe_to_arm : false; 43 | } 44 | 45 | void update( 46 | const uint32_t coreStep, 47 | const uint32_t timestamp, 48 | const vehicleState_t & vehicleState) 49 | { 50 | if (Clock::rateDoExecute(CLOCK_RATE, coreStep)) { 51 | 52 | _is_flying = isFlyingCheck(xTaskGetTickCount()); 53 | 54 | _is_safe_to_arm =safeAngle(vehicleState.phi) && 55 | safeAngle(vehicleState.theta); 56 | } 57 | } 58 | 59 | private: 60 | 61 | static const Clock::rate_t CLOCK_RATE = Clock::RATE_25_HZ; 62 | 63 | static const uint32_t IS_FLYING_HYSTERESIS_THRESHOLD = M2T(2000); 64 | 65 | static constexpr float MAX_SAFE_ANGLE = 30; 66 | 67 | bool _is_flying; 68 | 69 | bool _is_safe_to_arm; 70 | 71 | bool _is_armed; 72 | 73 | static bool safeAngle(float angle) 74 | { 75 | return fabs(angle) < MAX_SAFE_ANGLE; 76 | } 77 | 78 | // 79 | // We say we are flying if one or more motors are running over the idle 80 | // thrust. 81 | // 82 | static bool isFlyingCheck(const uint32_t tick) 83 | { 84 | auto isThrustOverIdle = false; 85 | 86 | for (int i = 0; i < 4; ++i) { 87 | if (motorsGetRatio(i) > 0) { 88 | isThrustOverIdle = true; 89 | break; 90 | } 91 | } 92 | 93 | static uint32_t latestThrustTick; 94 | 95 | if (isThrustOverIdle) { 96 | latestThrustTick = tick; 97 | } 98 | 99 | bool result = false; 100 | if (0 != latestThrustTick) { 101 | if ((tick - latestThrustTick) < IS_FLYING_HYSTERESIS_THRESHOLD) { 102 | result = true; 103 | } 104 | } 105 | 106 | return result; 107 | } 108 | }; 109 | 110 | -------------------------------------------------------------------------------- /src/system.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2011-2018 Bitcraze AB, 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | void systemInit(const uint8_t led_pin, const uint8_t flowdeck_cs_pin); 18 | 19 | const bool systemIsLedInverted(); 20 | 21 | void systemReportForever(const char * msg); 22 | 23 | bool systemUartReadByte(uint8_t *); 24 | 25 | void systemUartWriteByte(const uint8_t byte); 26 | 27 | void systemWaitStart(void); 28 | -------------------------------------------------------------------------------- /src/task.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2011-2018 Bitcraze AB, 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | 24 | class FreeRtosTask { 25 | 26 | private: 27 | 28 | typedef void (*taskfun_t)(void * obj); 29 | 30 | static const auto STACKSIZE = 3 * configMINIMAL_STACK_SIZE; // arbitrary 31 | 32 | StackType_t _taskStackBuffer[STACKSIZE]; 33 | 34 | StaticTask_t _taskTaskBuffer; 35 | 36 | bool _didInit; 37 | 38 | public: 39 | 40 | void init( 41 | const taskfun_t fun, 42 | const char * name, 43 | void * obj, 44 | const uint8_t priority 45 | ) 46 | { 47 | xTaskCreateStatic( 48 | fun, 49 | name, 50 | STACKSIZE, 51 | obj, 52 | priority, 53 | _taskStackBuffer, 54 | &_taskTaskBuffer); 55 | 56 | _didInit = true; 57 | } 58 | 59 | bool didInit() 60 | { 61 | return _didInit; 62 | } 63 | }; 64 | -------------------------------------------------------------------------------- /src/tasks/debug.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | #include 22 | 23 | class DebugTask { 24 | 25 | #if defined(ARDUINO) 26 | 27 | public: 28 | 29 | void begin() 30 | { 31 | if (_task.didInit()){ 32 | return; 33 | } 34 | 35 | _task.init(runDebugCommsTask, "debug", this, 2); 36 | } 37 | 38 | void setMessage(const char * format, ...) 39 | { 40 | va_list args = {}; 41 | 42 | char buffer[256] = {}; 43 | 44 | va_start(args, format); 45 | 46 | const auto vsErr = vsprintf(buffer, format, args); 47 | 48 | if (vsErr >= 0) { 49 | strcpy(_msg, buffer); 50 | } 51 | 52 | va_end(args); 53 | } 54 | 55 | private: 56 | 57 | static constexpr float REPORT_FREQ = 100; 58 | 59 | FreeRtosTask _task; 60 | 61 | char _msg[100]; 62 | 63 | static void runDebugCommsTask(void * obj) 64 | { 65 | ((DebugTask *)obj)->run(); 66 | } 67 | 68 | void run(void) 69 | { 70 | systemWaitStart(); 71 | 72 | while (true) { 73 | 74 | if (*_msg) { 75 | Serial.println(_msg); 76 | } 77 | 78 | vTaskDelay(M2T(1000/REPORT_FREQ)); 79 | } 80 | } 81 | 82 | #else 83 | public: 84 | 85 | void begin() 86 | { 87 | } 88 | 89 | void setMessage(const char * format, ...) 90 | { 91 | (void)format; 92 | } 93 | #endif 94 | }; 95 | 96 | -------------------------------------------------------------------------------- /src/tasks/led.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | class LedTask { 28 | 29 | public: 30 | 31 | void begin(Safety * safety, const uint8_t pin) 32 | { 33 | if (_task.didInit()){ 34 | return; 35 | } 36 | 37 | _pin = pin; 38 | 39 | _task.init(runLedCommsTask, "led", this, 2); 40 | 41 | pinMode(_pin, OUTPUT); 42 | 43 | set(LOW); 44 | 45 | _safety = safety; 46 | } 47 | 48 | private: 49 | 50 | static constexpr float HEARTBEAT_HZ = 1; 51 | 52 | static constexpr uint32_t PULSE_MSEC = 50; 53 | 54 | uint8_t _pin; 55 | 56 | FreeRtosTask _task; 57 | 58 | Safety * _safety; 59 | 60 | static void runLedCommsTask(void * obj) 61 | { 62 | ((LedTask *)obj)->run(); 63 | } 64 | 65 | void run(void) 66 | { 67 | systemWaitStart(); 68 | 69 | TickType_t lastWakeTime = xTaskGetTickCount(); 70 | 71 | while (true) { 72 | 73 | if (_safety->isArmed()) { 74 | set(true); 75 | } 76 | else { 77 | set(true); 78 | vTaskDelay(M2T(PULSE_MSEC)); 79 | set(false); 80 | vTaskDelayUntil(&lastWakeTime, M2T(1000/HEARTBEAT_HZ)); 81 | } 82 | 83 | } 84 | } 85 | 86 | void set(const bool on) 87 | { 88 | digitalWrite(_pin, systemIsLedInverted() ? !on : on); 89 | } 90 | }; 91 | -------------------------------------------------------------------------------- /src/tasks/logger.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | class LoggerTask { 28 | 29 | public: 30 | 31 | void begin(EstimatorTask * estimatorTask) 32 | { 33 | if (_task.didInit()){ 34 | return; 35 | } 36 | 37 | _estimatorTask = estimatorTask; 38 | 39 | _task.init(runLoggerTask, "logger", this, 3); 40 | } 41 | 42 | private: 43 | 44 | static constexpr float FREQ_HZ = 100; 45 | 46 | static void runLoggerTask(void * obj) 47 | { 48 | ((LoggerTask *)obj)->run(); 49 | } 50 | 51 | FreeRtosTask _task; 52 | 53 | EstimatorTask * _estimatorTask; 54 | 55 | void run(void) 56 | { 57 | systemWaitStart(); 58 | 59 | TickType_t lastWakeTime = xTaskGetTickCount(); 60 | 61 | MspSerializer serializer = {}; 62 | 63 | while (true) { 64 | 65 | vehicleState_t state = {}; 66 | 67 | _estimatorTask->getVehicleState(&state); 68 | 69 | serializer.serializeFloats(MSP_STATE, (float *)&state, 12); 70 | 71 | for (uint8_t k=0; k. 17 | */ 18 | 19 | #pragma once 20 | 21 | #include 22 | 23 | // Approximate thrust needed when in perfect hover. More weight/older 24 | // battery can use a higher value 25 | static constexpr float THRUST_BASE = 36000; 26 | static constexpr float THRUST_MIN = 20000; 27 | static constexpr float THRUST_MAX = UINT16_MAX; 28 | static constexpr float THRUST_SCALE = 1000; 29 | 30 | static constexpr Dynamics::vehicle_params_t VPARAMS = { 31 | 32 | // Actual values 33 | 1.0e-1, // mass [kg] 34 | 5.0e-2, // arm length L [m] 35 | 36 | // Estimated by using thrust constants above 37 | 1.4e-8, // force coefficient B [F=b*w^2] 38 | 7.0e-9, // drag coefficient D [T=d*w^2] 39 | 2.0e-5 // I [kg*m^2] // pitch, roll 40 | }; 41 | -------------------------------------------------------------------------------- /webots/.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__/ 2 | *.wbproj 3 | *.egg-info 4 | *.swp 5 | *.pth 6 | -------------------------------------------------------------------------------- /webots/README.md: -------------------------------------------------------------------------------- 1 |

2 | 3 |

4 | 5 | ## Hackflight simulator instructions 6 | 7 | 1. [Install Webots](https://cyberbotics.com/doc/guide/installation-procedure#installation-on-linux) 8 | on your computer. 9 | 10 | 2. Add the following line to your ```~/.bashrc``` file: 11 | 12 | ``` 13 | export WEBOTS_HOME=/usr/local/webots 14 | ``` 15 | 16 | 3. From the hackflight main directory, do the following: 17 | 18 | ``` 19 | cd webots/plugins/physics/standard 20 | make 21 | cd ../../../controllers/controller 22 | make 23 | make runstandard 24 | ``` 25 | 26 | If you have a game controller or R/C transmitter with adapter dongle, you can 27 | use that to fly (right shoulder button to takeoff and land); otherwise, the 28 | simulator will advise you that no such device was found and instruct you on how 29 | to fly with the keyboard. The following devices are currently supported: 30 | 31 | * [MGEAR Wired Controller for PS3](https://www.officedepot.com/a/products/7123231/Gear-Wired-Controller-For-PS3-Black/) 32 | * [Logitech Gamepad F310](https://www.amazon.com/gp/product/B003VAHYQY) 33 | * [Xbox Series S|X Controller](https://a.co/d/7QbSZaZ) 34 | 35 | ## Design principles 36 | 37 | The [basic time step](https://cyberbotics.com/doc/reference/worldinfo) in 38 | Webots is 32 milliseonds, which translates to an update rate of around 31 Hz 39 | in the control code (e.g., PID control). Although adequate for many robot 40 | applications, this rate is much slower than the typical rate of 500 - 1000 Hz 41 | used in actual flight controllers. Although it is possible to decrease the 42 | timestep in order to increase the update rate, there is as usual a 43 | [tradeoff](https://robotics.stackexchange.com/questions/24086/how-can-i-speed-up-my-webots-simulation), 44 | in which a faster update rate can slow down the simulation. Fortunately, Webots supports 45 | [custom physics plugins](https://cyberbotics.com/doc/reference/physics-plugin), 46 | in which you can replace Webots' built-in physics with a physics model 47 | of your own. The Hackflight simulator exploits this feature of Webots, 48 | using a 49 | [dynamics model](https://github.com/simondlevy/Hackflight/blob/master/src/dynamics.hpp) 50 | based on the equestions 51 | presented in this [paper](https://infoscience.epfl.ch/record/97532/files/325.pdf). 52 | The webots update step represents a slow, outer loop, in which is embeded 53 | a faster control loop, which in turn embeds an even faster dynamics loop. 54 | The following figure illustrates this arrangement, using a traditional 55 | [cascade-control](https://controlguru.com/the-cascade-control-architecture/) 56 | diagram: 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /webots/controllers/controller/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | controller 3 | *.csv 4 | -------------------------------------------------------------------------------- /webots/controllers/controller/Makefile: -------------------------------------------------------------------------------- 1 | # Copyright (C) 2025 Simon D. Levy 2 | 3 | # This program is free software: you can redistribute it and/or modify 4 | # it under the terms of the GNU General Public License as published by 5 | # the Free Software Foundation, in version 3. 6 | # 7 | # This program is distributed in the hope that it will be useful, 8 | # but WITHOUT ANY WARRANTY without even the implied warranty of 9 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 10 | # GNU General Public License for more details. 11 | # 12 | # You should have received a copy of the GNU General Public License 13 | # along with this program. If not, see . 14 | 15 | # TeNNLab framework stuff ---------------------------------------------------- 16 | 17 | FDIR = $(HOME)/Desktop/framework 18 | 19 | VIZ_NET = $(FDIR)/networks/difference_risp_train.txt 20 | 21 | VIZ_PORT = 8100 22 | 23 | # ----------------------------------------------------------------------------- 24 | 25 | CFLAGS = -O3 -std=c++11 -Wall -Wextra 26 | 27 | SRCDIR = ../../../src 28 | INCLUDE = -I$(SRCDIR) 29 | 30 | NAME = controller 31 | 32 | CXX_SOURCES = controller.cpp 33 | 34 | all: $(NAME) 35 | 36 | # Use the Webots C API in your C++ controller program 37 | USE_C_API = true 38 | 39 | VERBOSE = 1 40 | 41 | runstandard: 42 | cp ../../plugins/physics/standard/libstandard.so ../../plugins/physics/custom/libcustom.so 43 | webots ../../worlds/simple.wbt & 44 | /usr/local/webots/webots-controller --stdout-redirect $(NAME) 45 | 46 | runhaskell: 47 | cp ../../plugins/physics/haskell/libhaskell.so ../../plugins/physics/custom/libcustom.so 48 | webots ../../worlds/simple.wbt & 49 | /usr/local/webots/webots-controller --stdout-redirect $(NAME) 50 | 51 | runsnn: 52 | cp ../../plugins/physics/snn/libsnn.so ../../plugins/physics/custom/libcustom.so 53 | $(HOME)/Desktop/spikeplotter/spikeplot.py -f $(VIZ_NET) -a localhost \ 54 | -p $(VIZ_PORT) --ids '0,1,2,3,4,10,11,20' --display-counts & 55 | webots ../../worlds/simple.wbt & 56 | /usr/local/webots/webots-controller --stdout-redirect $(NAME) 57 | 58 | 59 | ### Do not modify: this includes Webots global Makefile.include 60 | null := 61 | space := $(null) $(null) 62 | WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 63 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 64 | 65 | edit: 66 | vim main.cpp 67 | 68 | -------------------------------------------------------------------------------- /webots/controllers/controller/newplot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | standard = np.loadtxt('standard.csv', skiprows=6 ) 7 | snn = np.loadtxt('snn.csv', skiprows=6) 8 | plt.plot(standard) 9 | plt.plot(snn) 10 | plt.legend(('standard', 'snn')) 11 | plt.show() 12 | 13 | 14 | -------------------------------------------------------------------------------- /webots/controllers/controller/plot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | 6 | data = np.loadtxt('climbsnn.csv', delimiter=',', skiprows=2) 7 | 8 | snn = data[:,0] 9 | pid = data[:,1] 10 | 11 | m,b = np.polyfit(snn, pid, 1) 12 | 13 | fit = snn * m + b 14 | 15 | plt.plot(pid) 16 | plt.plot(snn) 17 | plt.plot(fit) 18 | 19 | model = 'm = %f b = %f' % (m, b) 20 | 21 | print(model) 22 | 23 | plt.title(model) 24 | 25 | plt.legend(('PID', 'SNN', 'FIT')) 26 | 27 | plt.show() 28 | -------------------------------------------------------------------------------- /webots/meshes/stl_files/prop_ccw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/webots/meshes/stl_files/prop_ccw.stl -------------------------------------------------------------------------------- /webots/meshes/stl_files/prop_cw.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/webots/meshes/stl_files/prop_cw.stl -------------------------------------------------------------------------------- /webots/meshes/textures/fast_helix.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/simondlevy/Hackflight/a921bc3369c04620ed552564bde03e8ed674d2ba/webots/meshes/textures/fast_helix.png -------------------------------------------------------------------------------- /webots/plugins/physics/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.so 3 | -------------------------------------------------------------------------------- /webots/plugins/physics/custom/.gitignore: -------------------------------------------------------------------------------- 1 | libcustom.so 2 | -------------------------------------------------------------------------------- /webots/plugins/physics/haskell/.gitignore: -------------------------------------------------------------------------------- 1 | copilot* 2 | sim_control.hpp 3 | -------------------------------------------------------------------------------- /webots/plugins/physics/haskell/Makefile: -------------------------------------------------------------------------------- 1 | # Custom physics plugin for Hackflight simulator using NASA Copilit Haskell 2 | # compiler 3 | # 4 | # Copyright (C) 2025 Simon D. Levy 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, in version 3. 9 | # 10 | # This program is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | # 15 | # You should have received a copy of the GNU General Public License 16 | # along with this program. If not, see . 17 | 18 | SRC = ../../../../src 19 | 20 | CFLAGS = -O3 -std=c++11 -Wall -Wextra 21 | 22 | INCLUDE = -I$(SRC) 23 | 24 | HASKDIR = ../../../../haskell 25 | 26 | CXX_SOURCES = ../support.cpp copilot_core.cpp 27 | 28 | FILES_TO_REMOVE += copilot* sim_control.hpp 29 | 30 | all: libhaskell.so 31 | 32 | copilot_core.cpp : $(HASKDIR)/*.hs $(HASKDIR)/pids/*.hs 33 | cp $(SRC)/control/haskell.hpp ./sim_control.hpp 34 | runhaskell -i$(HASKDIR) -i$(HASKDIR)/pids $(HASKDIR)/Pids.hs -i$(PWD) 35 | mv copilot_core.c copilot_core.cpp 36 | 37 | ### Do not modify: this includes Webots global Makefile.include 38 | null := 39 | space := $(null) $(null) 40 | WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 41 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 42 | -------------------------------------------------------------------------------- /webots/plugins/physics/haskell/sim_control.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2025 Simon D. Levy 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, in version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, 9 | * but WITHOUT ANY WARRANTY without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | * GNU General Public License for more details. 12 | * 13 | * You should have received a copy of the GNU General Public License 14 | * along with this program. If not, see . 15 | */ 16 | 17 | #pragma once 18 | 19 | #include 20 | #include 21 | 22 | #include 23 | 24 | // Global data and routines shared with Haskell Copilot ---------------------- 25 | 26 | float stream_dt; 27 | 28 | bool stream_hovering; 29 | 30 | float stream_thrust; 31 | float stream_roll; 32 | float stream_pitch; 33 | float stream_yaw; 34 | 35 | float stream_dx; 36 | float stream_dy; 37 | float stream_z; 38 | float stream_dz; 39 | float stream_phi; 40 | float stream_dphi; 41 | float stream_theta; 42 | float stream_dtheta; 43 | float stream_psi; 44 | float stream_dpsi; 45 | 46 | float stream_landing_altitude_m; 47 | 48 | static demands_t _demands; 49 | 50 | void setDemands(float t, float r, float p, float y) 51 | { 52 | _demands.thrust = t; 53 | _demands.roll = r; 54 | _demands.pitch = p; 55 | _demands.yaw = y; 56 | } 57 | 58 | void copilot_step_core(); 59 | 60 | // --------------------------------------------------------------------------- 61 | 62 | static void runClosedLoopControl( 63 | const float dt, 64 | const bool inHoverMode, 65 | const vehicleState_t & vehicleState, 66 | const demands_t & openLoopDemands, 67 | const float landingAltitudeMeters, 68 | demands_t & demands) 69 | { 70 | stream_dt = dt; 71 | 72 | stream_hovering = inHoverMode; 73 | 74 | stream_thrust = openLoopDemands.thrust; 75 | stream_roll = openLoopDemands.roll; 76 | stream_pitch = openLoopDemands.pitch; 77 | stream_yaw = openLoopDemands.yaw; 78 | 79 | stream_dx = vehicleState.dx; 80 | stream_dy = vehicleState.dy; 81 | stream_z = vehicleState.z; 82 | stream_dz = vehicleState.dz; 83 | stream_phi = vehicleState.phi; 84 | stream_dphi = vehicleState.dphi; 85 | stream_theta = vehicleState.theta; 86 | stream_dtheta = vehicleState.dtheta; 87 | stream_psi = vehicleState.psi; 88 | stream_dpsi = vehicleState.dpsi; 89 | 90 | stream_landing_altitude_m = landingAltitudeMeters; 91 | 92 | // This will call setDemands() defined above 93 | copilot_step_core(); 94 | 95 | memcpy(&demands, &_demands, sizeof(demands_t)); 96 | } 97 | -------------------------------------------------------------------------------- /webots/plugins/physics/snn/.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.so 3 | -------------------------------------------------------------------------------- /webots/plugins/physics/snn/Makefile: -------------------------------------------------------------------------------- 1 | # Custom physics plugin for Hackflight simulator using TeNNLab Spiking Neural 2 | # networks for control 3 | # 4 | # Copyright (C) 2025 Simon D. Levy 5 | # 6 | # This program is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU General Public License as published by 8 | # the Free Software Foundation, in version 3. 9 | # 10 | # This program is distributed in the hope that it will be useful, 11 | # but WITHOUT ANY WARRANTY without even the implied warranty of 12 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 13 | # GNU General Public License for more details. 14 | # 15 | # You should have received a copy of the GNU General Public License 16 | # along with this program. If not, see . 17 | 18 | CFLAGS = -O3 -std=c++11 -Wall -Wextra 19 | 20 | INCLUDE = -I../../../simsrc -I../../../../src 21 | 22 | CXX_SOURCES = ../support.cpp 23 | 24 | # TeNNLab framework stuff ---------------------------------------------------- 25 | 26 | ROOT = $(HOME)/Desktop 27 | 28 | FDIR = $(HOME)/Desktop/framework 29 | 30 | PDIR = $(FDIR)/processors/risp 31 | 32 | INCLUDE += -I$(ROOT) -I$(FDIR)/include -I$(FDIR)/include/utils/levy -I$(FDIR)/cpp-apps/include 33 | CXX_SOURCES += $(FDIR)/src/io_stream.cpp 34 | CXX_SOURCES += $(FDIR)/src/jspace.cpp 35 | CXX_SOURCES += $(FDIR)/src/network.cpp 36 | CXX_SOURCES += $(FDIR)/src/properties.cpp 37 | CXX_SOURCES += $(FDIR)/src/spike_encoding.cpp 38 | 39 | INCLUDE += -I$(PDIR)/include 40 | CXX_SOURCES += $(PDIR)/src/processor.cpp 41 | CXX_SOURCES += $(PDIR)/src/static_proc.cpp 42 | 43 | VIZ_NET = $(shell pwd)/networks/difference_risp_train.txt 44 | VIZ_HOST = "localhost" 45 | VIZ_PORT = 8100 46 | 47 | # ---------------------------------------------------------------------------- 48 | 49 | all: libsnn.so 50 | 51 | ### Do not modify: this includes Webots global Makefile.include 52 | null := 53 | space := $(null) $(null) 54 | WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 55 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 56 | -------------------------------------------------------------------------------- /webots/plugins/physics/standard/.gitignore: -------------------------------------------------------------------------------- 1 | sim_control.hpp 2 | -------------------------------------------------------------------------------- /webots/plugins/physics/standard/Makefile: -------------------------------------------------------------------------------- 1 | # Custom physics plugin for Hackflight simulator 2 | # 3 | # Copyright (C) 2025 Simon D. Levy 4 | # 5 | # This program is free software: you can redistribute it and/or modify 6 | # it under the terms of the GNU General Public License as published by 7 | # the Free Software Foundation, in version 3. 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 | SRC = ../../../../src 18 | 19 | CFLAGS = -O3 -std=c++11 -Wall -Wextra 20 | 21 | INCLUDE = -I$(SRC) 22 | 23 | CXX_SOURCES = ../support.cpp 24 | 25 | FILES_TO_REMOVE += sim_control.hpp 26 | 27 | all: libstandard.so 28 | 29 | ../support.cpp: ./control.hpp 30 | 31 | ./control.hpp: 32 | cp $(SRC)/control/standard.hpp ./sim_control.hpp 33 | 34 | ### Do not modify: this includes Webots global Makefile.include 35 | null := 36 | space := $(null) $(null) 37 | WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME)))) 38 | include $(WEBOTS_HOME_PATH)/resources/Makefile.include 39 | -------------------------------------------------------------------------------- /webots/worlds/.gitignore: -------------------------------------------------------------------------------- 1 | *.jpg 2 | -------------------------------------------------------------------------------- /webots/worlds/simple.wbt: -------------------------------------------------------------------------------- 1 | #VRML_SIM R2025a utf8 2 | 3 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023b/projects/objects/backgrounds/protos/TexturedBackground.proto" 4 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/backgrounds/protos/TexturedBackgroundLight.proto" 5 | EXTERNPROTO "https://raw.githubusercontent.com/cyberbotics/webots/R2023a/projects/objects/floors/protos/Floor.proto" 6 | EXTERNPROTO "../protos/DiyQuad.proto" 7 | 8 | WorldInfo { 9 | title "Hackflight" 10 | physics "custom" 11 | contactProperties [ 12 | ContactProperties { 13 | coulombFriction [ 14 | 0.6 15 | ] 16 | bounce 0.4 17 | bounceVelocity 0 18 | } 19 | ] 20 | } 21 | Viewpoint { 22 | orientation -0.025277270831774102 0.9972663450424153 -0.0694326769254908 0.8441882808413657 23 | position -1.9588528414498076 0.14576258030523587 2.1801138888194225 24 | bloomThreshold 100 25 | } 26 | DEF diyquad DiyQuad { 27 | hidden linearVelocity_0 -9.131961233159464e-11 1.6003935021107788e-10 0.07881708229394113 28 | hidden angularVelocity_0 -1.2690808498329498e-08 -7.2414672313287404e-09 8.185844434928605e-19 29 | translation -2.9222275946110286e-12 5.121259206754492e-12 0.0025221466334061165 30 | rotation -0.8685502718007159 -0.49560107481209387 5.602336060631194e-11 4.67567491637056e-10 31 | } 32 | TexturedBackground { 33 | } 34 | TexturedBackgroundLight { 35 | } 36 | Floor { 37 | } 38 | --------------------------------------------------------------------------------