├── gcc ├── methods.c ├── progStLink.sh ├── Makefile └── flash.ld ├── Silverware ├── src │ ├── buzzer.h │ ├── drv_serial.h │ ├── project.h │ ├── drv_adc.h │ ├── drv_gpio.h │ ├── flip_sequencer.h │ ├── drv_fmc2.h │ ├── drv_time.h │ ├── sixaxis.h │ ├── debug.h │ ├── rx.h │ ├── pid.h │ ├── control.h │ ├── drv_i2c.h │ ├── drv_spi.h │ ├── drv_hw_i2c.h │ ├── drv_pwm.h │ ├── gestures.h │ ├── rx_bayang.h │ ├── drv_fmc.h │ ├── drv_softi2c.h │ ├── util.h │ ├── led.h │ ├── drv_spi_none.c │ ├── drv_fmc2.c │ ├── angle_pid.c │ ├── drv_fmc1.c │ ├── drv_clk.c │ ├── stickvector.c │ ├── drv_xn297_3wire.c │ ├── drv_xn297.c │ ├── drv_rgb.c │ ├── drv_gpio.c │ ├── defines.h │ ├── xn297.h │ ├── flash.c │ ├── buzzer.c │ ├── drv_i2c.c │ ├── drv_serial.c │ ├── drv_spi.c │ ├── motorcurve.c │ ├── osd_ltm.c │ ├── drv_adc.c │ ├── flip_sequencer.c │ ├── led.c │ ├── drv_spi_3wire.c │ ├── drv_time.c │ ├── util.c │ ├── imu.c │ ├── rgb_led.c │ ├── drv_dshot.h │ ├── gestures.c │ ├── rx_h7_protocol.c │ ├── binary.h │ ├── config.h │ ├── drv_hw_i2c.c │ └── rx_cx10blue_protocol.c └── EventRecorderStub.scvd ├── bin ├── e011_factory_firmware.bin └── readme.txt ├── Utilities ├── STM32_Nucleo │ └── Release_Notes.html └── stm32f0xx_conf.h ├── Libraries ├── CMSIS │ ├── CMSIS END USER LICENCE AGREEMENT.pdf │ ├── Lib │ │ └── license.txt │ ├── Device │ │ └── ST │ │ │ └── STM32F0xx │ │ │ └── Include │ │ │ └── system_stm32f0xx.h │ └── Include │ │ ├── arm_const_structs.h │ │ └── arm_common_tables.h └── STM32F0xx_StdPeriph_Driver │ ├── Release_Notes.html │ ├── MCD-ST Liberty SW License Agreement V2.pdf │ ├── inc │ ├── stm32f0xx_wwdg.h │ ├── stm32f0xx_dbgmcu.h │ ├── stm32f0xx_misc.h │ ├── stm32f0xx_iwdg.h │ ├── stm32f0xx_crc.h │ └── stm32f0xx_pwr.h │ └── src │ ├── stm32f0xx_misc.c │ └── stm32f0xx_dbgmcu.c ├── .gitattributes ├── .gitignore ├── .travis.yml ├── LICENSE ├── README.md └── INSTALL.md /gcc/methods.c: -------------------------------------------------------------------------------- 1 | void _sbrk() {} -------------------------------------------------------------------------------- /Silverware/src/buzzer.h: -------------------------------------------------------------------------------- 1 | void buzzer( void); 2 | 3 | 4 | -------------------------------------------------------------------------------- /Silverware/src/drv_serial.h: -------------------------------------------------------------------------------- 1 | void serial_init(void); 2 | 3 | 4 | -------------------------------------------------------------------------------- /Silverware/src/project.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | #include "stm32f0xx.h" 5 | -------------------------------------------------------------------------------- /Silverware/src/drv_adc.h: -------------------------------------------------------------------------------- 1 | 2 | void adc_init(void); 3 | float adc_read(int channel); 4 | 5 | -------------------------------------------------------------------------------- /Silverware/src/drv_gpio.h: -------------------------------------------------------------------------------- 1 | 2 | void gpio_init(void); 3 | int gpio_init_fpv(void); 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /bin/e011_factory_firmware.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/silver13/Eachine-E011/HEAD/bin/e011_factory_firmware.bin -------------------------------------------------------------------------------- /bin/readme.txt: -------------------------------------------------------------------------------- 1 | Precompiled binaries: 2 | 3 | e011_factory_firmware.bin - original firmware which can be flashed back to the quad -------------------------------------------------------------------------------- /Utilities/STM32_Nucleo/Release_Notes.html: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/silver13/Eachine-E011/HEAD/Utilities/STM32_Nucleo/Release_Notes.html -------------------------------------------------------------------------------- /Silverware/src/flip_sequencer.h: -------------------------------------------------------------------------------- 1 | 2 | void start_flip( void); 3 | 4 | 5 | void flip_sequencer(void); 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Libraries/CMSIS/CMSIS END USER LICENCE AGREEMENT.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/silver13/Eachine-E011/HEAD/Libraries/CMSIS/CMSIS END USER LICENCE AGREEMENT.pdf -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/Release_Notes.html: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/silver13/Eachine-E011/HEAD/Libraries/STM32F0xx_StdPeriph_Driver/Release_Notes.html -------------------------------------------------------------------------------- /Silverware/src/drv_fmc2.h: -------------------------------------------------------------------------------- 1 | 2 | int flash2_fmc_write( int data1 , int data2); 3 | int flash2_readdata( unsigned int data ); 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /Silverware/src/drv_time.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | void time_init(void); 4 | unsigned long gettime(void); 5 | 6 | void delay(uint32_t data); 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/MCD-ST Liberty SW License Agreement V2.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/silver13/Eachine-E011/HEAD/Libraries/STM32F0xx_StdPeriph_Driver/MCD-ST Liberty SW License Agreement V2.pdf -------------------------------------------------------------------------------- /Silverware/src/sixaxis.h: -------------------------------------------------------------------------------- 1 | 2 | void sixaxis_init( void); 3 | int sixaxis_check( void); 4 | void sixaxis_read( void); 5 | void gyro_read( void); 6 | void gyro_cal( void); 7 | 8 | void acc_cal(void); 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /Silverware/src/debug.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | typedef struct debug 4 | { 5 | int gyroid; 6 | float vbatt_comp; 7 | float adcfilt; 8 | float totaltime; 9 | float timefilt; 10 | float adcreffilt; 11 | } debug_type; 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /Silverware/src/rx.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | void rx_init(void); 4 | void checkrx( void); 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /Silverware/src/pid.h: -------------------------------------------------------------------------------- 1 | 2 | float pid( int x ); 3 | int next_pid_term( void); // Return value : 0 - p, 1 - i, 2 - d 4 | int next_pid_axis( void); // Return value : 0 - Roll, 1 - Pitch, 2 - Yaw 5 | int increase_pid( void ); 6 | int decrease_pid( void ); 7 | void pid_precalc( void); 8 | 9 | 10 | -------------------------------------------------------------------------------- /Silverware/src/control.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | void control( void); 4 | float clip_ff(float motorin, int number); 5 | float motorfilter( float motorin ,int number); 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /Silverware/src/drv_i2c.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | void i2c_init( void); 10 | int i2c_readdata( int reg, int *data, int size ); 11 | int i2c_readreg( int reg ); 12 | void i2c_writereg( int reg ,int data); 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /Silverware/src/drv_spi.h: -------------------------------------------------------------------------------- 1 | 2 | // soft spi header file 3 | // 4 | #include 5 | 6 | void spi_init(void); 7 | void spi_cson(void); 8 | void spi_csoff(void); 9 | void spi_sendbyte( int ); 10 | int spi_sendrecvbyte( int); 11 | int spi_sendzerorecvbyte( void ); 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /Silverware/src/drv_hw_i2c.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | 5 | 6 | 7 | 8 | 9 | void hw_i2c_init( void); 10 | int hw_i2c_readdata( int reg, int *data, int size ); 11 | int hw_i2c_readreg( int reg ); 12 | void hw_i2c_writereg( int reg ,int data); 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /Silverware/src/drv_pwm.h: -------------------------------------------------------------------------------- 1 | 2 | #include 3 | 4 | void pwm_init(void); 5 | void pwm_set( uint8_t number , float pwm); 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /Silverware/EventRecorderStub.scvd: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /Silverware/src/gestures.h: -------------------------------------------------------------------------------- 1 | 2 | int gestures2( void); 3 | int gesture_sequence( int gesture); 4 | 5 | // warning: using if x>GESTURE_UDR to check pid gestures 6 | enum gestures_enum{ 7 | GESTURE_NONE = 0, 8 | GESTURE_DDD, 9 | GESTURE_UUU, 10 | GESTURE_LLD, 11 | GESTURE_RRD, 12 | GESTURE_UDU, 13 | GESTURE_UDD, 14 | GESTURE_UDR, 15 | GESTURE_UDL 16 | 17 | }; 18 | 19 | 20 | -------------------------------------------------------------------------------- /Silverware/src/rx_bayang.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | void rx_init(void); 4 | void checkrx( void); 5 | 6 | 7 | struct rxdebug 8 | { 9 | unsigned long packettime; 10 | int failcount; 11 | int packetpersecond; 12 | int channelcount[4]; 13 | }; 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /Silverware/src/drv_fmc.h: -------------------------------------------------------------------------------- 1 | int fmc_write( int data1 , int data2); 2 | int readdata( unsigned int data ); 3 | void writeword( unsigned long address, unsigned long value); 4 | int fmc_write2( void); 5 | unsigned long fmc_read(unsigned long address); 6 | void fmc_read2( void); 7 | float fmc_read_float(unsigned long address); 8 | void fmc_unlock( void); 9 | void fmc_lock( void); 10 | void fmc_write_float(unsigned long address, float float_to_write); 11 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /Silverware/src/drv_softi2c.h: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | 4 | void softi2c_init(void); 5 | void softi2c_readdata(int device_address ,int register_address , int *data, int size ); 6 | void softi2c_writedata(int device_address ,int register_address , int *data, int size ); 7 | 8 | int softi2c_read(int device_address , int register_address); 9 | int softi2c_write( int device_address , int address,int value); 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /Silverware/src/util.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | float lpfcalc( float sampleperiod , float filtertime); 5 | float lpfcalc_hz(float sampleperiod, float filterhz); 6 | float mapf(float x, float in_min, float in_max, float out_min, float out_max); 7 | void lpf( float *out, float in , float coeff); 8 | 9 | float rcexpo ( float x , float exp ); 10 | 11 | void limitf ( float *input , const float limit); 12 | 13 | void TS( void); 14 | void TE( void); 15 | 16 | float fastsin( float x ); 17 | float fastcos( float x ); 18 | 19 | 20 | void limit180(float *); 21 | -------------------------------------------------------------------------------- /Silverware/src/led.h: -------------------------------------------------------------------------------- 1 | 2 | #define LEDALL 15 3 | 4 | #include 5 | 6 | void ledon( uint8_t val ); 7 | void ledoff( uint8_t val ); 8 | void ledset( int val ); 9 | void ledflash( uint32_t period , int duty ); 10 | 11 | void auxledon( uint8_t val ); 12 | void auxledoff( uint8_t val ); 13 | void auxledflash( uint32_t period , int duty ); 14 | 15 | uint8_t led_pwm( uint8_t pwmval); 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /Silverware/src/drv_spi_none.c: -------------------------------------------------------------------------------- 1 | #include "drv_spi.h" 2 | #include "project.h" 3 | #include "config.h" 4 | 5 | #ifdef SOFTSPI_NONE 6 | // spi disabled (for pin setting) 7 | int lastbyte = 0; 8 | 9 | void spi_init(void) 10 | {} 11 | void spi_cson(void) 12 | {} 13 | void spi_csoff(void) 14 | {} 15 | void spi_sendbyte( int x) 16 | { lastbyte = x;} 17 | int spi_sendrecvbyte( int x) 18 | { 19 | // make it look like a real nrf24 to pass the check 20 | if ( lastbyte == (0x0f) ) return 0xc6; 21 | lastbyte = x; 22 | return 255;} 23 | int spi_sendzerorecvbyte( void ) 24 | { return 255;} 25 | 26 | #endif 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /gcc/progStLink.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x 4 | 5 | PROGRAMMER=interface/stlink-v2.cfg 6 | TARGET=target/stm32f0x.cfg 7 | 8 | 9 | ELF=`ls *.elf | head -1` 10 | echo "ELF=$ELF" 11 | 12 | if [ "$1" != "--no-backup" ]; then 13 | NOW=`date +"%F_%T"` 14 | DIR="backup/$NOW" 15 | echo "creating buckup: $DIR" 16 | mkdir -p $DIR 17 | cd $DIR 18 | openocd -f $PROGRAMMER -f $TARGET -c "init; dump_image rom.bin 0x08000000 0x4000; shutdown " || exit_backup 19 | cd - 20 | fi 21 | 22 | if [ "$1" == "rom.bin" ]; then 23 | openocd -f $PROGRAMMER -f $TARGET -c "program rom.bin 0x08000000 verify reset; shutdown" 24 | else 25 | openocd -f $PROGRAMMER -f $TARGET -c "program $ELF verify reset; shutdown" 26 | fi -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | output/ 2 | Objects/ 3 | 4 | *.dep 5 | *.bak 6 | 7 | *.map 8 | 9 | *.lst 10 | *.crf 11 | 12 | # Windows image file caches 13 | Thumbs.db 14 | ehthumbs.db 15 | 16 | # Folder config file 17 | Desktop.ini 18 | 19 | # Recycle Bin used on file shares 20 | $RECYCLE.BIN/ 21 | 22 | # Windows Installer files 23 | *.cab 24 | *.msi 25 | *.msm 26 | *.msp 27 | 28 | # Windows shortcuts 29 | *.lnk 30 | 31 | # ========================= 32 | # Operating System Files 33 | # ========================= 34 | 35 | # OSX 36 | # ========================= 37 | 38 | .DS_Store 39 | .AppleDouble 40 | .LSOverride 41 | 42 | # Thumbnails 43 | ._* 44 | 45 | # Files that might appear on external disk 46 | .Spotlight-V100 47 | .Trashes 48 | 49 | # Directories potentially created on remote AFP share 50 | .AppleDB 51 | .AppleDesktop 52 | Network Trash Folder 53 | Temporary Items 54 | .apdisk 55 | -------------------------------------------------------------------------------- /Silverware/src/drv_fmc2.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "project.h" 4 | #include "drv_fmc.h" 5 | 6 | // flash save to option bytes , 2*8 bit 7 | int flash2_fmc_write( int data1 , int data2) 8 | { 9 | 10 | FLASH_Unlock(); 11 | FLASH_OB_Unlock(); 12 | 13 | 14 | FLASH_OB_Erase(); 15 | 16 | 17 | FLASH_OB_ProgramData( 0x1FFFF804, data1 ); 18 | 19 | 20 | FLASH_OB_ProgramData( 0x1FFFF806, data2 ); 21 | 22 | 23 | FLASH_Lock(); 24 | FLASH_OB_Lock(); 25 | 26 | return 0; 27 | } 28 | 29 | 30 | // x = flash2_readdata( OB->DATA0 ); 31 | // x = flash2_readdata( OB->DATA1 ); 32 | 33 | int flash2_readdata( unsigned int data ) 34 | { 35 | // checks that data and ~data are valid 36 | unsigned int userdata = data ; 37 | int complement = ((userdata &0x0000FF00)>>8 ); 38 | complement |=0xFFFFFF00; 39 | 40 | userdata&=0x000000FF; 41 | 42 | if ( userdata!=~complement) 43 | return 127; 44 | 45 | else return userdata; 46 | } 47 | 48 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # travis build configuration 2 | 3 | env: 4 | #Do not define 'global' env vars here. They cannot be used with API builds 5 | matrix: 6 | - TARGET=all 7 | 8 | language: cpp 9 | #sudo: required 10 | sudo: false 11 | 12 | addons: 13 | apt: 14 | packages: 15 | libc6-i386 16 | 17 | cache: 18 | directories: 19 | - $HOME/gcc-arm-none-eabi-5_4-2016q3 20 | 21 | 22 | install: 23 | - export GCC_DIR=$HOME/gcc-arm-none-eabi-5_4-2016q3 24 | - export GCC_ARCHIVE=$HOME/gcc-arm-none-eabi-5_4-2016q3-20160926-linux.tar.bz2 25 | - export GCC_URL=https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q3-update/+download/gcc-arm-none-eabi-5_4-2016q3-20160926-linux.tar.bz2 26 | - if [ ! -e $GCC_DIR/bin/arm-none-eabi-gcc ];then 27 | wget $GCC_URL -O $GCC_ARCHIVE; 28 | tar xfj $GCC_ARCHIVE -C $HOME; 29 | fi 30 | - export PATH=$PATH:$GCC_DIR/bin 31 | 32 | script: 33 | - ls 34 | - cd gcc 35 | - make $TARGET 36 | - make clean 37 | - make $TARGET 38 | # - arm-none-eabi-size h8mini.elf 39 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2016 silverx 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, 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, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Silverware/src/angle_pid.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include 4 | #include "pid.h" 5 | #include "util.h" 6 | #include "config.h" 7 | #include "defines.h" 8 | 9 | #define APIDNUMBER 2 10 | 11 | // ANGLE PIDS - used in level mode 12 | // acro pids are also used at the same time in level mode 13 | 14 | // yaw is done by the rate yaw pid 15 | // Kp ROLL PITCH 16 | float apidkp[APIDNUMBER] = {11e-2, 11e-2 }; 17 | 18 | // Kd 19 | float apidkd[APIDNUMBER] = { 0.0e-1, 0.0e-1 }; 20 | 21 | // code variables below 22 | 23 | // rate limit 24 | #define OUTLIMIT_FLOAT (float)LEVEL_MAX_RATE*DEGTORAD 25 | 26 | #define ITERMLIMIT_FLOAT OUTLIMIT_FLOAT 27 | 28 | 29 | extern int onground; 30 | extern float looptime; 31 | extern float gyro[3]; 32 | 33 | float apidoutput[APIDNUMBER]; 34 | float angleerror[APIDNUMBER]; 35 | 36 | float lasterror[APIDNUMBER]; 37 | 38 | float apid(int x) 39 | { 40 | 41 | // P term 42 | apidoutput[x] = angleerror[x] * apidkp[x]; 43 | 44 | 45 | extern float timefactor; 46 | 47 | apidoutput[x] = apidoutput[x] + (angleerror[x] - lasterror[x]) * apidkd[x] * timefactor; 48 | lasterror[x] = angleerror[x]; 49 | 50 | limitf(&apidoutput[x], OUTLIMIT_FLOAT); 51 | 52 | 53 | return apidoutput[x]; 54 | } 55 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![Build Status](https://travis-ci.org/silver13/Eachine-E011.svg?branch=master)](https://travis-ci.org/silver13/Eachine-E011) 2 | 3 | ### E011 Alternate Firmware 4 | 5 | * *Note, santa version uses bwhoop pro code* 6 | 7 | The board pads ( near the middle ) are *reverse* labeled. The 4 pin connector is *correctly* labeled. 8 | 9 | Thanks to rcg user "NotfastEnough" for figuring out the hardware connctions 10 | 11 | ### Factory firmware 12 | The factory firmware can be flashed back after using this, it's in the bin folder 13 | 14 | ### Gestures 15 | The firmware uses "gestures" to activate certain features, currently accelerometer calibration, acro / level mode switch and pid gestures ( field tuning ) 16 | 17 | Gestures reference: 18 | http://sirdomsen.diskstation.me/dokuwiki/doku.php?id=gestures 19 | 20 | ### Flashing the firmware 21 | Flashing instructions ( uses St-link Utility program ): 22 | https://www.rcgroups.com/forums/showthread.php?2876797-Boldclash-bwhoop-B-03-opensource-firmware 23 | 24 | Compiling instructions ( uses Keil uVision IDE): 25 | https://www.rcgroups.com/forums/showthread.php?2877480-Compilation-instructions-for-silverware#post37391059 26 | 27 | 28 | ### Wiki 29 | http://sirdomsen.diskstation.me/dokuwiki/doku.php?id=start 30 | 31 | 32 | ### 02.07.17 33 | * initial code 34 | -------------------------------------------------------------------------------- /Silverware/src/drv_fmc1.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "project.h" 4 | #include "drv_fmc.h" 5 | 6 | // address 32768 - 1024 = 31k - last flash block 7 | #define FLASH_ADDR 0x08007C00 8 | 9 | extern void failloop( int); 10 | 11 | // address 0 - 255 ( for current block FLASH_ADDR ) 12 | // value 32 bit to write 13 | void writeword( unsigned long address, unsigned long value) 14 | { 15 | int test = FLASH_ProgramWord( FLASH_ADDR + (address<<2), value); 16 | if ( test != FLASH_COMPLETE ) 17 | { 18 | FLASH_Lock(); 19 | failloop(5); 20 | } 21 | } 22 | 23 | void fmc_write_float(unsigned long address, float float_to_write) { 24 | writeword(address, *(unsigned long *) &float_to_write); 25 | } 26 | 27 | float fmc_read_float(unsigned long address) { 28 | unsigned long result = fmc_read(address); 29 | return *(float*)&result; 30 | } 31 | 32 | void fmc_unlock() { 33 | FLASH_Unlock(); 34 | } 35 | 36 | void fmc_lock() { 37 | FLASH_Lock(); 38 | } 39 | 40 | int fmc_erase( void ) 41 | { 42 | int test = FLASH_ErasePage( FLASH_ADDR ); 43 | if ( test != FLASH_COMPLETE ) FLASH_Lock(); 44 | else return 0; 45 | return 1;// error occured 46 | } 47 | 48 | // reads 32 bit value 49 | unsigned long fmc_read(unsigned long address) 50 | { 51 | address = address * 4 + FLASH_ADDR; 52 | unsigned int *addressptr = (unsigned int *)address; 53 | return (*addressptr); 54 | } 55 | 56 | 57 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Lib/license.txt: -------------------------------------------------------------------------------- 1 | All pre-build libraries contained in the folders "ARM", "GCC" and "G++" 2 | are guided by the following license: 3 | 4 | Copyright (C) 2009-2012 ARM Limited. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | - Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | - Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | - Neither the name of ARM nor the names of its contributors may be used 15 | to endorse or promote products derived from this software without 16 | specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE 22 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | POSSIBILITY OF SUCH DAMAGE. 29 | -------------------------------------------------------------------------------- /Silverware/src/drv_clk.c: -------------------------------------------------------------------------------- 1 | #include "project.h" 2 | #include "binary.h" 3 | #include "config.h" 4 | 5 | // the clock is already set, we turn pll off and set a new multiplier 6 | void setclock(void) 7 | { 8 | // turn on HSI 9 | RCC->CR |= 1; 10 | // wait for HSI ready 11 | while((RCC->CR & RCC_CR_HSIRDY) == 0) 12 | { 13 | } 14 | // set clock source HSI 15 | RCC->CFGR&=(0xFFFFFFFC); 16 | // wait for switch 17 | while( (RCC->CFGR&B00001100) ) 18 | { 19 | 20 | } 21 | // turn pll off 22 | RCC->CR &= (uint32_t)0xFEFFFFFF; 23 | 24 | while((RCC->CR & RCC_CR_PLLRDY) != 0) 25 | { 26 | } 27 | 28 | //reset pll settings 29 | RCC->CFGR &= (uint32_t)0xFFC0FFFF; 30 | 31 | #ifdef ENABLE_OVERCLOCK 32 | RCC->CFGR &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMULL); 33 | RCC->CFGR |= RCC_CFGR_PLLSRC_HSI_Div2 | RCC_CFGR_PLLMULL16; 34 | #else 35 | RCC->CFGR &= ~(RCC_CFGR_PLLSRC | RCC_CFGR_PLLMULL); 36 | RCC->CFGR |= RCC_CFGR_PLLSRC_HSI_Div2 | RCC_CFGR_PLLMULL12; 37 | #endif 38 | 39 | //PLL on 40 | RCC->CR |= RCC_CR_PLLON; 41 | 42 | while((RCC->CR & RCC_CR_PLLRDY) == 0) 43 | { 44 | } 45 | //set PLL as source 46 | // RCC->CFGR&=(0xFFFFFF|B11111110); 47 | RCC->CFGR|=(B00000010); 48 | 49 | // RCC->CFGR &= (uint32_t)((uint32_t)~(RCC_CFGR_SW)); 50 | // RCC->CFGR |= (uint32_t)RCC_CFGR_SW_PLL; 51 | 52 | //wait until PLL is the clock 53 | #ifdef ENABLE_OVERCLOCK 54 | // only if overclocked as it is redundant 55 | while( (RCC->CFGR&B00001100) != B00001000 ) 56 | { 57 | } 58 | #endif 59 | } 60 | 61 | void clk_init(void) 62 | { 63 | // reset clock to default values 64 | // sets clock to 48Mhz 65 | 66 | // this executes anyway, it's in the .s file 67 | // SystemInit(); 68 | 69 | // SetSysClock(); 70 | 71 | #ifdef ENABLE_OVERCLOCK 72 | // set clock to 64Mhz (PLL max multiplier) 73 | setclock(); 74 | #endif 75 | 76 | 77 | } 78 | 79 | -------------------------------------------------------------------------------- /gcc/Makefile: -------------------------------------------------------------------------------- 1 | TARGET=bwhoop 2 | EXECUTABLE=bwhoop.elf 3 | 4 | CC=arm-none-eabi-gcc 5 | LD=arm-none-eabi-gcc 6 | AR=arm-none-eabi-ar 7 | AS=arm-none-eabi-as 8 | CP=arm-none-eabi-objcopy 9 | OD=arm-none-eabi-objdump 10 | 11 | topdir = .. 12 | 13 | DEFS = -DUSE_STDPERIPH_DRIVER -DSTM32F031 14 | STARTUP = $(topdir)/Libraries/CMSIS/Device/ST/STM32F0xx/Source/Templates/gcc_ride7/startup_stm32f030.s 15 | 16 | MCU = cortex-m0 17 | MCFLAGS = -mcpu=$(MCU) -g -ggdb -mthumb -fdata-sections -ffunction-sections -fsingle-precision-constant -ffast-math -nostartfiles --specs=nano.specs -flto 18 | 19 | INCLUDES = -I$(topdir)/Silverware/src/ \ 20 | -I$(topdir)/Libraries/CMSIS/Device/ST/STM32F0xx/Include/ \ 21 | -I$(topdir)/Libraries/CMSIS/Include/ \ 22 | -I$(topdir)/Utilities/ \ 23 | -I$(topdir)/Libraries/STM32F0xx_StdPeriph_Driver/inc/ 24 | 25 | OPTIMIZE = -Os 26 | 27 | CFLAGS = $(MCFLAGS) $(OPTIMIZE) $(DEFS) -I. -I./ $(INCLUDES) -Wl,-T,flash.ld,-Map,output.map,--gc-sections -std=gnu99 28 | 29 | AFLAGS = $(MCFLAGS) 30 | 31 | SRC = $(wildcard $(topdir)/Silverware/src/*.c) \ 32 | $(wildcard $(topdir)/Silverware/src/*.h) \ 33 | $(wildcard $(topdir)/Silverware/src/*.cpp) \ 34 | $(topdir)/Libraries/STM32F0xx_StdPeriph_Driver/src/*.c \ 35 | $(topdir)/Utilities/system_stm32f0xx.c \ 36 | methods.c 37 | 38 | OBJDIR = . 39 | OBJ = $(patsubst %.c,$(OBJDIR)/%.o,$(filter %.c,$(SRC))) 40 | OBJ += $(patsubst %.cpp,$(OBJDIR)/%.o,$(filter %.cpp,$(SRC))) 41 | OBJ += Startup.o 42 | 43 | 44 | all: $(TARGET) 45 | 46 | $(TARGET).hex: $(EXECUTABLE) 47 | $(CP) -O ihex $^ $@ 48 | $(TARGET): $(EXECUTABLE) 49 | $(CP) -O binary $^ $@ 50 | 51 | $(EXECUTABLE): $(SRC) $(STARTUP) 52 | $(CC) $(CFLAGS) $^ -lm -o $@ 53 | arm-none-eabi-size $(EXECUTABLE) 54 | 55 | 56 | clean: 57 | rm -f Startup.lst $(TARGET) $(TARGET).lst $(OBJ) $(AUTOGEN) \ 58 | $(TARGET).out $(TARGET).hex $(TARGET).map \ 59 | $(TARGET).dmp $(EXECUTABLE) 60 | -------------------------------------------------------------------------------- /Silverware/src/stickvector.c: -------------------------------------------------------------------------------- 1 | #include "config.h" 2 | #include "util.h" 3 | 4 | #include 5 | #include 6 | 7 | 8 | 9 | extern float GEstG[3]; 10 | extern float Q_rsqrt( float number ); 11 | 12 | // error vector between stick position and quad orientation 13 | // this is the output of this function 14 | float errorvect[3]; 15 | // cache the last result so it does not get calculated everytime 16 | float last_rx[2] = {13.13f , 12.12f}; 17 | float stickvector[3] = { 0 , 0 , 1}; 18 | 19 | 20 | 21 | 22 | void stick_vector( float rx_input[] , float maxangle) 23 | { 24 | // only compute stick rotation if values changed 25 | if ( last_rx[0] == rx_input[0] && last_rx[1] == rx_input[1] ) 26 | { 27 | 28 | } 29 | else 30 | { 31 | last_rx[0] = rx_input[0]; 32 | last_rx[1] = rx_input[1]; 33 | 34 | 35 | float pitch, roll; 36 | 37 | // rotate down vector to match stick position 38 | pitch = rx_input[1] * MAX_ANGLE_HI * DEGTORAD + (float) TRIM_PITCH * DEGTORAD; 39 | roll = rx_input[0] * MAX_ANGLE_HI * DEGTORAD + (float) TRIM_ROLL * DEGTORAD; 40 | 41 | stickvector[0] = fastsin( roll ); 42 | stickvector[1] = fastsin( pitch ); 43 | stickvector[2] = fastcos( roll ) * fastcos( pitch ); 44 | 45 | 46 | float mag2 = (stickvector[0] * stickvector[0] + stickvector[1] * stickvector[1]); 47 | 48 | if ( mag2 > 0.001f ) 49 | { 50 | mag2 = Q_rsqrt( mag2 / (1 - stickvector[2] * stickvector[2]) ); 51 | } 52 | else mag2 = 0.707f; 53 | 54 | stickvector[0] *=mag2; 55 | stickvector[1] *=mag2; 56 | } 57 | 58 | // find error between stick vector and quad orientation 59 | // vector cross product 60 | errorvect[1]= -((GEstG[1]*stickvector[2]) - (GEstG[2]*stickvector[1])); 61 | errorvect[0]= (GEstG[2]*stickvector[0]) - (GEstG[0]*stickvector[2]); 62 | 63 | // some limits just in case 64 | 65 | limitf( &errorvect[0] , 1.0); 66 | limitf( &errorvect[1] , 1.0); 67 | 68 | 69 | 70 | } 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /Silverware/src/drv_xn297_3wire.c: -------------------------------------------------------------------------------- 1 | 2 | #include "binary.h" 3 | #include "drv_spi.h" 4 | 5 | #include "project.h" 6 | #include "xn297.h" 7 | #include "hardware.h" 8 | 9 | #ifdef SOFTSPI_3WIRE 10 | 11 | extern void mosi_input( void); 12 | extern int spi_recvbyte( void); 13 | 14 | void xn_writereg( int reg , int val) 15 | { 16 | reg = reg&0x3F; 17 | reg = reg|0x20; 18 | spi_cson(); 19 | spi_sendbyte( reg); 20 | spi_sendbyte( val); 21 | spi_csoff(); 22 | } 23 | 24 | int xn_readreg( int reg) 25 | { 26 | reg = reg&0x1F; 27 | spi_cson(); 28 | spi_sendbyte( reg); 29 | mosi_input( ); 30 | int val =spi_recvbyte(); 31 | spi_csoff(); 32 | return val; 33 | } 34 | 35 | int xn_command( int command) 36 | { 37 | spi_cson(); 38 | spi_sendbyte(command); 39 | spi_csoff(); 40 | return 0; 41 | } 42 | // 43 | 44 | 45 | void xn_readpayload( int *data , int size ) 46 | { 47 | int index = 0; 48 | spi_cson(); 49 | spi_sendbyte( B01100001 ); // read rx payload 50 | mosi_input(); 51 | while(index 0) 12 | 13 | void rgb_init(void) 14 | { 15 | // spi port inits 16 | 17 | if ( (RGB_PIN == GPIO_Pin_13 || RGB_PIN == GPIO_Pin_14) && RGB_PORT == GPIOA ) 18 | { 19 | // programming port used 20 | 21 | // wait until 2 seconds from powerup passed 22 | while ( gettime() < 2e6 ) ; 23 | } 24 | 25 | GPIO_InitTypeDef GPIO_InitStructure; 26 | 27 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 28 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 29 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; 30 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 31 | 32 | GPIO_InitStructure.GPIO_Pin = RGB_PIN; 33 | GPIO_Init(RGB_PORT, &GPIO_InitStructure); 34 | 35 | 36 | for (int i = 0 ; i < RGB_LED_NUMBER ; i++) 37 | { 38 | rgb_send( 0 ); 39 | } 40 | 41 | } 42 | 43 | 44 | #define gpioset( port , pin) port->BSRR = pin 45 | #define gpioreset( port , pin) port->BRR = pin 46 | 47 | #define RGBHIGH gpioset( RGB_PORT, RGB_PIN) 48 | #define RGBLOW gpioreset( RGB_PORT, RGB_PIN); 49 | 50 | 51 | 52 | #pragma push 53 | 54 | #pragma Otime 55 | #pragma O2 56 | 57 | void delay1a() 58 | { 59 | uint8_t count = 2; 60 | while (count--); 61 | } 62 | 63 | void delay1b() 64 | { 65 | uint8_t count = 2; 66 | while (count--); 67 | } 68 | 69 | void delay2a() 70 | { 71 | uint8_t count = 1; 72 | while (count--); 73 | } 74 | 75 | void delay2b() 76 | { 77 | uint8_t count = 3; 78 | while (count--); 79 | } 80 | 81 | void rgb_send( int data) 82 | { 83 | for ( int i =23 ; i >=0 ; i--) 84 | { 85 | if ( (data>>i)&1 ) 86 | { 87 | RGBHIGH; 88 | delay1a(); 89 | RGBLOW; 90 | delay1b(); 91 | } 92 | else 93 | { 94 | RGBHIGH; 95 | delay2a(); 96 | RGBLOW; 97 | delay2b(); 98 | } 99 | 100 | } 101 | 102 | } 103 | 104 | 105 | 106 | #pragma pop 107 | 108 | #else 109 | // rgb led not found 110 | // some dummy headers just in case 111 | void rgb_init(void) 112 | { 113 | 114 | } 115 | 116 | void rgb_send( int data) 117 | { 118 | 119 | } 120 | #endif 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | -------------------------------------------------------------------------------- /Silverware/src/drv_gpio.c: -------------------------------------------------------------------------------- 1 | #include "project.h" 2 | #include "drv_gpio.h" 3 | #include "config.h" 4 | 5 | void gpio_init(void) 6 | { 7 | // clocks on to all ports 8 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_GPIOA | RCC_AHBPeriph_GPIOB | RCC_AHBENR_GPIOFEN , ENABLE); 9 | 10 | GPIO_InitTypeDef GPIO_InitStructure; 11 | 12 | 13 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 14 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 15 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 16 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 17 | 18 | 19 | #ifdef ENABLE_VREG_PIN 20 | GPIO_InitStructure.GPIO_Pin = VREG_PIN_1; 21 | GPIO_Init(VREG_PORT_1, &GPIO_InitStructure); 22 | GPIO_SetBits( VREG_PORT_1, VREG_PIN_1); 23 | #endif 24 | 25 | 26 | #if ( LED_NUMBER > 0 ) 27 | GPIO_InitStructure.GPIO_Pin = LED1PIN; 28 | GPIO_Init(LED1PORT, &GPIO_InitStructure); 29 | #if ( LED_NUMBER > 1 ) 30 | GPIO_InitStructure.GPIO_Pin = LED2PIN; 31 | GPIO_Init(LED2PORT, &GPIO_InitStructure); 32 | #if ( LED_NUMBER > 2 ) 33 | GPIO_InitStructure.GPIO_Pin = LED3PIN; 34 | GPIO_Init(LED3PORT, &GPIO_InitStructure); 35 | #if ( LED_NUMBER > 3 ) 36 | GPIO_InitStructure.GPIO_Pin = LED4PIN; 37 | GPIO_Init(LED4PORT, &GPIO_InitStructure); 38 | #endif 39 | #endif 40 | #endif 41 | #endif 42 | 43 | #if ( AUX_LED_NUMBER > 0 ) 44 | GPIO_InitStructure.GPIO_Pin = AUX_LED1PIN; 45 | GPIO_Init(AUX_LED1PORT, &GPIO_InitStructure); 46 | #endif 47 | #if ( AUX_LED_NUMBER > 1 ) 48 | GPIO_InitStructure.GPIO_Pin = AUX_LED2PIN; 49 | GPIO_Init(AUX_LED2PORT, &GPIO_InitStructure); 50 | #endif 51 | 52 | } 53 | 54 | 55 | 56 | // init fpv pin separately because it may use SWDAT/SWCLK don't want to enable it right away 57 | int gpio_init_fpv(void) 58 | { 59 | // only repurpose the pin after rx/tx have bound 60 | extern int rxmode; 61 | if (rxmode == RXMODE_NORMAL) 62 | { 63 | // set gpio pin as output 64 | GPIO_InitTypeDef GPIO_InitStructure; 65 | 66 | // common settings to set ports 67 | GPIO_InitStructure.GPIO_Pin = FPV_PIN; 68 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 69 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 70 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 71 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 72 | GPIO_Init(FPV_PORT,&GPIO_InitStructure); 73 | 74 | return 1; 75 | } 76 | return 0; 77 | } 78 | 79 | 80 | 81 | 82 | -------------------------------------------------------------------------------- /Silverware/src/defines.h: -------------------------------------------------------------------------------- 1 | 2 | // defines for things that do not normally need changing 3 | 4 | 5 | #define MOTOR_BL 0 6 | #define MOTOR_FL 1 7 | #define MOTOR_FR 3 8 | #define MOTOR_BR 2 9 | 10 | #define PIDNUMBER 3 11 | 12 | #define DEGTORAD 0.017453292f 13 | #define RADTODEG 57.29577951f 14 | 15 | #define AUXNUMBER 12 16 | 17 | #define ROLL 0 18 | #define PITCH 1 19 | #define YAW 2 20 | 21 | 22 | #define FILTERCALC( sampleperiod, filtertime) (1.0f - ( 6.0f*(float)sampleperiod) / ( 3.0f *(float)sampleperiod + (float)filtertime)) 23 | 24 | 25 | #define RXMODE_BIND 0 26 | #define RXMODE_NORMAL (!RXMODE_BIND) 27 | 28 | /* 29 | #define CH_ON 4 30 | #define CH_OFF 5 31 | #define CH_FLIP 0 32 | #define CH_EXPERT 1 33 | #define CH_HEADFREE 2 34 | #define CH_RTH 3 35 | */ 36 | 37 | #define CH_ON (AUXNUMBER - 2) 38 | #define CH_OFF (AUXNUMBER - 1) 39 | #define CH_FLIP 0 40 | #define CH_EXPERT 1 41 | #define CH_HEADFREE 2 42 | #define CH_RTH 3 43 | #define CH_AUX1 4 44 | #define CH_AUX2 5 45 | // trims numbers have to be sequential, atart at CH_PIT_TRIM 46 | #define CH_PIT_TRIM 6 47 | #define CH_RLL_TRIM 7 48 | #define CH_THR_TRIM 8 49 | #define CH_YAW_TRIM 9 50 | 51 | #define CH_INV 6 52 | #define CH_VID 7 53 | #define CH_PIC 8 54 | 55 | #define CH_CG023_LED 3 56 | #define CH_CG023_FLIP 0 57 | #define CH_CG023_STILL 2 58 | #define CH_CG023_VIDEO 1 59 | 60 | #define CH_H7_FLIP 0 61 | #define CH_H7_VIDEO 1 62 | #define CH_H7_FS 2 63 | 64 | #define CH_CX10_CH0 0 65 | #define CH_CX10_CH2 2 66 | 67 | #define CH_AUX3 CH_OFF 68 | #define CH_AUX4 CH_OFF 69 | 70 | 71 | #define DEVO_CHAN_5 CH_INV 72 | #define DEVO_CHAN_6 CH_FLIP 73 | #define DEVO_CHAN_7 CH_PIC 74 | #define DEVO_CHAN_8 CH_VID 75 | #define DEVO_CHAN_9 CH_HEADFREE 76 | #define DEVO_CHAN_10 CH_RTH 77 | 78 | 79 | #define MULTI_CHAN_5 CH_FLIP 80 | #define MULTI_CHAN_6 CH_RTH 81 | #define MULTI_CHAN_7 CH_PIC 82 | #define MULTI_CHAN_8 CH_VID 83 | #define MULTI_CHAN_9 CH_HEADFREE 84 | #define MULTI_CHAN_10 CH_INV 85 | 86 | // used for the pwm driver 87 | #define CH1 0 88 | #define CH2 1 89 | #define CH3 2 90 | #define CH4 3 91 | 92 | 93 | #define int32 int_fast32_t 94 | #define int16 int_fast16_t 95 | #define int8 int_fast8_t 96 | 97 | #define uint32 uint_fast32_t 98 | #define uint16 uint_fast16_t 99 | #define uint8 uint_fast8_t 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | -------------------------------------------------------------------------------- /Silverware/src/xn297.h: -------------------------------------------------------------------------------- 1 | 2 | 3 | void xn_writerxaddress( int *addr ) ; 4 | void xn_writereg( int reg , int val); 5 | int xn_command( int command); 6 | int xn_readreg( int reg); 7 | void _spi_write_address( int reg, int val); 8 | void xn_readpayload( int *data , int size ); 9 | void xn_writepayload( int data[] , int size ); 10 | void xn_writetxaddress( int *addr ) ; 11 | 12 | 13 | // registers 14 | #define CONFIG 0x00 15 | #define EN_AA 0x01 16 | #define EN_RXADDR 0x02 17 | #define SETUP_AW 0x03 18 | #define SETUP_RETR 0x04 19 | #define RF_CH 0x05 20 | #define RF_SETUP 0x06 21 | #define STATUS 0x07 22 | #define OBSERVE_TX 0x08 23 | #define CD 0x09 24 | #define RX_ADDR_P0 0x0A 25 | #define RX_ADDR_P1 0x0B 26 | #define RX_ADDR_P2 0x0C 27 | #define RX_ADDR_P3 0x0D 28 | #define RX_ADDR_P4 0x0E 29 | #define RX_ADDR_P5 0x0F 30 | #define TX_ADDR 0x10 31 | #define RX_PW_P0 0x11 32 | #define RX_PW_P1 0x12 33 | #define RX_PW_P2 0x13 34 | #define RX_PW_P3 0x14 35 | #define RX_PW_P4 0x15 36 | #define RX_PW_P5 0x16 37 | #define FIFO_STATUS 0x17 38 | 39 | // bit masks 40 | #define MASK_RX_DR 6 41 | #define MASK_TX_DS 5 42 | #define MASK_MAX_RT 4 43 | #define EN_CRC 3 44 | #define CRCO 2 45 | #define PWR_UP 1 46 | #define PRIM_RX 0 47 | #define ENAA_P5 5 48 | #define ENAA_P4 4 49 | #define ENAA_P3 3 50 | #define ENAA_P2 2 51 | #define ENAA_P1 1 52 | #define ENAA_P0 0 53 | #define ERX_P5 5 54 | #define ERX_P4 4 55 | #define ERX_P3 3 56 | #define ERX_P2 2 57 | #define ERX_P1 1 58 | #define ERX_P0 0 59 | #define AW 0 60 | #define ARD 4 61 | #define ARC 0 62 | #define PLL_LOCK 4 63 | #define RF_DR 3 64 | #define RF_PWR 1 65 | #define LNA_HCURR 0 66 | #define RX_DR 6 67 | #define TX_DS 5 68 | #define MAX_RT 4 69 | #define RX_P_NO 1 70 | #define TX_FULL 0 71 | #define PLOS_CNT 4 72 | #define ARC_CNT 0 73 | #define TX_REUSE 6 74 | #define FIFO_FULL 5 75 | #define TX_EMPTY 4 76 | #define RX_FULL 1 77 | #define RX_EMPTY 0 78 | 79 | // commands 80 | #define R_REGISTER 0x00 81 | #define W_REGISTER 0x20 82 | #define REGISTER_MASK 0x1F 83 | #define R_RX_PAYLOAD 0x61 84 | #define W_TX_PAYLOAD 0xA0 85 | #define FLUSH_TX 0xE1 86 | #define FLUSH_RX 0xE2 87 | #define REUSE_TX_PL 0xE3 88 | #define NOP 0xFF 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | -------------------------------------------------------------------------------- /Silverware/src/flash.c: -------------------------------------------------------------------------------- 1 | 2 | #include "project.h" 3 | #include "drv_fmc.h" 4 | 5 | extern int fmc_erase( void ); 6 | extern void fmc_unlock(void); 7 | extern void fmc_lock(void); 8 | 9 | extern float accelcal[]; 10 | extern float * pids_array[3]; 11 | 12 | extern float hardcoded_pid_identifier; 13 | 14 | 15 | #define FMC_HEADER 0x12AA0001 16 | 17 | float initial_pid_identifier = -10; 18 | float saved_pid_identifier; 19 | 20 | 21 | float flash_get_hard_coded_pid_identifier( void) { 22 | float result = 0; 23 | 24 | for (int i=0; i<3 ; i++) { 25 | for (int j=0; j<3 ; j++) { 26 | result += pids_array[i][j] * (i+1) * (j+1) * 0.932f; 27 | } 28 | } 29 | return result; 30 | } 31 | 32 | 33 | void flash_hard_coded_pid_identifier( void) 34 | { 35 | initial_pid_identifier = flash_get_hard_coded_pid_identifier(); 36 | } 37 | 38 | 39 | 40 | 41 | void flash_save( void) { 42 | 43 | fmc_unlock(); 44 | fmc_erase(); 45 | 46 | unsigned long addresscount = 0; 47 | 48 | writeword(addresscount++, FMC_HEADER); 49 | 50 | fmc_write_float(addresscount++, initial_pid_identifier ); 51 | 52 | for (int i=0; i<3 ; i++) { 53 | for (int j=0; j<3 ; j++) { 54 | fmc_write_float(addresscount++, pids_array[i][j]); 55 | } 56 | } 57 | 58 | 59 | fmc_write_float(addresscount++, accelcal[0]); 60 | fmc_write_float(addresscount++, accelcal[1]); 61 | fmc_write_float(addresscount++, accelcal[2]); 62 | 63 | writeword(255, FMC_HEADER); 64 | 65 | fmc_lock(); 66 | } 67 | 68 | 69 | 70 | void flash_load( void) { 71 | 72 | unsigned long addresscount = 0; 73 | // check if saved data is present 74 | if (FMC_HEADER == fmc_read(addresscount++)&& FMC_HEADER == fmc_read(255)) 75 | { 76 | 77 | saved_pid_identifier = fmc_read_float(addresscount++); 78 | // load pids from flash if pid.c values are still the same 79 | if ( saved_pid_identifier == initial_pid_identifier ) 80 | { 81 | for (int i=0; i<3 ; i++) { 82 | for (int j=0; j<3 ; j++) { 83 | pids_array[i][j] = fmc_read_float(addresscount++); 84 | } 85 | } 86 | } 87 | else{ 88 | addresscount+=9; 89 | } 90 | 91 | accelcal[0] = fmc_read_float(addresscount++ ); 92 | accelcal[1] = fmc_read_float(addresscount++ ); 93 | accelcal[2] = fmc_read_float(addresscount++ ); 94 | 95 | } 96 | else 97 | { 98 | 99 | } 100 | 101 | } 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Device/ST/STM32F0xx/Include/system_stm32f0xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f0xx.h 4 | * @author MCD Application Team 5 | * @version V1.3.2 6 | * @date 27-March-2014 7 | * @brief CMSIS Cortex-M0 Device Peripheral Access Layer System Header File. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2014 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 stm32f0xx_system 33 | * @{ 34 | */ 35 | 36 | /** 37 | * @brief Define to prevent recursive inclusion 38 | */ 39 | #ifndef __SYSTEM_STM32F0XX_H 40 | #define __SYSTEM_STM32F0XX_H 41 | 42 | #ifdef __cplusplus 43 | extern "C" { 44 | #endif 45 | 46 | /** @addtogroup STM32F0xx_System_Includes 47 | * @{ 48 | */ 49 | 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @addtogroup STM32F0xx_System_Exported_types 56 | * @{ 57 | */ 58 | 59 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 60 | 61 | /** 62 | * @} 63 | */ 64 | 65 | /** @addtogroup STM32F0xx_System_Exported_Constants 66 | * @{ 67 | */ 68 | 69 | /** 70 | * @} 71 | */ 72 | 73 | /** @addtogroup STM32F0xx_System_Exported_Macros 74 | * @{ 75 | */ 76 | 77 | /** 78 | * @} 79 | */ 80 | 81 | /** @addtogroup STM32F0xx_System_Exported_Functions 82 | * @{ 83 | */ 84 | 85 | extern void SystemInit(void); 86 | extern void SystemCoreClockUpdate(void); 87 | /** 88 | * @} 89 | */ 90 | 91 | #ifdef __cplusplus 92 | } 93 | #endif 94 | 95 | #endif /*__SYSTEM_STM32F0XX_H */ 96 | 97 | /** 98 | * @} 99 | */ 100 | 101 | /** 102 | * @} 103 | */ 104 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 105 | -------------------------------------------------------------------------------- /Silverware/src/buzzer.c: -------------------------------------------------------------------------------- 1 | 2 | #include "project.h" 3 | #include "config.h" 4 | #include "drv_time.h" 5 | #include "buzzer.h" 6 | 7 | #ifdef BUZZER_ENABLE 8 | 9 | #define PIN_OFF( port , pin ) GPIO_ResetBits( port , pin) 10 | #define PIN_ON( port , pin ) GPIO_SetBits( port , pin) 11 | 12 | 13 | int gpio_init_buzzer(void) 14 | { 15 | 16 | // set gpio pin as output 17 | GPIO_InitTypeDef GPIO_InitStructure; 18 | 19 | // common settings to set ports 20 | GPIO_InitStructure.GPIO_Pin = BUZZER_PIN; 21 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 22 | //GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 23 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 24 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 25 | 26 | GPIO_Init(BUZZER_PIN_PORT,&GPIO_InitStructure); 27 | return 1; 28 | } 29 | 30 | 31 | void buzzer() 32 | { 33 | extern int failsafe; 34 | extern int lowbatt; 35 | extern int rxmode; 36 | extern unsigned long lastlooptime; 37 | 38 | static int toggle; 39 | static unsigned long buzzertime; 40 | static int buzzer_init = 0; 41 | 42 | unsigned long pulse_rate; 43 | 44 | // waits 5 seconds 45 | // before configuring the gpio buzzer pin to ensure 46 | // there is time to program the chip (if using SWDAT or SWCLK) 47 | 48 | if ( lowbatt || failsafe ) 49 | { 50 | unsigned long time = gettime(); 51 | if ( buzzertime == 0) 52 | buzzertime = time; 53 | else 54 | { 55 | 56 | // rank lowbatt > failsafe > throttle 57 | if (lowbatt) 58 | pulse_rate = 200000; // 1/5th second 59 | else if (failsafe) 60 | pulse_rate = 400000; // 2/5ths second 61 | else 62 | pulse_rate = 600000; // 3/5ths second 63 | 64 | // start the buzzer if timeout has elapsed 65 | if ( time - buzzertime > BUZZER_DELAY || lowbatt) 66 | { 67 | // initialize pin only after minimum 10 seconds from powerup 68 | if ( !buzzer_init && time > 10e6) 69 | { 70 | buzzer_init = gpio_init_buzzer(); 71 | } 72 | 73 | // don't continue if buzzer not initialized 74 | if ( !buzzer_init ) return; 75 | 76 | 77 | // enable buzzer 78 | if (time%pulse_rate>pulse_rate/2) 79 | { 80 | if ( toggle ) // cycle the buzzer 81 | { 82 | PIN_ON( BUZZER_PIN_PORT, BUZZER_PIN); // on 83 | } 84 | else 85 | { 86 | PIN_OFF( BUZZER_PIN_PORT, BUZZER_PIN); // off 87 | } 88 | toggle = !toggle; 89 | } 90 | else 91 | { 92 | PIN_OFF(BUZZER_PIN_PORT, BUZZER_PIN ); 93 | } 94 | } 95 | 96 | } 97 | 98 | } 99 | else 100 | { 101 | buzzertime = 0; 102 | // set buzzer to off if beeping condition stopped 103 | if (buzzer_init) PIN_OFF(BUZZER_PIN_PORT, BUZZER_PIN ); 104 | 105 | } 106 | 107 | } 108 | 109 | #endif 110 | 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /Silverware/src/drv_i2c.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 silverx 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | 26 | 27 | #include "project.h" 28 | 29 | 30 | #include "drv_i2c.h" 31 | #include "drv_softi2c.h" 32 | #include "drv_hw_i2c.h" 33 | 34 | #include "config.h" 35 | 36 | #ifndef USE_HARDWARE_I2C 37 | #ifndef USE_SOFTWARE_I2C 38 | #ifndef USE_DUMMY_I2C 39 | #define USE_SOFTWARE_I2C 40 | #endif 41 | #endif 42 | #endif 43 | 44 | int liberror = 0; 45 | 46 | void i2c_init( void) 47 | { 48 | #ifdef USE_HARDWARE_I2C 49 | hw_i2c_init(); 50 | #endif 51 | 52 | #ifdef USE_SOFTWARE_I2C 53 | softi2c_init(); 54 | #endif 55 | 56 | #ifdef USE_DUMMY_I2C 57 | #warning I2C FUNCTIONS DISABLED 58 | #endif 59 | 60 | } 61 | 62 | 63 | void i2c_writereg( int reg ,int data) 64 | { 65 | #ifdef USE_HARDWARE_I2C 66 | hw_i2c_writereg( reg , data); 67 | #endif 68 | 69 | #ifdef USE_SOFTWARE_I2C 70 | softi2c_write( SOFTI2C_GYRO_ADDRESS , reg , data); 71 | #endif 72 | 73 | #ifdef USE_DUMMY_I2C 74 | 75 | #endif 76 | } 77 | 78 | 79 | int i2c_readdata( int reg, int *data, int size ) 80 | { 81 | #ifdef USE_HARDWARE_I2C 82 | return hw_i2c_readdata( reg, data, size ); 83 | #endif 84 | 85 | #ifdef USE_SOFTWARE_I2C 86 | softi2c_readdata( SOFTI2C_GYRO_ADDRESS , reg , data, size ); 87 | return 1; 88 | #endif 89 | 90 | #ifdef USE_DUMMY_I2C 91 | return 1; 92 | #endif 93 | } 94 | 95 | int i2c_readreg( int reg ) 96 | { 97 | #ifdef USE_HARDWARE_I2C 98 | return hw_i2c_readreg(reg); 99 | #endif 100 | 101 | #ifdef USE_SOFTWARE_I2C 102 | return softi2c_read( SOFTI2C_GYRO_ADDRESS , reg); 103 | #endif 104 | 105 | #ifdef USE_DUMMY_I2C 106 | return 255; 107 | #endif 108 | } 109 | 110 | 111 | -------------------------------------------------------------------------------- /Silverware/src/drv_serial.c: -------------------------------------------------------------------------------- 1 | // serial for stm32 not used yet 2 | #include "project.h" 3 | #include "stm32f0xx_usart.h" 4 | #include 5 | #include "drv_serial.h" 6 | #include "config.h" 7 | 8 | 9 | // enable serial driver ( pin SWCLK after calibration) 10 | // WILL DISABLE PROGRAMMING AFTER GYRO CALIBRATION - 2 - 3 seconds after powerup) 11 | 12 | // this has to be in config.h 13 | //#define SERIAL_ENABLE 14 | 15 | 16 | #define SERIAL_BUFFER_SIZE 64 17 | 18 | #define SERIAL_BAUDRATE 115200 19 | 20 | #ifdef SERIAL_ENABLE 21 | 22 | uint8_t buffer[SERIAL_BUFFER_SIZE]; 23 | char buffer_start = 0; 24 | char buffer_end = 0; 25 | 26 | 27 | void USART1_IRQHandler(void) 28 | { 29 | if (buffer_end != buffer_start) 30 | { 31 | USART_SendData(USART1, buffer[buffer_start]); 32 | buffer_start++; 33 | buffer_start = buffer_start % (SERIAL_BUFFER_SIZE); 34 | } 35 | else 36 | { 37 | USART_ITConfig(USART1, USART_IT_TXE, DISABLE); 38 | } 39 | } 40 | 41 | void serial_init(void) 42 | { 43 | 44 | GPIO_InitTypeDef GPIO_InitStructure; 45 | 46 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 47 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 48 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 49 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 50 | 51 | 52 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_14; 53 | GPIO_Init(GPIOA, &GPIO_InitStructure); 54 | 55 | GPIO_PinAFConfig(GPIOA, GPIO_PinSource14 , GPIO_AF_1); 56 | 57 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); 58 | 59 | USART_InitTypeDef USART_InitStructure; 60 | 61 | USART_InitStructure.USART_BaudRate = SERIAL_BAUDRATE; 62 | USART_InitStructure.USART_WordLength = USART_WordLength_8b; 63 | USART_InitStructure.USART_StopBits = USART_StopBits_1; 64 | USART_InitStructure.USART_Parity = USART_Parity_No; 65 | USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 66 | USART_InitStructure.USART_Mode = USART_Mode_Tx;//USART_Mode_Rx | USART_Mode_Tx; 67 | 68 | USART_Init(USART1, &USART_InitStructure); 69 | 70 | // USART_ITConfig(USART1, USART_IT_TXE, ENABLE); 71 | USART_Cmd(USART1, ENABLE); 72 | 73 | NVIC_InitTypeDef NVIC_InitStructure; 74 | 75 | NVIC_InitStructure.NVIC_IRQChannel = USART1_IRQn; 76 | NVIC_InitStructure.NVIC_IRQChannelPriority = 0; 77 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 78 | NVIC_Init(&NVIC_InitStructure); 79 | 80 | 81 | } 82 | 83 | 84 | int fputc(int ch, FILE * f) 85 | { 86 | buffer[buffer_end] = (char)ch; 87 | buffer_end++; 88 | buffer_end = buffer_end % (SERIAL_BUFFER_SIZE); 89 | 90 | USART_ITConfig(USART1, USART_IT_TXE, ENABLE); 91 | return ch; 92 | } 93 | 94 | void buffer_add(int val ) 95 | { 96 | buffer[buffer_end] = (char)val; 97 | buffer_end++; 98 | buffer_end = buffer_end % (SERIAL_BUFFER_SIZE); 99 | 100 | USART_ITConfig(USART1, USART_IT_TXE, ENABLE); 101 | return; 102 | } 103 | 104 | #else 105 | // serial disabled - dummy functions 106 | void serial_init(void) 107 | { 108 | 109 | } 110 | 111 | void buffer_add(int val ) 112 | { 113 | 114 | } 115 | 116 | #endif 117 | 118 | 119 | 120 | -------------------------------------------------------------------------------- /Silverware/src/drv_spi.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "project.h" 4 | #include "drv_spi.h" 5 | #include "binary.h" 6 | #include "config.h" 7 | 8 | #ifdef SOFTSPI_4WIRE 9 | 10 | void spi_init(void) 11 | { 12 | // spi port inits 13 | 14 | GPIO_InitTypeDef GPIO_InitStructure; 15 | 16 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 17 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 18 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; 19 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 20 | 21 | GPIO_InitStructure.GPIO_Pin = SPI_MOSI_PIN; 22 | GPIO_Init(SPI_MOSI_PORT, &GPIO_InitStructure); 23 | 24 | GPIO_InitStructure.GPIO_Pin = SPI_CLK_PIN; 25 | GPIO_Init(SPI_CLK_PORT, &GPIO_InitStructure); 26 | 27 | GPIO_InitStructure.GPIO_Pin = SPI_SS_PIN; 28 | GPIO_Init(SPI_SS_PORT, &GPIO_InitStructure); 29 | 30 | //miso should be input by default 31 | 32 | spi_csoff(); 33 | 34 | } 35 | 36 | 37 | #define gpioset( port , pin) port->BSRR = pin 38 | #define gpioreset( port , pin) port->BRR = pin 39 | 40 | #define MOSIHIGH gpioset( SPI_MOSI_PORT, SPI_MOSI_PIN) 41 | #define MOSILOW gpioreset( SPI_MOSI_PORT, SPI_MOSI_PIN); 42 | #define SCKHIGH gpioset( SPI_CLK_PORT, SPI_CLK_PIN); 43 | #define SCKLOW gpioreset( SPI_CLK_PORT, SPI_CLK_PIN); 44 | 45 | //#define READMISO (GPIO_ReadInputDataBit(SPI_MISO_PORT, SPI_MISO_PIN) ) 46 | #define READMISO (SPI_MISO_PORT->IDR & SPI_MISO_PIN) 47 | 48 | #pragma push 49 | 50 | #pragma Otime 51 | #pragma O2 52 | 53 | void spi_cson( ) 54 | { 55 | SPI_SS_PORT->BRR = SPI_SS_PIN; 56 | } 57 | 58 | void spi_csoff( ) 59 | { 60 | SPI_SS_PORT->BSRR = SPI_SS_PIN; 61 | } 62 | 63 | 64 | void spi_sendbyte ( int data) 65 | { 66 | for ( int i =7 ; i >=0 ; i--) 67 | { 68 | if ( (data>>i)&1 ) 69 | { 70 | MOSIHIGH; 71 | } 72 | else 73 | { 74 | MOSILOW; 75 | } 76 | 77 | SCKHIGH; 78 | SCKLOW; 79 | } 80 | } 81 | 82 | 83 | int spi_sendrecvbyte2( int data) 84 | { 85 | int recv = 0; 86 | for ( int i =7 ; i >=0 ; i--) 87 | { 88 | if ( (data) & (1<<7) ) 89 | { 90 | MOSIHIGH; 91 | } 92 | else 93 | { 94 | MOSILOW; 95 | } 96 | SCKHIGH; 97 | data = data<<1; 98 | if ( READMISO ) recv= recv|(1<<7); 99 | recv = recv<<1; 100 | SCKLOW; 101 | } 102 | recv = recv>>8; 103 | return recv; 104 | } 105 | 106 | 107 | int spi_sendrecvbyte( int data) 108 | { int recv = 0; 109 | 110 | for ( int i = 7 ; i >=0 ; i--) 111 | { 112 | recv = recv<<1; 113 | if ( (data) & (1<<7) ) 114 | { 115 | MOSIHIGH; 116 | } 117 | else 118 | { 119 | MOSILOW; 120 | } 121 | 122 | data = data<<1; 123 | 124 | SCKHIGH; 125 | 126 | if ( READMISO ) recv= recv|1; 127 | 128 | SCKLOW; 129 | 130 | } 131 | 132 | return recv; 133 | } 134 | 135 | 136 | int spi_sendzerorecvbyte( ) 137 | { int recv = 0; 138 | MOSILOW; 139 | 140 | for ( int i = 7 ; i >=0 ; i--) 141 | { 142 | recv = recv<<1; 143 | 144 | SCKHIGH; 145 | 146 | if ( READMISO ) recv= recv|1; 147 | 148 | SCKLOW; 149 | 150 | } 151 | return recv; 152 | } 153 | 154 | 155 | #pragma pop 156 | 157 | #endif 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | -------------------------------------------------------------------------------- /Silverware/src/motorcurve.c: -------------------------------------------------------------------------------- 1 | #include "config.h" 2 | 3 | #ifdef BOLDCLASH_716MM_8K 4 | 5 | float motormap(float input) 6 | { 7 | // this is a thrust to pwm function 8 | // float 0 to 1 input and output 9 | // output can go negative slightly 10 | // boldclash f03 with 716 motors ant 4 blade prop 11 | // a*x^2 + b*x + c 12 | 13 | if (input > 1) 14 | input = 1; 15 | if (input < 0) 16 | input = 0; 17 | 18 | input = input * input * 0.51f + input * (0.44f); 19 | input += 0.05f; 20 | 21 | return input; 22 | } 23 | #endif 24 | 25 | #ifdef BOLDCLASH_716MM_24K 26 | 27 | float motormap(float input) 28 | { 29 | // this is a thrust to pwm function 30 | // float 0 to 1 input and output 31 | // output can go negative slightly 32 | // boldclash f03 with 716 motors ant 4 blade prop 33 | // a*x^2 + b*x + c 34 | 35 | if (input > 1) 36 | input = 1; 37 | if (input < 0) 38 | input = 0; 39 | 40 | input = input * input * 0.149f + input * (0.846f); 41 | input += 0.005f; 42 | 43 | return input; 44 | } 45 | #endif 46 | 47 | #ifdef MOTOR_CURVE_6MM_490HZ 48 | // the old map for 490Hz 49 | float motormap(float input) 50 | { 51 | // this is a thrust to pwm function 52 | // float 0 to 1 input and output 53 | // output can go negative slightly 54 | // measured eachine motors and prop, stock battery 55 | // a*x^2 + b*x + c 56 | // a = 0.262 , b = 0.771 , c = -0.0258 57 | 58 | if (input > 1) 59 | input = 1; 60 | if (input < 0) 61 | input = 0; 62 | 63 | input = input * input * 0.262f + input * (0.771f); 64 | input += -0.0258f; 65 | 66 | return input; 67 | } 68 | #endif 69 | 70 | 71 | #ifdef MOTOR_CURVE_6MM_H101_490HZ 72 | float motormap( float input) 73 | { 74 | 75 | // H101 thrust curve for normal thrust direction 76 | // a*x^2 + b*x + c 77 | 78 | if (input > 1.0f) input = 1.0f; 79 | if (input < 0) input = 0; 80 | 81 | input = input*input*0.277f + input*(0.715f); 82 | input += 0.0102f; 83 | 84 | return input; 85 | } 86 | #endif 87 | 88 | // 8k pwm is where the motor thrust is relatively linear for the H8 6mm motors 89 | // it's due to the motor inductance cancelling the nonlinearities. 90 | #ifdef MOTOR_CURVE_NONE 91 | float motormap(float input) 92 | { 93 | return input; 94 | } 95 | #endif 96 | 97 | 98 | #ifdef MOTOR_CURVE_85MM_8KHZ 99 | // Hubsan 8.5mm 8khz pwm motor map 100 | float motormap(float input) 101 | { 102 | // Hubsan 8.5mm motors and props 103 | 104 | if (input > 1) 105 | input = 1; 106 | if (input < 0) 107 | input = 0; 108 | 109 | input = input * input * 0.683f + input * (0.262f); 110 | input += 0.06f; 111 | 112 | return input; 113 | } 114 | #endif 115 | 116 | 117 | #ifdef MOTOR_CURVE_85MM_8KHZ_OLD 118 | // Hubsan 8.5mm 8khz pwm motor map 119 | float motormap(float input) 120 | { 121 | // Hubsan 8.5mm motors and props 122 | 123 | if (input > 1) 124 | input = 1; 125 | if (input < 0) 126 | input = 0; 127 | 128 | input = input * input * 0.789f + input * (0.172f); 129 | input += 0.04f; 130 | 131 | return input; 132 | } 133 | #endif 134 | 135 | 136 | #ifdef MOTOR_CURVE_85MM_32KHZ 137 | // Hubsan 8.5mm 32khz pwm motor map 138 | float motormap(float input) 139 | { 140 | // Hubsan 8.5mm motors and props 141 | 142 | if (input > 1) 143 | input = 1; 144 | if (input < 0) 145 | input = 0; 146 | 147 | input = input * input * 0.197f + input * (0.74f); 148 | input += 0.067f; 149 | 150 | return input; 151 | } 152 | #endif 153 | 154 | -------------------------------------------------------------------------------- /Utilities/stm32f0xx_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file ADC/ADC_DMA/stm32f0xx_conf.h 4 | * @author MCD Application Team 5 | * @version V1.2.0 6 | * @date 11-April-2014 7 | * @brief Library configuration file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2014 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 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __STM32F0XX_CONF_H 30 | #define __STM32F0XX_CONF_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | /* Comment the line below to disable peripheral header file inclusion */ 34 | #include "stm32f0xx_adc.h" 35 | #include "stm32f0xx_cec.h" 36 | #include "stm32f0xx_crc.h" 37 | #include "stm32f0xx_comp.h" 38 | #include "stm32f0xx_dac.h" 39 | #include "stm32f0xx_dbgmcu.h" 40 | #include "stm32f0xx_dma.h" 41 | #include "stm32f0xx_exti.h" 42 | #include "stm32f0xx_flash.h" 43 | #include "stm32f0xx_gpio.h" 44 | #include "stm32f0xx_syscfg.h" 45 | #include "stm32f0xx_i2c.h" 46 | #include "stm32f0xx_iwdg.h" 47 | #include "stm32f0xx_pwr.h" 48 | #include "stm32f0xx_rcc.h" 49 | #include "stm32f0xx_rtc.h" 50 | #include "stm32f0xx_spi.h" 51 | #include "stm32f0xx_tim.h" 52 | #include "stm32f0xx_usart.h" 53 | #include "stm32f0xx_wwdg.h" 54 | #include "stm32f0xx_misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ 55 | 56 | /* Exported types ------------------------------------------------------------*/ 57 | /* Exported constants --------------------------------------------------------*/ 58 | /* Uncomment the line below to expanse the "assert_param" macro in the 59 | Standard Peripheral Library drivers code */ 60 | /* #define USE_FULL_ASSERT 1 */ 61 | 62 | /* Exported macro ------------------------------------------------------------*/ 63 | #ifdef USE_FULL_ASSERT 64 | 65 | /** 66 | * @brief The assert_param macro is used for function's parameters check. 67 | * @param expr: If expr is false, it calls assert_failed function which reports 68 | * the name of the source file and the source line number of the call 69 | * that failed. If expr is true, it returns no value. 70 | * @retval None 71 | */ 72 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 73 | /* Exported functions ------------------------------------------------------- */ 74 | void assert_failed(uint8_t* file, uint32_t line); 75 | #else 76 | #define assert_param(expr) ((void)0) 77 | #endif /* USE_FULL_ASSERT */ 78 | 79 | #endif /* __STM32F0XX_CONF_H */ 80 | 81 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 82 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Include/arm_const_structs.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------- 2 | * Copyright (C) 2010-2013 ARM Limited. All rights reserved. 3 | * 4 | * $Date: 17. January 2013 5 | * $Revision: V1.4.1 6 | * 7 | * Project: CMSIS DSP Library 8 | * Title: arm_const_structs.h 9 | * 10 | * Description: This file has constant structs that are initialized for 11 | * user convenience. For example, some can be given as 12 | * arguments to the arm_cfft_f32() function. 13 | * 14 | * Target Processor: Cortex-M4/Cortex-M3 15 | * 16 | * Redistribution and use in source and binary forms, with or without 17 | * modification, are permitted provided that the following conditions 18 | * are met: 19 | * - Redistributions of source code must retain the above copyright 20 | * notice, this list of conditions and the following disclaimer. 21 | * - Redistributions in binary form must reproduce the above copyright 22 | * notice, this list of conditions and the following disclaimer in 23 | * the documentation and/or other materials provided with the 24 | * distribution. 25 | * - Neither the name of ARM LIMITED nor the names of its contributors 26 | * may be used to endorse or promote products derived from this 27 | * software without specific prior written permission. 28 | * 29 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 30 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 31 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 32 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 33 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 34 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 35 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 36 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 37 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 38 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 39 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 40 | * POSSIBILITY OF SUCH DAMAGE. 41 | * -------------------------------------------------------------------- */ 42 | 43 | #ifndef _ARM_CONST_STRUCTS_H 44 | #define _ARM_CONST_STRUCTS_H 45 | 46 | #include "arm_math.h" 47 | #include "arm_common_tables.h" 48 | 49 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = { 50 | 16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH 51 | }; 52 | 53 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = { 54 | 32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH 55 | }; 56 | 57 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = { 58 | 64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH 59 | }; 60 | 61 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = { 62 | 128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH 63 | }; 64 | 65 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = { 66 | 256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH 67 | }; 68 | 69 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = { 70 | 512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH 71 | }; 72 | 73 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = { 74 | 1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH 75 | }; 76 | 77 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = { 78 | 2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH 79 | }; 80 | 81 | const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = { 82 | 4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH 83 | }; 84 | 85 | #endif 86 | -------------------------------------------------------------------------------- /gcc/flash.ld: -------------------------------------------------------------------------------- 1 | /* Entry Point */ 2 | ENTRY(Reset_Handler) 3 | 4 | /* Highest address of the user mode stack */ 5 | _estack = 0x20000FFF; /* end of RAM */ 6 | 7 | /* Generate a link error if heap and stack don't fit into RAM */ 8 | _Min_Heap_Size = 0x400; /* required amount of heap */ 9 | _Min_Stack_Size = 0x400; /* required amount of stack */ 10 | 11 | /* Specify the memory areas */ 12 | MEMORY 13 | { 14 | FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 16K 15 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 4K 16 | } 17 | 18 | /* Define output sections */ 19 | SECTIONS 20 | { 21 | /* The startup code goes first into FLASH */ 22 | .isr_vector : 23 | { 24 | . = ALIGN(4); 25 | KEEP(*(.isr_vector)) /* Startup code */ 26 | . = ALIGN(4); 27 | } >FLASH 28 | 29 | /* The program code and other data goes into FLASH */ 30 | .text : 31 | { 32 | . = ALIGN(4); 33 | *(.text) /* .text sections (code) */ 34 | *(.text*) /* .text* sections (code) */ 35 | *(.glue_7) /* glue arm to thumb code */ 36 | *(.glue_7t) /* glue thumb to arm code */ 37 | *(.eh_frame) 38 | 39 | KEEP (*(.init)) 40 | KEEP (*(.fini)) 41 | 42 | . = ALIGN(4); 43 | _etext = .; /* define a global symbols at end of code */ 44 | } >FLASH 45 | 46 | /* Constant data goes into FLASH */ 47 | .rodata : 48 | { 49 | . = ALIGN(4); 50 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 51 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 52 | . = ALIGN(4); 53 | } >FLASH 54 | 55 | .ARM.extab : { *(.ARM.extab* .gnu.linkonce.armextab.*) } >FLASH 56 | .ARM : { 57 | __exidx_start = .; 58 | *(.ARM.exidx*) 59 | __exidx_end = .; 60 | } >FLASH 61 | 62 | .preinit_array : 63 | { 64 | PROVIDE_HIDDEN (__preinit_array_start = .); 65 | KEEP (*(.preinit_array*)) 66 | PROVIDE_HIDDEN (__preinit_array_end = .); 67 | } >FLASH 68 | .init_array : 69 | { 70 | PROVIDE_HIDDEN (__init_array_start = .); 71 | KEEP (*(SORT(.init_array.*))) 72 | KEEP (*(.init_array*)) 73 | PROVIDE_HIDDEN (__init_array_end = .); 74 | } >FLASH 75 | .fini_array : 76 | { 77 | PROVIDE_HIDDEN (__fini_array_start = .); 78 | KEEP (*(.fini_array*)) 79 | KEEP (*(SORT(.fini_array.*))) 80 | PROVIDE_HIDDEN (__fini_array_end = .); 81 | } >FLASH 82 | 83 | /* used by the startup to initialize data */ 84 | _sidata = LOADADDR(.data); 85 | 86 | /* Initialized data sections goes into RAM, load LMA copy after code */ 87 | .data : 88 | { 89 | . = ALIGN(4); 90 | _sdata = .; /* create a global symbol at data start */ 91 | *(.data) /* .data sections */ 92 | *(.data*) /* .data* sections */ 93 | 94 | . = ALIGN(4); 95 | _edata = .; /* define a global symbol at data end */ 96 | } >RAM AT> FLASH 97 | 98 | /* Uninitialized data section */ 99 | . = ALIGN(4); 100 | .bss : 101 | { 102 | /* This is used by the startup in order to initialize the .bss secion */ 103 | _sbss = .; /* define a global symbol at bss start */ 104 | __bss_start__ = _sbss; 105 | *(.bss) 106 | *(.bss*) 107 | *(COMMON) 108 | 109 | . = ALIGN(4); 110 | _ebss = .; /* define a global symbol at bss end */ 111 | __bss_end__ = _ebss; 112 | } >RAM 113 | 114 | /* User_heap_stack section, used to check that there is enough RAM left */ 115 | ._user_heap_stack : 116 | { 117 | . = ALIGN(4); 118 | PROVIDE ( end = . ); 119 | PROVIDE ( _end = . ); 120 | . = . + _Min_Heap_Size; 121 | . = . + _Min_Stack_Size; 122 | . = ALIGN(4); 123 | } >RAM 124 | 125 | 126 | /* Remove information from the standard libraries */ 127 | /DISCARD/ : 128 | { 129 | libc.a ( * ) 130 | libm.a ( * ) 131 | libgcc.a ( * ) 132 | } 133 | 134 | .ARM.attributes 0 : { *(.ARM.attributes) } 135 | } 136 | -------------------------------------------------------------------------------- /INSTALL.md: -------------------------------------------------------------------------------- 1 | # Install and Flashing Instructions 2 | The flashing procedure consists of the "unlocking" of the board, as it is read/write protected originally, and flashing the actual firmware. A ST-LINK v2 is used, either clone, original, or Discovery/Nucleo board. 3 | 4 | Connections to the programming port require 3 wires, ground, swclk and swdat (swio). While flashing, the quad is powered from its battery. 5 | 6 | # Flashing over 16k 7 | The mcu chip used ( stm32f030k4 ) can be flashed to 32k, but unfortunately some modifications to openocd software need to be performed first. 8 | This is useful if some features make the binary over 16K. 9 | Read [this post](http://www.rcgroups.com/forums/showpost.php?p=38162521&postcount=11339) for more info 10 | 11 | # Windows 12 | 13 | See [RC Groups Thread First Post](http://www.rcgroups.com/forums/showthread.php?t=2634611) for now. 14 | 15 | # Linux 16 | 17 | For flashing on Linux, the [OpenOCD](http://openocd.org/) toolchain is used. The install instructions have been tested to work with OpenOCD 0.9.0 on Debian-based systems (Ubuntu 14.04). 18 | 19 | ## Install OpenOCD 0.9.0 20 | 21 | On some OS versions, older versions of OpenOCD might be the default. On Debian-based systems you can check which version is the default by running 22 | ``` 23 | apt-cache policy openocd 24 | ``` 25 | * If the output indicates a version below 0.9.0, you can install it on Debian-based systems (such as Ubuntu 14.04) using these commands: 26 | ``` 27 | wget http://lug.mtu.edu/ubuntu/pool/universe/h/hidapi/libhidapi-hidraw0_0.8.0~rc1+git20140201.3a66d4e+dfsg-3_amd64.deb 28 | sudo dpkg -i libhidapi-hidraw0_0.8.0~rc1+git20140201.3a66d4e+dfsg-3_amd64.deb 29 | wget http://ubuntu.mirrors.tds.net/ubuntu/pool/universe/j/jimtcl/libjim0.76_0.76-1_amd64.deb 30 | sudo dpkg -i libjim0.76_0.76-1_amd64.deb 31 | wget http://ubuntu.mirrors.tds.net/ubuntu/pool/universe/o/openocd/openocd_0.9.0-1build1_amd64.deb 32 | sudo dpkg -i openocd_0.9.0-1build1_amd64.deb 33 | ``` 34 | * If the output of the check indicates a version equal or higher to 0.9.0, just install openocd: 35 | ``` 36 | sudo apt-get install openocd 37 | ``` 38 | 39 | ## Toolchain Install and Build 40 | 41 | Run the following to install the necessary build tools: 42 | ``` 43 | apt-get install git build-essential gcc-arm-none-eabi libnewlib-arm-none-eabi 44 | ``` 45 | Clone the repository: 46 | ``` 47 | git clone https://github.com/silver13/Eachine-E011 48 | cd Eachine-E011 49 | ``` 50 | Build the firmware: 51 | ``` 52 | cd gcc 53 | make 54 | ``` 55 | 56 | ## Flashing 57 | 58 | Before being able to flash, the board needs to be unlocked. **This only has to be performed once for every flight controller board.** 59 | ``` 60 | openocd -f /usr/share/openocd/scripts/interface/stlink-v2.cfg -f /usr/share/openocd/scripts/target/stm32f0x.cfg -c init -c "reset halt" -c "stm32f0x unlock 0" -c "reset run" -c shutdown 61 | ``` 62 | 63 | The board needs a power cycle after unlocking. 64 | 65 | Once the board is unlocked, the firmware can be flashed using 66 | ``` 67 | openocd -f /usr/share/openocd/scripts/interface/stlink-v2.cfg -f /usr/share/openocd/scripts/target/stm32f0x.cfg -c init -c "reset halt" -c "flash write_image erase bwhoop 0x08000000" -c "verify_image bwhoop 0x08000000" -c "reset run" -c shutdown 68 | ``` 69 | 70 | ## Troubleshooting 71 | 72 | It appears that on Ubuntu 14.04, there is an issue with the standard .deb install for the build toolchain. You might get an error saying 73 | ``` 74 | arm-none-eabi-gcc: error: nano.specs: No such file or directory 75 | ``` 76 | In that case, another version of the ggc arm toolchain needs to be installed (see bug reports: [1](https://bugs.launchpad.net/gcc-arm-embedded/+bug/1309060), [2](https://bugs.launchpad.net/gcc-arm-embedded/+bug/1309060)): 77 | ``` 78 | sudo apt-get remove binutils-arm-none-eabi gcc-arm-none-eabi 79 | sudo add-apt-repository ppa:terry.guo/gcc-arm-embedded 80 | sudo apt-get update 81 | sudo apt-get install gcc-arm-none-eabi 82 | ``` 83 | After this, the "Build the firmware" step above can be performed. 84 | 85 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_wwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_wwdg.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the WWDG 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_WWDG_H 31 | #define __STM32F0XX_WWDG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup WWDG 45 | * @{ 46 | */ 47 | /* Exported types ------------------------------------------------------------*/ 48 | /* Exported constants --------------------------------------------------------*/ 49 | 50 | /** @defgroup WWDG_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup WWDG_Prescaler 55 | * @{ 56 | */ 57 | 58 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000) 59 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080) 60 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100) 61 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180) 62 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ 63 | ((PRESCALER) == WWDG_Prescaler_2) || \ 64 | ((PRESCALER) == WWDG_Prescaler_4) || \ 65 | ((PRESCALER) == WWDG_Prescaler_8)) 66 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) 67 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) 68 | 69 | /** 70 | * @} 71 | */ 72 | 73 | /** 74 | * @} 75 | */ 76 | 77 | /* Exported macro ------------------------------------------------------------*/ 78 | /* Exported functions ------------------------------------------------------- */ 79 | /* Function used to set the WWDG configuration to the default reset state ****/ 80 | void WWDG_DeInit(void); 81 | 82 | /* Prescaler, Refresh window and Counter configuration functions **************/ 83 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); 84 | void WWDG_SetWindowValue(uint8_t WindowValue); 85 | void WWDG_EnableIT(void); 86 | void WWDG_SetCounter(uint8_t Counter); 87 | 88 | /* WWDG activation functions **************************************************/ 89 | void WWDG_Enable(uint8_t Counter); 90 | 91 | /* Interrupts and flags management functions **********************************/ 92 | FlagStatus WWDG_GetFlagStatus(void); 93 | void WWDG_ClearFlag(void); 94 | 95 | #ifdef __cplusplus 96 | } 97 | #endif 98 | 99 | #endif /* __STM32F0XX_WWDG_H */ 100 | 101 | /** 102 | * @} 103 | */ 104 | 105 | /** 106 | * @} 107 | */ 108 | 109 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 110 | -------------------------------------------------------------------------------- /Silverware/src/osd_ltm.c: -------------------------------------------------------------------------------- 1 | // ltm protocol 2 | // This is currently non - standard in that it uses 115200 baudrate 3 | // Some of the data is also adjusted to work with current dev version of MWOSD 4 | // current = june '16 5 | // 6 | // a dummy gps frame is sent to stop "gps alerts" 7 | // the routine sends attitude, fc volts and rssi ( if available) 8 | 9 | #include "binary.h" 10 | #include "defines.h" 11 | #include "config.h" 12 | #include "rx_bayang.h" // for struct rxdebug; 13 | #include 14 | 15 | 16 | #ifdef OSD_LTM_PROTOCOL 17 | 18 | char crc; 19 | 20 | //extern int fputc(int ch, FILE * f); 21 | 22 | extern void buffer_add(int val ); 23 | 24 | void Serial_print(char ch) 25 | { 26 | buffer_add( (int) ch ); 27 | } 28 | 29 | 30 | void sendbyte( char val) 31 | { 32 | crc ^= val; 33 | Serial_print((char) val ); 34 | } 35 | 36 | void sendint( int val) 37 | { 38 | sendbyte( (char) (val) ); 39 | sendbyte( (char) (val>>8) ); 40 | } 41 | 42 | void sendcrc() 43 | { 44 | Serial_print((char) (crc) ); 45 | } 46 | 47 | 48 | void sendheader() 49 | { 50 | sendbyte((char)'$'); 51 | sendbyte((char)'T'); 52 | } 53 | 54 | // a frame 55 | // 6 bytes 56 | // Pitch, int16, [degrees] 57 | // Roll, int16, [degrees] 58 | // Heading, int16, [degrees] 59 | 60 | extern float attitude[]; 61 | 62 | void send_a_frame() 63 | { 64 | sendheader(); 65 | Serial_print((char) 'A'); 66 | crc = 0; 67 | sendint( attitude[0] + 0.5f );// 68 | sendint( attitude[1] + 0.5f); // roll (pitch?) 69 | sendint(0); //heading 70 | sendcrc(); 71 | } 72 | 73 | // g frame 74 | // 14 bytes 75 | 76 | // Latitude, int32 [decimal degrees * 10,000,000] 77 | // Longitude, int32 [decimal degrees * 10,000,000] 78 | // Ground Speed, uchar [m/s] 79 | // Altitude, uint32, [cm] 80 | // Sats, uchar, 81 | // bits 0-1 : fix 82 | // bits 2-7 : number of satellites 83 | 84 | void send_g_frame() 85 | { 86 | sendheader(); 87 | Serial_print( 'G'); 88 | crc = 0; 89 | /* 90 | sendint( 0 ); // lat 91 | sendint( 0 ); // lat2 92 | sendint( 0 ); // long 93 | sendint( 0 ); // long2 94 | */ 95 | sendbyte(0); // groundspeed (m/s) 96 | // sendint(0); // alti (cm) 97 | // sendint(0); // alti2 98 | 99 | // sending all above dummy data in one go for optimization 100 | for ( int i = 0 ; i < 6; i++) 101 | { 102 | sendint(0); 103 | } 104 | 105 | sendbyte(B00111111); // sats 106 | sendcrc(); 107 | } 108 | // S frame 109 | // 7 bytes 110 | 111 | // Vbat, uint16, [mV] 112 | // Current, uint16, [mA] 113 | // RSSI, uchar 114 | // Airspeed, uchar8, [m/s] 115 | // Status, uchar 116 | 117 | extern float vbattfilt; 118 | extern int failsafe; 119 | extern int rxmode; 120 | extern char aux[]; 121 | 122 | 123 | //extern int packetpersecond; 124 | extern struct rxdebug rxdebug; 125 | 126 | void send_s_frame() 127 | { 128 | sendheader(); 129 | Serial_print('S'); 130 | crc = 0; 131 | sendint( (unsigned int) vbattfilt *10 + 0.5f );// vbatt mV 126 = 12.6 132 | sendint( 1000 ); // current mA 133 | 134 | int rssi = rxdebug.packetpersecond; 135 | if (rssi > 255) rssi = 255; 136 | 137 | sendbyte(rssi); // rssi 138 | sendbyte(0); // airspeed 139 | #define ARMED ( (rxmode!=RXMODE_BIND) ) 140 | #define FAILSAFE failsafe 141 | #define MODE ( (aux[LEVELMODE])?3:4 ) 142 | 143 | //0 : Manual 144 | //1 : Rate 145 | //2 : Angle 146 | //3 : Horizon 147 | //4 : Acro 148 | char status = (ARMED<<0)|(FAILSAFE<<1)|(MODE<<2); 149 | 150 | sendbyte(status); // status 151 | 152 | sendcrc(); 153 | } 154 | 155 | 156 | int osdcount = 0; 157 | 158 | void osdcycle() 159 | { 160 | osdcount++; 161 | if (osdcount%30 == 29) 162 | { 163 | send_a_frame(); 164 | return; 165 | } 166 | 167 | if (osdcount%999 == 1) 168 | { 169 | send_g_frame(); 170 | return; 171 | } 172 | 173 | if (osdcount%332 == 5) 174 | { 175 | send_s_frame(); 176 | return; 177 | } 178 | 179 | } 180 | #else 181 | // ods disabled - dummy functions 182 | void osdcycle() 183 | { 184 | 185 | } 186 | 187 | 188 | 189 | 190 | #endif 191 | 192 | -------------------------------------------------------------------------------- /Silverware/src/drv_adc.c: -------------------------------------------------------------------------------- 1 | #include "project.h" 2 | #include "drv_adc.h" 3 | #include "util.h" 4 | #include "config.h" 5 | #include "debug.h" 6 | 7 | uint16_t adcarray[2]; 8 | extern debug_type debug; 9 | 10 | 11 | #ifndef DISABLE_LVC 12 | 13 | #ifndef ADC_BATT_VOLTAGE 14 | #define ADC_BATT_VOLTAGE 3.77 15 | #undef ADC_READOUT 16 | #define ADC_READOUT 2727 17 | #endif 18 | 19 | typedef struct { 20 | __IO uint16_t word1; 21 | __IO uint16_t word2; 22 | 23 | } adcrefcal; 24 | 25 | 26 | uint16_t adcref_read(adcrefcal* adcref_address) { 27 | return adcref_address ->word1; 28 | } 29 | 30 | 31 | float vref_cal; 32 | 33 | void adc_init(void) 34 | { 35 | ADC_InitTypeDef ADC_InitStructure; 36 | 37 | { 38 | GPIO_InitTypeDef GPIO_InitStructure; 39 | 40 | GPIO_InitStructure.GPIO_Pin = BATTERYPIN ; 41 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AN; 42 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL ; 43 | GPIO_Init(BATTERYPORT, &GPIO_InitStructure); 44 | } 45 | 46 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); 47 | 48 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1 , ENABLE); 49 | { 50 | DMA_InitTypeDef DMA_InitStructure; 51 | 52 | DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)0x40012440; 53 | DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)adcarray; 54 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; 55 | DMA_InitStructure.DMA_BufferSize = 2; 56 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; 57 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; 58 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; 59 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; 60 | DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; 61 | DMA_InitStructure.DMA_Priority = DMA_Priority_High; 62 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; 63 | DMA_Init(DMA1_Channel1, &DMA_InitStructure); 64 | } 65 | 66 | ADC_DMARequestModeConfig(ADC1, ADC_DMAMode_Circular); 67 | 68 | ADC_DMACmd(ADC1, ENABLE); 69 | 70 | ADC_StructInit(&ADC_InitStructure); 71 | 72 | 73 | ADC_InitStructure.ADC_Resolution = ADC_Resolution_12b; 74 | ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; 75 | ADC_InitStructure.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; 76 | ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; 77 | ADC_InitStructure.ADC_ScanDirection = ADC_ScanDirection_Backward; 78 | ADC_Init(ADC1, &ADC_InitStructure); 79 | 80 | ADC_ChannelConfig(ADC1, ADC_Channel_Vrefint , ADC_SampleTime_239_5Cycles); 81 | 82 | ADC_ChannelConfig(ADC1, BATTERY_ADC_CHANNEL , ADC_SampleTime_239_5Cycles); 83 | 84 | ADC_VrefintCmd(ENABLE); 85 | 86 | ADC_GetCalibrationFactor(ADC1); 87 | 88 | ADC_Cmd(ADC1, ENABLE); 89 | 90 | ADC_StartOfConversion(ADC1); 91 | 92 | DMA_Cmd(DMA1_Channel1, ENABLE); 93 | 94 | // reference is measured a 3.3v, we are powered by 2.8, so a 1.17 multiplier 95 | // different vccs will translate to a different adc scale factor, 96 | // so actual vcc is not important as long as the voltage is correct in the end 97 | vref_cal = 1.17857f * (float) ( adcref_read ((adcrefcal *) 0x1FFFF7BA) ); 98 | } 99 | 100 | float adc_read(int channel) 101 | { 102 | switch(channel) 103 | { 104 | case 0: 105 | #ifdef DEBUG 106 | lpf(&debug.adcfilt , (float) adcarray[0] , 0.998); 107 | #endif 108 | return (float) adcarray[0] * ((float) ADC_SCALEFACTOR) ; 109 | 110 | case 1: 111 | #ifdef DEBUG 112 | lpf(&debug.adcreffilt , (float) adcarray[1] , 0.998); 113 | #endif 114 | return vref_cal / (float) adcarray[1]; 115 | 116 | default: 117 | return 0; 118 | } 119 | 120 | 121 | } 122 | #else 123 | // // lvc disabled 124 | void adc_init(void) 125 | { 126 | 127 | } 128 | // dummy function with lvc disabled 129 | float adc_read(int channel) 130 | { 131 | switch(channel) 132 | { 133 | case 0: 134 | return 4.20f; 135 | 136 | case 1: 137 | return 1.0f; 138 | 139 | default: 140 | return 0; 141 | } 142 | 143 | 144 | } 145 | 146 | 147 | #endif 148 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_dbgmcu.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_dbgmcu.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the DBGMCU firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_DBGMCU_H 31 | #define __STM32F0XX_DBGMCU_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup DBGMCU 45 | * @{ 46 | */ 47 | /* Exported types ------------------------------------------------------------*/ 48 | /* Exported constants --------------------------------------------------------*/ 49 | 50 | 51 | /** @defgroup DBGMCU_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | #define DBGMCU_STOP DBGMCU_CR_DBG_STOP 56 | #define DBGMCU_STANDBY DBGMCU_CR_DBG_STANDBY 57 | #define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0xFFFFFFF9) == 0x00) && ((PERIPH) != 0x00)) 58 | 59 | #define DBGMCU_TIM2_STOP DBGMCU_APB1_FZ_DBG_TIM2_STOP /*!< Not applicable for STM32F030 devices */ 60 | #define DBGMCU_TIM3_STOP DBGMCU_APB1_FZ_DBG_TIM3_STOP 61 | #define DBGMCU_TIM6_STOP DBGMCU_APB1_FZ_DBG_TIM6_STOP 62 | #define DBGMCU_TIM7_STOP DBGMCU_APB1_FZ_DBG_TIM7_STOP /*!< Only applicable for STM32F072 devices */ 63 | #define DBGMCU_TIM14_STOP DBGMCU_APB1_FZ_DBG_TIM14_STOP 64 | #define DBGMCU_RTC_STOP DBGMCU_APB1_FZ_DBG_RTC_STOP 65 | #define DBGMCU_WWDG_STOP DBGMCU_APB1_FZ_DBG_WWDG_STOP 66 | #define DBGMCU_IWDG_STOP DBGMCU_APB1_FZ_DBG_IWDG_STOP 67 | #define DBGMCU_I2C1_SMBUS_TIMEOUT DBGMCU_APB1_FZ_DBG_I2C1_SMBUS_TIMEOUT 68 | #define DBGMCU_CAN1_STOP DBGMCU_APB1_FZ_DBG_CAN1_STOP /*!< Only applicable for STM32F042 and STM32F072 devices */ 69 | #define IS_DBGMCU_APB1PERIPH(PERIPH) ((((PERIPH) & 0xFDDFE2CC) == 0x00) && ((PERIPH) != 0x00)) 70 | 71 | #define DBGMCU_TIM1_STOP DBGMCU_APB2_FZ_DBG_TIM1_STOP 72 | #define DBGMCU_TIM15_STOP DBGMCU_APB2_FZ_DBG_TIM15_STOP 73 | #define DBGMCU_TIM16_STOP DBGMCU_APB2_FZ_DBG_TIM16_STOP 74 | #define DBGMCU_TIM17_STOP DBGMCU_APB2_FZ_DBG_TIM17_STOP 75 | #define IS_DBGMCU_APB2PERIPH(PERIPH) ((((PERIPH) & 0xFFF8F7FF) == 0x00) && ((PERIPH) != 0x00)) 76 | 77 | /** 78 | * @} 79 | */ 80 | 81 | /* Exported macro ------------------------------------------------------------*/ 82 | /* Exported functions ------------------------------------------------------- */ 83 | 84 | /* Device and Revision ID management functions ********************************/ 85 | uint32_t DBGMCU_GetREVID(void); 86 | uint32_t DBGMCU_GetDEVID(void); 87 | 88 | /* Peripherals Configuration functions ****************************************/ 89 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState); 90 | void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); 91 | void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState); 92 | 93 | #ifdef __cplusplus 94 | } 95 | #endif 96 | 97 | #endif /* __STM32F0XX_DBGMCU_H */ 98 | 99 | /** 100 | * @} 101 | */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 108 | -------------------------------------------------------------------------------- /Silverware/src/flip_sequencer.c: -------------------------------------------------------------------------------- 1 | #include "drv_time.h" 2 | #include "config.h" 3 | #include "flip_sequencer.h" 4 | 5 | 6 | #define THROTTLE_UP 0.8 7 | #define THROTTLE_HOVER 0.5 8 | #define THROTTLE_EXIT 0.6 9 | 10 | #define FLIP_RATE 1500 11 | //time to up throttle in beggining stage 12 | // 200ms 13 | #define THROTTLE_UP_TIME 200e3 14 | // end time in level mode 15 | #define LEVEL_MODE_TIME 200e3 16 | #define LEVEL_MODE_ANGLE -0 17 | 18 | #define FLIP_TIMEOUT_TOTAL 1500e3 19 | #define FLIP_TIMEOUT_STAGE1 500e3 20 | 21 | 22 | 23 | 24 | // don't change below 25 | 26 | #define STAGE_FLIP_NONE 0 27 | #define STAGE_FLIP_START 1 28 | #define STAGE_FLIP_THROTTLEUP 2 29 | #define STAGE_FLIP_ROTATING 3 30 | #define STAGE_FLIP_ROTATING_INVERTED 4 31 | #define STAGE_FLIP_LEVELMODE 5 32 | #define STAGE_FLIP_EXIT 6 33 | 34 | 35 | 36 | #include 37 | 38 | 39 | 40 | int acro_override = 0; 41 | int level_override = 0; 42 | int controls_override = 0; 43 | unsigned long fliptime; 44 | int isflipping = 0; 45 | int flipstage = STAGE_FLIP_NONE; 46 | unsigned int levelmodetime; 47 | int flipindex = 0; 48 | int flipdir = 0; 49 | 50 | extern int onground; 51 | extern float GEstG[3]; 52 | extern float rx[]; 53 | 54 | float rx_override[4]; 55 | 56 | void start_flip() 57 | { 58 | 59 | if ( isflipping == 0 && !onground) 60 | { 61 | isflipping = 1; 62 | fliptime = gettime(); 63 | flipstage = STAGE_FLIP_START; 64 | 65 | } 66 | 67 | flipindex = 0; 68 | flipdir = 0; 69 | if ( fabsf(rx[0]) < fabsf(rx[1]) ) 70 | { 71 | flipindex = 1; 72 | if ( rx[1] > 0 ) flipdir = 1; 73 | } 74 | else if ( rx[0] > 0 ) flipdir = 1; 75 | } 76 | 77 | 78 | void flip_sequencer() 79 | { 80 | if ( !isflipping) return; 81 | 82 | if (onground) flipstage = STAGE_FLIP_EXIT; 83 | 84 | if ( isflipping && gettime() - fliptime > FLIP_TIMEOUT_TOTAL ) 85 | { 86 | // abort after timeout second 87 | flipstage = STAGE_FLIP_EXIT; 88 | } 89 | 90 | switch (flipstage ) 91 | { 92 | case STAGE_FLIP_NONE: 93 | 94 | break; 95 | 96 | case STAGE_FLIP_START: 97 | // 98 | acro_override = 1; 99 | controls_override = 1; 100 | 101 | rx_override[1] = 0; 102 | rx_override[2] = 0; 103 | rx_override[3] = THROTTLE_UP; 104 | 105 | if ( GEstG[2] < 0 ) 106 | { 107 | // flip initiated inverted 108 | if ( flipdir) 109 | rx_override[flipindex] = (float) FLIP_RATE / (float) MAX_RATE; 110 | else 111 | rx_override[flipindex] = (float)- FLIP_RATE / (float) MAX_RATE; 112 | rx_override[3] = THROTTLE_HOVER; 113 | flipstage = STAGE_FLIP_ROTATING_INVERTED; 114 | } 115 | flipstage = STAGE_FLIP_THROTTLEUP; 116 | 117 | break; 118 | 119 | case STAGE_FLIP_THROTTLEUP: 120 | 121 | if ( gettime() - fliptime > THROTTLE_UP_TIME ) 122 | { 123 | if ( flipdir) 124 | rx_override[flipindex] = (float) FLIP_RATE / (float) MAX_RATE; 125 | else 126 | rx_override[flipindex] = (float)- FLIP_RATE / (float) MAX_RATE; 127 | rx_override[3] = THROTTLE_UP; 128 | flipstage = STAGE_FLIP_ROTATING; 129 | } 130 | 131 | break; 132 | 133 | case STAGE_FLIP_ROTATING: 134 | if ( gettime() - fliptime > FLIP_TIMEOUT_STAGE1 + THROTTLE_UP_TIME ) 135 | { 136 | // abort 137 | flipstage = STAGE_FLIP_EXIT; 138 | } 139 | if ( GEstG[2] < 0 ) 140 | { 141 | //we are inverted 142 | rx_override[3] = THROTTLE_HOVER; 143 | flipstage = STAGE_FLIP_ROTATING_INVERTED; 144 | } 145 | break; 146 | 147 | case STAGE_FLIP_ROTATING_INVERTED: 148 | if ( GEstG[2] > 0 ) 149 | { 150 | 151 | //we no longer inverted 152 | levelmodetime = gettime(); 153 | 154 | rx_override[3] = THROTTLE_EXIT; 155 | acro_override = 0; 156 | level_override = 1; 157 | flipstage = STAGE_FLIP_LEVELMODE; 158 | } 159 | break; 160 | 161 | 162 | case STAGE_FLIP_LEVELMODE: 163 | // allow control in other axis at this point 164 | rx_override[0] = rx[0]; 165 | rx_override[1] = rx[1]; 166 | rx_override[2] = rx[2]; 167 | 168 | if ( flipdir ) 169 | rx_override[flipindex] = (float) LEVEL_MODE_ANGLE / (float) MAX_ANGLE_HI; 170 | else 171 | rx_override[flipindex] = (float) - LEVEL_MODE_ANGLE / (float) MAX_ANGLE_HI; 172 | if( gettime() - levelmodetime > LEVEL_MODE_TIME ) 173 | flipstage = STAGE_FLIP_EXIT; 174 | break; 175 | 176 | 177 | case STAGE_FLIP_EXIT: 178 | isflipping = 0; 179 | flipstage = STAGE_FLIP_NONE; 180 | acro_override = 0; 181 | level_override = 0; 182 | controls_override = 0; 183 | break; 184 | 185 | 186 | default: 187 | flipstage = STAGE_FLIP_EXIT; 188 | break; 189 | 190 | } 191 | 192 | } 193 | 194 | 195 | -------------------------------------------------------------------------------- /Silverware/src/led.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 silverx 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | #include "project.h" 26 | #include "drv_time.h" 27 | #include "led.h" 28 | #include "config.h" 29 | 30 | #define LEDALL 15 31 | 32 | void ledon( uint8_t val ) 33 | { 34 | #if ( LED_NUMBER > 0 ) 35 | #ifdef LED1_INVERT 36 | if ( val&1) GPIO_ResetBits( LED1PORT, LED1PIN); 37 | #else 38 | if ( val&1) GPIO_SetBits( LED1PORT, LED1PIN); 39 | #endif 40 | #endif 41 | #if ( LED_NUMBER > 1 ) 42 | #ifdef LED2_INVERT 43 | if ( val&2) GPIO_ResetBits( LED2PORT, LED2PIN); 44 | #else 45 | if ( val&2) GPIO_SetBits( LED2PORT, LED2PIN); 46 | #endif 47 | #endif 48 | #if ( LED_NUMBER > 2 ) 49 | #ifdef LED3_INVERT 50 | if ( val&4) GPIO_ResetBits( LED3PORT, LED3PIN); 51 | #else 52 | if ( val&4) GPIO_SetBits( LED3PORT, LED3PIN); 53 | #endif 54 | #endif 55 | #if ( LED_NUMBER > 3 ) 56 | #ifdef LED4_INVERT 57 | if ( val&8) GPIO_ResetBits( LED4PORT, LED4PIN); 58 | #else 59 | if ( val&8) GPIO_SetBits( LED4PORT, LED4PIN); 60 | #endif 61 | #endif 62 | 63 | } 64 | 65 | void ledoff( uint8_t val ) 66 | { 67 | #if ( LED_NUMBER > 0 ) 68 | #ifdef LED1_INVERT 69 | if ( val&1) GPIO_SetBits( LED1PORT, LED1PIN); 70 | #else 71 | if ( val&1) GPIO_ResetBits( LED1PORT, LED1PIN); 72 | #endif 73 | #endif 74 | #if ( LED_NUMBER > 1 ) 75 | #ifdef LED2_INVERT 76 | if ( val&2) GPIO_SetBits( LED2PORT, LED2PIN); 77 | #else 78 | if ( val&2) GPIO_ResetBits( LED2PORT, LED2PIN); 79 | #endif 80 | #endif 81 | #if ( LED_NUMBER > 2 ) 82 | #ifdef LED3_INVERT 83 | if ( val&4) GPIO_SetBits( LED3PORT, LED3PIN); 84 | #else 85 | if ( val&4) GPIO_ResetBits( LED3PORT, LED3PIN); 86 | #endif 87 | #endif 88 | #if ( LED_NUMBER > 3 ) 89 | #ifdef LED1_INVERT 90 | if ( val&8) GPIO_SetBits( LED4PORT, LED4PIN); 91 | #else 92 | if ( val&8) GPIO_ResetBits( LED4PORT, LED4PIN); 93 | #endif 94 | #endif 95 | } 96 | 97 | void ledflash( uint32_t period , int duty ) 98 | { 99 | #if ( LED_NUMBER > 0 ) 100 | if ( gettime() % period > (period*duty)>>4 ) 101 | { 102 | ledon(LEDALL); 103 | } 104 | else 105 | { 106 | ledoff(LEDALL); 107 | } 108 | #endif 109 | } 110 | 111 | 112 | int ledlevel = 0; 113 | 114 | uint8_t led_pwm2( uint8_t pwmval) 115 | { 116 | static int loopcount = 0; 117 | 118 | ledlevel = pwmval; 119 | loopcount++; 120 | loopcount&=0xF; 121 | if ( ledlevel > loopcount ) 122 | { 123 | ledon( 255); 124 | } 125 | else 126 | { 127 | ledoff( 255); 128 | } 129 | return ledlevel; 130 | } 131 | 132 | int ledlevel2 = 0; 133 | unsigned long lastledtime; 134 | float lastledbrightness = 0; 135 | 136 | //#define DEBUG 137 | 138 | #ifdef DEBUG 139 | int debug_led; 140 | #endif 141 | 142 | #include "util.h" 143 | 144 | // delta- sigma first order modulator. 145 | uint8_t led_pwm( uint8_t pwmval) 146 | { 147 | static float ds_integrator= 0; 148 | unsigned int time = gettime(); 149 | unsigned int ledtime = time - lastledtime; 150 | 151 | lastledtime = time; 152 | 153 | 154 | float desiredbrightness = pwmval*( 1.0f/ 15.0f); 155 | 156 | // limitf( &lastledbrightness, 2); 157 | 158 | limitf( &ds_integrator, 2); 159 | 160 | ds_integrator += (desiredbrightness - lastledbrightness)*ledtime* ( 1.0f /(float) LOOPTIME); 161 | 162 | if ( ds_integrator > 0.49f ) 163 | { 164 | ledon( 255); 165 | lastledbrightness = 1.0f; 166 | #ifdef DEBUG 167 | debug_led<<=1; 168 | debug_led|=1; 169 | #endif 170 | } 171 | else 172 | { 173 | ledoff( 255); 174 | lastledbrightness = 0; 175 | #ifdef DEBUG 176 | debug_led<<=1; 177 | debug_led&=0xFFFFFFFE; 178 | #endif 179 | } 180 | return 0; 181 | } 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | -------------------------------------------------------------------------------- /Silverware/src/drv_spi_3wire.c: -------------------------------------------------------------------------------- 1 | 2 | 3 | #include "project.h" 4 | #include "drv_spi.h" 5 | #include "binary.h" 6 | #include "config.h" 7 | 8 | #ifdef SOFTSPI_3WIRE 9 | 10 | GPIO_InitTypeDef mosi_init_struct; 11 | int mosi_out = 0; 12 | 13 | void spi_init(void) 14 | { 15 | // spi port inits 16 | 17 | GPIO_InitTypeDef GPIO_InitStructure; 18 | 19 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT; 20 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 21 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP; 22 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 23 | 24 | GPIO_InitStructure.GPIO_Pin = SPI_CLK_PIN; 25 | GPIO_Init(SPI_CLK_PORT, &GPIO_InitStructure); 26 | 27 | GPIO_InitStructure.GPIO_Pin = SPI_SS_PIN; 28 | GPIO_Init(SPI_SS_PORT, &GPIO_InitStructure); 29 | 30 | 31 | mosi_init_struct.GPIO_Pin = SPI_MOSI_PIN; 32 | mosi_init_struct.GPIO_Mode = GPIO_Mode_IN; 33 | mosi_init_struct.GPIO_OType = GPIO_OType_PP; 34 | mosi_init_struct.GPIO_PuPd = GPIO_PuPd_UP; 35 | mosi_init_struct.GPIO_Speed = GPIO_Speed_50MHz; 36 | 37 | GPIO_Init(SPI_MOSI_PORT, &mosi_init_struct); 38 | 39 | //mosi_out = 0; // already 40 | 41 | spi_csoff(); 42 | 43 | } 44 | 45 | 46 | 47 | void mosi_input( void) 48 | { 49 | if ( mosi_out) 50 | { 51 | mosi_out = 0; 52 | mosi_init_struct.GPIO_Mode = GPIO_Mode_IN; 53 | GPIO_Init(SPI_MOSI_PORT, &mosi_init_struct); 54 | } 55 | 56 | } 57 | 58 | void mosi_output( void) 59 | { 60 | if ( !mosi_out) 61 | { 62 | mosi_out = 1; 63 | mosi_init_struct.GPIO_Mode = GPIO_Mode_OUT; 64 | GPIO_Init(SPI_MOSI_PORT, &mosi_init_struct); 65 | } 66 | 67 | } 68 | 69 | #define gpioset( port , pin) port->BSRR = pin 70 | #define gpioreset( port , pin) port->BRR = pin 71 | 72 | #define MOSIHIGH gpioset( SPI_MOSI_PORT, SPI_MOSI_PIN) 73 | #define MOSILOW gpioreset( SPI_MOSI_PORT, SPI_MOSI_PIN); 74 | #define SCKHIGH gpioset( SPI_CLK_PORT, SPI_CLK_PIN); 75 | #define SCKLOW gpioreset( SPI_CLK_PORT, SPI_CLK_PIN); 76 | 77 | 78 | #define READMOSI (SPI_MOSI_PORT->IDR & SPI_MOSI_PIN) 79 | 80 | #pragma push 81 | 82 | #pragma Otime 83 | #pragma O2 84 | 85 | void spi_cson( ) 86 | { 87 | SPI_SS_PORT->BRR = SPI_SS_PIN; 88 | } 89 | 90 | void spi_csoff( ) 91 | { 92 | SPI_SS_PORT->BSRR = SPI_SS_PIN; 93 | } 94 | 95 | 96 | void spi_sendbyte ( int data) 97 | { 98 | mosi_output(); 99 | for ( int i =7 ; i >=0 ; i--) 100 | { 101 | if ( (data>>i)&1 ) 102 | { 103 | MOSIHIGH; 104 | } 105 | else 106 | { 107 | MOSILOW; 108 | } 109 | 110 | SCKHIGH; 111 | SCKLOW; 112 | } 113 | } 114 | 115 | 116 | int spi_recvbyte( void) 117 | { int recv = 0; 118 | 119 | for ( int i = 7 ; i >=0 ; i--) 120 | { 121 | 122 | SCKHIGH; 123 | 124 | recv = recv<<1; 125 | 126 | recv = recv|((SPI_MOSI_PORT->IDR & (int)SPI_MOSI_PIN)?1:0); 127 | 128 | SCKLOW; 129 | 130 | } 131 | 132 | return recv; 133 | } 134 | 135 | 136 | int spi_recvbyte_unrolled( void) 137 | { uint8_t recv = 0; 138 | 139 | SCKHIGH; 140 | 141 | recv = recv<<1; 142 | 143 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 144 | 145 | SCKLOW; 146 | 147 | SCKHIGH; 148 | 149 | recv = recv<<1; 150 | 151 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 152 | 153 | SCKLOW; 154 | 155 | SCKHIGH; 156 | 157 | recv = recv<<1; 158 | 159 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 160 | 161 | SCKLOW; 162 | 163 | SCKHIGH; 164 | 165 | recv = recv<<1; 166 | 167 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 168 | 169 | SCKLOW; 170 | 171 | SCKHIGH; 172 | 173 | recv = recv<<1; 174 | 175 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 176 | 177 | SCKLOW; 178 | 179 | SCKHIGH; 180 | 181 | recv = recv<<1; 182 | 183 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 184 | 185 | SCKLOW; 186 | 187 | SCKHIGH; 188 | 189 | recv = recv<<1; 190 | 191 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 192 | 193 | SCKLOW; 194 | 195 | SCKHIGH; 196 | 197 | recv = recv<<1; 198 | 199 | recv = recv|((SPI_MOSI_PORT->IDR & SPI_MOSI_PIN)?1:0); 200 | 201 | SCKLOW; 202 | 203 | 204 | return recv; 205 | } 206 | 207 | /* 208 | 209 | int spi_sendzerorecvbyte( ) 210 | { int recv = 0; 211 | MOSILOW; 212 | 213 | for ( int i = 7 ; i >=0 ; i--) 214 | { 215 | recv = recv<<1; 216 | 217 | SCKHIGH; 218 | 219 | if ( READMISO ) recv= recv|1; 220 | 221 | SCKLOW; 222 | 223 | } 224 | return recv; 225 | } 226 | */ 227 | 228 | #pragma pop 229 | 230 | #endif 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | -------------------------------------------------------------------------------- /Silverware/src/drv_time.c: -------------------------------------------------------------------------------- 1 | // 2 | #include "project.h" 3 | #include "drv_time.h" 4 | #include "config.h" 5 | 6 | void failloop( int val); 7 | 8 | unsigned long lastticks; 9 | unsigned long globalticks; 10 | volatile unsigned long systickcount = 0; 11 | 12 | 13 | #ifndef SYS_CLOCK_FREQ_HZ 14 | #define SYS_CLOCK_FREQ_HZ 48000000 15 | #warning SYS_CLOCK_FREQ_HZ not present 16 | #endif 17 | 18 | 19 | // divider by 8 is enabled in this systick config 20 | static __INLINE uint32_t SysTick_Config2(uint32_t ticks) 21 | { 22 | if (ticks > SysTick_LOAD_RELOAD_Msk) return (1); /* Reload value impossible */ 23 | 24 | SysTick->LOAD = (ticks & SysTick_LOAD_RELOAD_Msk) - 1; /* set reload register */ 25 | NVIC_SetPriority (SysTick_IRQn, (1<<__NVIC_PRIO_BITS) - 1); /* set Priority for Cortex-M0 System Interrupts */ 26 | SysTick->VAL = 0; /* Load the SysTick Counter Value */ 27 | SysTick->CTRL = //SysTick_CTRL_CLKSOURCE_Msk | // divide by 8 28 | SysTick_CTRL_TICKINT_Msk | 29 | SysTick_CTRL_ENABLE_Msk; /* Enable SysTick IRQ and SysTick Timer */ 30 | return (0); /* Function successful */ 31 | } 32 | 33 | 34 | void time_init() 35 | { 36 | if (SysTick_Config2( SYS_CLOCK_FREQ_HZ /8 )) 37 | {// not able to set divider 38 | failloop(5); 39 | } 40 | } 41 | 42 | // return time in uS from start ( micros()) 43 | // called at least once per second or time will overflow 44 | unsigned long gettime(void) 45 | { 46 | 47 | unsigned long maxticks = SysTick->LOAD; 48 | unsigned long ticks = SysTick->VAL; 49 | unsigned long elapsedticks; 50 | 51 | static unsigned long remainder = 0; // carry forward the remainder ticks; 52 | 53 | if (ticks < lastticks) elapsedticks = lastticks - ticks; 54 | else 55 | {// overflow ( underflow really) 56 | elapsedticks = lastticks + ( maxticks - ticks); 57 | } 58 | 59 | lastticks = ticks; 60 | elapsedticks += remainder; 61 | 62 | #ifdef ENABLE_OVERCLOCK 63 | const unsigned long quotient = elapsedticks / 8; 64 | remainder = elapsedticks - quotient * 8; 65 | #else 66 | unsigned long quotient = elapsedticks / 6; 67 | remainder = elapsedticks - quotient * 6; 68 | #endif 69 | globalticks += quotient; 70 | return globalticks; 71 | } 72 | 73 | 74 | 75 | #ifdef ENABLE_OVERCLOCK 76 | // delay in uS 77 | void delay(uint32_t data) 78 | { 79 | volatile uint32_t count; 80 | count = data * 7; 81 | while (count--); 82 | } 83 | #else 84 | // delay in uS 85 | void delay(uint32_t data) 86 | { 87 | volatile uint32_t count; 88 | count = data * 5; 89 | while (count--); 90 | } 91 | #endif 92 | 93 | void SysTick_Handler(void) 94 | { 95 | 96 | } 97 | -------------------------------------------------------------------------------- /Libraries/CMSIS/Include/arm_common_tables.h: -------------------------------------------------------------------------------- 1 | /* ---------------------------------------------------------------------- 2 | * Copyright (C) 2010-2013 ARM Limited. All rights reserved. 3 | * 4 | * $Date: 17. January 2013 5 | * $Revision: V1.4.1 6 | * 7 | * Project: CMSIS DSP Library 8 | * Title: arm_common_tables.h 9 | * 10 | * Description: This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions 11 | * 12 | * Target Processor: Cortex-M4/Cortex-M3 13 | * 14 | * Redistribution and use in source and binary forms, with or without 15 | * modification, are permitted provided that the following conditions 16 | * are met: 17 | * - Redistributions of source code must retain the above copyright 18 | * notice, this list of conditions and the following disclaimer. 19 | * - Redistributions in binary form must reproduce the above copyright 20 | * notice, this list of conditions and the following disclaimer in 21 | * the documentation and/or other materials provided with the 22 | * distribution. 23 | * - Neither the name of ARM LIMITED nor the names of its contributors 24 | * may be used to endorse or promote products derived from this 25 | * software without specific prior written permission. 26 | * 27 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 28 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 29 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 30 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 31 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 32 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 33 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 34 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 35 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 36 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 37 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 38 | * POSSIBILITY OF SUCH DAMAGE. 39 | * -------------------------------------------------------------------- */ 40 | 41 | #ifndef _ARM_COMMON_TABLES_H 42 | #define _ARM_COMMON_TABLES_H 43 | 44 | #include "arm_math.h" 45 | 46 | extern const uint16_t armBitRevTable[1024]; 47 | extern const q15_t armRecipTableQ15[64]; 48 | extern const q31_t armRecipTableQ31[64]; 49 | extern const q31_t realCoefAQ31[1024]; 50 | extern const q31_t realCoefBQ31[1024]; 51 | extern const float32_t twiddleCoef_16[32]; 52 | extern const float32_t twiddleCoef_32[64]; 53 | extern const float32_t twiddleCoef_64[128]; 54 | extern const float32_t twiddleCoef_128[256]; 55 | extern const float32_t twiddleCoef_256[512]; 56 | extern const float32_t twiddleCoef_512[1024]; 57 | extern const float32_t twiddleCoef_1024[2048]; 58 | extern const float32_t twiddleCoef_2048[4096]; 59 | extern const float32_t twiddleCoef_4096[8192]; 60 | #define twiddleCoef twiddleCoef_4096 61 | extern const q31_t twiddleCoefQ31[6144]; 62 | extern const q15_t twiddleCoefQ15[6144]; 63 | extern const float32_t twiddleCoef_rfft_32[32]; 64 | extern const float32_t twiddleCoef_rfft_64[64]; 65 | extern const float32_t twiddleCoef_rfft_128[128]; 66 | extern const float32_t twiddleCoef_rfft_256[256]; 67 | extern const float32_t twiddleCoef_rfft_512[512]; 68 | extern const float32_t twiddleCoef_rfft_1024[1024]; 69 | extern const float32_t twiddleCoef_rfft_2048[2048]; 70 | extern const float32_t twiddleCoef_rfft_4096[4096]; 71 | 72 | 73 | #define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20 ) 74 | #define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48 ) 75 | #define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56 ) 76 | #define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 ) 77 | #define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 ) 78 | #define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 ) 79 | #define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800) 80 | #define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808) 81 | #define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032) 82 | 83 | extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH]; 84 | extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH]; 85 | extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH]; 86 | extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH]; 87 | extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH]; 88 | extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH]; 89 | extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH]; 90 | extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH]; 91 | extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH]; 92 | 93 | #endif /* ARM_COMMON_TABLES_H */ 94 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_misc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_misc.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the miscellaneous 8 | * firmware library functions (add-on to CMSIS functions). 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_MISC_H 31 | #define __STM32F0XX_MISC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup MISC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | 50 | /** 51 | * @brief NVIC Init Structure definition 52 | */ 53 | 54 | typedef struct 55 | { 56 | uint8_t NVIC_IRQChannel; /*!< Specifies the IRQ channel to be enabled or disabled. 57 | This parameter can be a value of @ref IRQn_Type 58 | (For the complete STM32 Devices IRQ Channels list, 59 | please refer to stm32f0xx.h file) */ 60 | 61 | uint8_t NVIC_IRQChannelPriority; /*!< Specifies the priority level for the IRQ channel specified 62 | in NVIC_IRQChannel. This parameter can be a value 63 | between 0 and 3. */ 64 | 65 | FunctionalState NVIC_IRQChannelCmd; /*!< Specifies whether the IRQ channel defined in NVIC_IRQChannel 66 | will be enabled or disabled. 67 | This parameter can be set either to ENABLE or DISABLE */ 68 | } NVIC_InitTypeDef; 69 | 70 | /** 71 | * 72 | @verbatim 73 | 74 | @endverbatim 75 | */ 76 | 77 | /* Exported constants --------------------------------------------------------*/ 78 | 79 | /** @defgroup MISC_Exported_Constants 80 | * @{ 81 | */ 82 | 83 | /** @defgroup MISC_System_Low_Power 84 | * @{ 85 | */ 86 | 87 | #define NVIC_LP_SEVONPEND ((uint8_t)0x10) 88 | #define NVIC_LP_SLEEPDEEP ((uint8_t)0x04) 89 | #define NVIC_LP_SLEEPONEXIT ((uint8_t)0x02) 90 | #define IS_NVIC_LP(LP) (((LP) == NVIC_LP_SEVONPEND) || \ 91 | ((LP) == NVIC_LP_SLEEPDEEP) || \ 92 | ((LP) == NVIC_LP_SLEEPONEXIT)) 93 | /** 94 | * @} 95 | */ 96 | 97 | /** @defgroup MISC_Preemption_Priority_Group 98 | * @{ 99 | */ 100 | #define IS_NVIC_PRIORITY(PRIORITY) ((PRIORITY) < 0x04) 101 | 102 | /** 103 | * @} 104 | */ 105 | 106 | /** @defgroup MISC_SysTick_clock_source 107 | * @{ 108 | */ 109 | 110 | #define SysTick_CLKSource_HCLK_Div8 ((uint32_t)0xFFFFFFFB) 111 | #define SysTick_CLKSource_HCLK ((uint32_t)0x00000004) 112 | #define IS_SYSTICK_CLK_SOURCE(SOURCE) (((SOURCE) == SysTick_CLKSource_HCLK) || \ 113 | ((SOURCE) == SysTick_CLKSource_HCLK_Div8)) 114 | /** 115 | * @} 116 | */ 117 | 118 | /** 119 | * @} 120 | */ 121 | 122 | /* Exported macro ------------------------------------------------------------*/ 123 | /* Exported functions ------------------------------------------------------- */ 124 | 125 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct); 126 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState); 127 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource); 128 | 129 | #ifdef __cplusplus 130 | } 131 | #endif 132 | 133 | #endif /* __STM32F0XX_MISC_H */ 134 | 135 | /** 136 | * @} 137 | */ 138 | 139 | /** 140 | * @} 141 | */ 142 | 143 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 144 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_iwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_iwdg.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the IWDG 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_IWDG_H 31 | #define __STM32F0XX_IWDG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup IWDG 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup IWDG_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** @defgroup IWDG_WriteAccess 56 | * @{ 57 | */ 58 | 59 | #define IWDG_WriteAccess_Enable ((uint16_t)0x5555) 60 | #define IWDG_WriteAccess_Disable ((uint16_t)0x0000) 61 | #define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \ 62 | ((ACCESS) == IWDG_WriteAccess_Disable)) 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @defgroup IWDG_prescaler 68 | * @{ 69 | */ 70 | 71 | #define IWDG_Prescaler_4 ((uint8_t)0x00) 72 | #define IWDG_Prescaler_8 ((uint8_t)0x01) 73 | #define IWDG_Prescaler_16 ((uint8_t)0x02) 74 | #define IWDG_Prescaler_32 ((uint8_t)0x03) 75 | #define IWDG_Prescaler_64 ((uint8_t)0x04) 76 | #define IWDG_Prescaler_128 ((uint8_t)0x05) 77 | #define IWDG_Prescaler_256 ((uint8_t)0x06) 78 | #define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \ 79 | ((PRESCALER) == IWDG_Prescaler_8) || \ 80 | ((PRESCALER) == IWDG_Prescaler_16) || \ 81 | ((PRESCALER) == IWDG_Prescaler_32) || \ 82 | ((PRESCALER) == IWDG_Prescaler_64) || \ 83 | ((PRESCALER) == IWDG_Prescaler_128)|| \ 84 | ((PRESCALER) == IWDG_Prescaler_256)) 85 | /** 86 | * @} 87 | */ 88 | 89 | /** @defgroup IWDG_Flag 90 | * @{ 91 | */ 92 | 93 | #define IWDG_FLAG_PVU IWDG_SR_PVU 94 | #define IWDG_FLAG_RVU IWDG_SR_RVU 95 | #define IWDG_FLAG_WVU IWDG_SR_WVU 96 | #define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU) || \ 97 | ((FLAG) == IWDG_FLAG_WVU)) 98 | 99 | #define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF) 100 | 101 | #define IS_IWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0xFFF) 102 | /** 103 | * @} 104 | */ 105 | 106 | /** 107 | * @} 108 | */ 109 | 110 | /* Exported macro ------------------------------------------------------------*/ 111 | /* Exported functions ------------------------------------------------------- */ 112 | 113 | /* Prescaler and Counter configuration functions ******************************/ 114 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess); 115 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler); 116 | void IWDG_SetReload(uint16_t Reload); 117 | void IWDG_ReloadCounter(void); 118 | void IWDG_SetWindowValue(uint16_t WindowValue); 119 | 120 | /* IWDG activation function ***************************************************/ 121 | void IWDG_Enable(void); 122 | 123 | /* Flag management function ***************************************************/ 124 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG); 125 | 126 | #ifdef __cplusplus 127 | } 128 | #endif 129 | 130 | #endif /* __STM32F0XX_IWDG_H */ 131 | 132 | /** 133 | * @} 134 | */ 135 | 136 | /** 137 | * @} 138 | */ 139 | 140 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 141 | -------------------------------------------------------------------------------- /Silverware/src/util.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 silverx 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | 26 | #include 27 | #include "util.h" 28 | #include "drv_time.h" 29 | 30 | 31 | // calculates the coefficient for lpf filter, times in the same units 32 | float lpfcalc(float sampleperiod, float filtertime) { 33 | float ga = 1.0f - sampleperiod / filtertime; 34 | if (ga > 1.0f) 35 | ga = 1.0f; 36 | if (ga < 0.0f) 37 | ga = 0.0f; 38 | return ga; 39 | } 40 | 41 | 42 | // calculates the coefficient for lpf filter 43 | float lpfcalc_hz(float sampleperiod, float filterhz) { 44 | float ga = 1.0f - sampleperiod * filterhz; 45 | if (ga > 1.0f) 46 | ga = 1.0f; 47 | if (ga < 0.0f) 48 | ga = 0.0f; 49 | return ga; 50 | } 51 | 52 | 53 | float mapf(float x, float in_min, float in_max, float out_min, float out_max) 54 | { 55 | 56 | return ((x - in_min) * (out_max - out_min)) / (in_max - in_min) + out_min; 57 | 58 | } 59 | 60 | 61 | void lpf( float *out, float in , float coeff) 62 | { 63 | *out = ( *out )* coeff + in * ( 1-coeff); 64 | } 65 | 66 | 67 | void limitf ( float *input , const float limit) 68 | { 69 | if (*input > limit) *input = limit; 70 | if (*input < - limit) *input = - limit; 71 | } 72 | 73 | float rcexpo ( float in , float exp ) 74 | { 75 | if ( exp > 1 ) exp = 1; 76 | if ( exp < -1 ) exp = -1; 77 | float ans = in*in*in * exp + in * ( 1 - exp ); 78 | limitf( &ans , 1.0); 79 | return ans; 80 | } 81 | 82 | 83 | // timing routines for debugging 84 | static unsigned long timestart; 85 | unsigned long timeend; 86 | 87 | // timestart 88 | void TS( void) 89 | { 90 | timestart = gettime(); 91 | } 92 | // timeend 93 | void TE( void) 94 | { 95 | timeend =( gettime() - timestart ); 96 | } 97 | 98 | 99 | 100 | float fastsin( float x ) 101 | { 102 | //always wrap input angle to -PI..PI 103 | while (x < -3.14159265f) 104 | x += 6.28318531f; 105 | 106 | while (x > 3.14159265f) 107 | x -= 6.28318531f; 108 | float sin1; 109 | 110 | //compute sine 111 | if (x < 0) 112 | sin1 = (1.27323954f + .405284735f * x) *x; 113 | else 114 | sin1 = (1.27323954f - .405284735f * x) *x; 115 | 116 | 117 | return sin1; 118 | 119 | } 120 | 121 | 122 | float fastcos( float x ) 123 | { 124 | x += 1.57079632f; 125 | return fastsin(x); 126 | } 127 | 128 | 129 | 130 | #include 131 | uint32_t seed = 7; 132 | uint32_t random( void) 133 | { 134 | seed ^= seed << 13; 135 | seed ^= seed >> 17; 136 | seed ^= seed << 5; 137 | return seed; 138 | } 139 | 140 | 141 | // serial print routines 142 | #ifdef SERIAL_ENABLE 143 | 144 | extern void buffer_add(int val ); 145 | #include 146 | 147 | // print a 32bit signed int 148 | void print_int( int val ) 149 | { 150 | // multiple of 4 151 | #define SP_INT_BUFFERSIZE 12 152 | char buffer2[SP_INT_BUFFERSIZE]; 153 | 154 | if (val < 0) 155 | { 156 | buffer_add( (char) '-' ); 157 | val = abs(val); 158 | } 159 | 160 | int power = SP_INT_BUFFERSIZE; 161 | 162 | do 163 | { 164 | power--; 165 | int quotient = val/(10); 166 | int remainder = val-quotient*10; 167 | val = quotient; 168 | buffer2[power] = remainder+'0'; 169 | } 170 | while (( val ) && power >=0) ; 171 | 172 | 173 | for ( ; power <= SP_INT_BUFFERSIZE-1 ; power++) 174 | { 175 | buffer_add(buffer2[power] ); 176 | } 177 | } 178 | 179 | // print float with 2 decimal points 180 | // this does not handle Nans inf and values over 32bit signed int 181 | void print_float( float val ) 182 | { 183 | 184 | int ival = (int) val; 185 | 186 | if ( val < 0 && ival == 0 ) buffer_add( (char) '-' ); 187 | 188 | print_int( ival ); 189 | 190 | buffer_add( (char) '.' ); 191 | 192 | val = val - (int) val; 193 | 194 | int decimals = val * 100; 195 | 196 | decimals = abs(decimals); 197 | 198 | if (decimals < 10) buffer_add( (char) '0' ); 199 | print_int( decimals ); 200 | 201 | } 202 | 203 | void print_str(const char *str) 204 | { 205 | int count = 0; 206 | // a 64 character limit so we don't print the entire flash by mistake 207 | while (str[count]&&!(count>>6) ) 208 | { 209 | buffer_add( (char) str[count] ); 210 | count++; 211 | } 212 | } 213 | 214 | #endif 215 | 216 | -------------------------------------------------------------------------------- /Silverware/src/imu.c: -------------------------------------------------------------------------------- 1 | 2 | // library headers 3 | #include 4 | #include 5 | 6 | //#define _USE_MATH_DEFINES 7 | #include 8 | #include "drv_time.h" 9 | 10 | #include "util.h" 11 | #include "sixaxis.h" 12 | #include "config.h" 13 | 14 | #include 15 | 16 | 17 | #ifdef DEBUG 18 | #include "debug.h" 19 | extern debug_type debug; 20 | #endif 21 | 22 | #define ACC_1G 1.0f 23 | 24 | // disable drift correction ( for testing) 25 | #define DISABLE_ACC 0 26 | 27 | // filter time in seconds 28 | // time to correct gyro readings using the accelerometer 29 | // 1-4 are generally good 30 | #define FILTERTIME 2.0 31 | 32 | // accel magnitude limits for drift correction 33 | #define ACC_MIN 0.7f 34 | #define ACC_MAX 1.3f 35 | 36 | 37 | float GEstG[3] = { 0, 0, ACC_1G }; 38 | 39 | float attitude[3]; 40 | 41 | extern float gyro[3]; 42 | extern float accel[3]; 43 | extern float accelcal[3]; 44 | 45 | 46 | void imu_init(void) 47 | { 48 | // init the gravity vector with accel values 49 | for (int xx = 0; xx < 100; xx++) 50 | { 51 | sixaxis_read(); 52 | 53 | for (int x = 0; x < 3; x++) 54 | { 55 | lpf(&GEstG[x], accel[x]* ( 1/ 2048.0f) , 0.85); 56 | } 57 | delay(1000); 58 | 59 | 60 | } 61 | } 62 | 63 | // from http://en.wikipedia.org/wiki/Fast_inverse_square_root 64 | // originally from quake3 code 65 | 66 | float Q_rsqrt( float number ) 67 | { 68 | 69 | long i; 70 | float x2, y; 71 | const float threehalfs = 1.5F; 72 | 73 | x2 = number * 0.5F; 74 | y = number; 75 | i = * ( long * ) &y; 76 | i = 0x5f3759df - ( i >> 1 ); 77 | y = * ( float * ) &i; 78 | y = y * ( threehalfs - ( x2 * y * y ) ); // 1st iteration 79 | y = y * ( threehalfs - ( x2 * y * y ) ); // 2nd iteration, this can be removed 80 | // y = y * ( threehalfs - ( x2 * y * y ) ); // 3nd iteration, this can be removed 81 | 82 | return y; 83 | } 84 | 85 | 86 | void vectorcopy(float *vector1, float *vector2); 87 | 88 | 89 | float atan2approx(float y, float x); 90 | 91 | float calcmagnitude(float vector[3]) 92 | { 93 | float accmag = 0; 94 | for (uint8_t axis = 0; axis < 3; axis++) 95 | { 96 | accmag += vector[axis] * vector[axis]; 97 | } 98 | accmag = 1.0f/Q_rsqrt(accmag); 99 | return accmag; 100 | } 101 | 102 | 103 | void vectorcopy(float *vector1, float *vector2) 104 | { 105 | for (int axis = 0; axis < 3; axis++) 106 | { 107 | vector1[axis] = vector2[axis]; 108 | } 109 | } 110 | 111 | extern float looptime; 112 | 113 | void imu_calc(void) 114 | { 115 | 116 | 117 | 118 | // remove bias 119 | accel[0] = accel[0] - accelcal[0]; 120 | accel[1] = accel[1] - accelcal[1]; 121 | 122 | 123 | // reduce to accel in G 124 | for (int i = 0; i < 3; i++) 125 | { 126 | accel[i] *= ( 1/ 2048.0f); 127 | } 128 | 129 | 130 | float deltaGyroAngle[3]; 131 | 132 | for ( int i = 0 ; i < 3 ; i++) 133 | { 134 | deltaGyroAngle[i] = (gyro[i]) * looptime; 135 | } 136 | 137 | 138 | GEstG[2] = GEstG[2] - (deltaGyroAngle[0]) * GEstG[0]; 139 | GEstG[0] = (deltaGyroAngle[0]) * GEstG[2] + GEstG[0]; 140 | 141 | 142 | GEstG[1] = GEstG[1] + (deltaGyroAngle[1]) * GEstG[2]; 143 | GEstG[2] = -(deltaGyroAngle[1]) * GEstG[1] + GEstG[2]; 144 | 145 | 146 | GEstG[0] = GEstG[0] - (deltaGyroAngle[2]) * GEstG[1]; 147 | GEstG[1] = (deltaGyroAngle[2]) * GEstG[0] + GEstG[1]; 148 | 149 | 150 | // calc acc mag 151 | float accmag; 152 | 153 | accmag = calcmagnitude(&accel[0]); 154 | 155 | 156 | if ((accmag > ACC_MIN * ACC_1G) && (accmag < ACC_MAX * ACC_1G) && !DISABLE_ACC) 157 | { 158 | // normalize acc 159 | for (int axis = 0; axis < 3; axis++) 160 | { 161 | accel[axis] = accel[axis] * ( ACC_1G / accmag); 162 | } 163 | float filtcoeff = lpfcalc_hz( looptime, 1.0f/(float)FILTERTIME); 164 | for (int x = 0; x < 3; x++) 165 | { 166 | lpf(&GEstG[x], accel[x], filtcoeff); 167 | } 168 | } 169 | 170 | 171 | // vectorcopy(&GEstG[0], &EstG[0]); 172 | #ifdef DEBUG 173 | attitude[0] = atan2approx(EstG[0], EstG[2]) ; 174 | 175 | attitude[1] = atan2approx(EstG[1], EstG[2]) ; 176 | #endif 177 | } 178 | 179 | 180 | 181 | #define M_PI 3.14159265358979323846 /* pi */ 182 | 183 | 184 | #define OCTANTIFY(_x, _y, _o) do { \ 185 | float _t; \ 186 | _o= 0; \ 187 | if(_y< 0) { _x= -_x; _y= -_y; _o += 4; } \ 188 | if(_x<= 0) { _t= _x; _x= _y; _y= -_t; _o += 2; } \ 189 | if(_x<=_y) { _t= _y-_x; _x= _x+_y; _y= _t; _o += 1; } \ 190 | } while(0); 191 | 192 | // +-0.09 deg error 193 | float atan2approx(float y, float x) 194 | { 195 | 196 | if (x == 0) 197 | x = 123e-15f; 198 | float phi = 0; 199 | float dphi; 200 | float t; 201 | 202 | OCTANTIFY(x, y, phi); 203 | 204 | t = (y / x); 205 | // atan function for 0 - 1 interval 206 | dphi = t*( ( M_PI/4 + 0.2447f ) + t *( ( -0.2447f + 0.0663f ) + t*( - 0.0663f)) ); 207 | phi *= M_PI / 4; 208 | dphi = phi + dphi; 209 | if (dphi > (float) M_PI) 210 | dphi -= 2 * M_PI; 211 | return RADTODEG * dphi; 212 | } 213 | 214 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_crc.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_CRC_H 31 | #define __STM32F0XX_CRC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /*!< Includes ----------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup CRC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup CRC_ReverseInputData 52 | * @{ 53 | */ 54 | #define CRC_ReverseInputData_No ((uint32_t)0x00000000) /*!< No reverse operation of Input Data */ 55 | #define CRC_ReverseInputData_8bits CRC_CR_REV_IN_0 /*!< Reverse operation of Input Data on 8 bits */ 56 | #define CRC_ReverseInputData_16bits CRC_CR_REV_IN_1 /*!< Reverse operation of Input Data on 16 bits */ 57 | #define CRC_ReverseInputData_32bits CRC_CR_REV_IN /*!< Reverse operation of Input Data on 32 bits */ 58 | 59 | #define IS_CRC_REVERSE_INPUT_DATA(DATA) (((DATA) == CRC_ReverseInputData_No) || \ 60 | ((DATA) == CRC_ReverseInputData_8bits) || \ 61 | ((DATA) == CRC_ReverseInputData_16bits) || \ 62 | ((DATA) == CRC_ReverseInputData_32bits)) 63 | 64 | /** 65 | * @} 66 | */ 67 | 68 | /** @defgroup CRC_PolynomialSize 69 | * @brief Only applicable for STM32F042 and STM32F072 devices 70 | * @{ 71 | */ 72 | #define CRC_PolSize_7 CRC_CR_POLSIZE /*!< 7-bit polynomial for CRC calculation */ 73 | #define CRC_PolSize_8 CRC_CR_POLSIZE_1 /*!< 8-bit polynomial for CRC calculation */ 74 | #define CRC_PolSize_16 CRC_CR_POLSIZE_0 /*!< 16-bit polynomial for CRC calculation */ 75 | #define CRC_PolSize_32 ((uint32_t)0x00000000)/*!< 32-bit polynomial for CRC calculation */ 76 | 77 | #define IS_CRC_POL_SIZE(SIZE) (((SIZE) == CRC_PolSize_7) || \ 78 | ((SIZE) == CRC_PolSize_8) || \ 79 | ((SIZE) == CRC_PolSize_16) || \ 80 | ((SIZE) == CRC_PolSize_32)) 81 | 82 | /** 83 | * @} 84 | */ 85 | 86 | /* Exported macro ------------------------------------------------------------*/ 87 | /* Exported functions ------------------------------------------------------- */ 88 | /* Configuration of the CRC computation unit **********************************/ 89 | void CRC_DeInit(void); 90 | void CRC_ResetDR(void); 91 | void CRC_PolynomialSizeSelect(uint32_t CRC_PolSize); /*!< Only applicable for STM32F042 and STM32F072 devices */ 92 | void CRC_ReverseInputDataSelect(uint32_t CRC_ReverseInputData); 93 | void CRC_ReverseOutputDataCmd(FunctionalState NewState); 94 | void CRC_SetInitRegister(uint32_t CRC_InitValue); 95 | void CRC_SetPolynomial(uint32_t CRC_Pol); /*!< Only applicable for STM32F042 and STM32F072 devices */ 96 | 97 | /* CRC computation ************************************************************/ 98 | uint32_t CRC_CalcCRC(uint32_t CRC_Data); 99 | uint32_t CRC_CalcCRC16bits(uint16_t CRC_Data); /*!< Only applicable for STM32F042 and STM32F072 devices */ 100 | uint32_t CRC_CalcCRC8bits(uint8_t CRC_Data); /*!< Only applicable for STM32F042 and STM32F072 devices */ 101 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 102 | uint32_t CRC_GetCRC(void); 103 | 104 | /* Independent register (IDR) access (write/read) *****************************/ 105 | void CRC_SetIDRegister(uint8_t CRC_IDValue); 106 | uint8_t CRC_GetIDRegister(void); 107 | 108 | #ifdef __cplusplus 109 | } 110 | #endif 111 | 112 | #endif /* __STM32F0XX_CRC_H */ 113 | 114 | /** 115 | * @} 116 | */ 117 | 118 | /** 119 | * @} 120 | */ 121 | 122 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 123 | -------------------------------------------------------------------------------- /Silverware/src/rgb_led.c: -------------------------------------------------------------------------------- 1 | #include "config.h" 2 | #include "drv_time.h" 3 | #include "util.h" 4 | #include 5 | 6 | extern int lowbatt; 7 | extern int rxmode; 8 | extern int failsafe; 9 | extern int ledcommand; 10 | extern char aux[]; 11 | 12 | 13 | // normal flight rgb colour - LED switch ON 14 | #define RGB_VALUE_INFLIGHT_ON RGB( 255 , 255 , 255 ) 15 | 16 | // normal flight rgb colour - LED switch OFF 17 | #define RGB_VALUE_INFLIGHT_OFF RGB( 0 , 0 , 0 ) 18 | 19 | // colour before bind 20 | #define RGB_VALUE_BEFORE_BIND RGB( 0 , 128 , 128 ) 21 | 22 | // fade from one color to another when changed 23 | #define RGB_FILTER_ENABLE 24 | #define RGB_FILTER_TIME_MICROSECONDS 50e3 25 | 26 | // runs the update once every 16 loop times ( 16 mS ) 27 | #define DOWNSAMPLE 16 28 | 29 | #define RGB_FILTER_TIME FILTERCALC( 1000*DOWNSAMPLE , RGB_FILTER_TIME_MICROSECONDS) 30 | #define RGB( r , g , b ) ( ( ((int)g&0xff)<<16)|( ((int)r&0xff)<<8)|( (int)b&0xff )) 31 | 32 | extern void rgb_send( int data); 33 | 34 | 35 | 36 | #if ( RGB_LED_NUMBER > 0) 37 | 38 | // array with individual led brightnesses 39 | int rgb_led_value[RGB_LED_NUMBER]; 40 | // loop count for downsampling 41 | int rgb_loopcount = 0; 42 | //rgb low pass filter variables 43 | float r_filt, g_filt, b_filt; 44 | 45 | 46 | // sets all leds to a brightness 47 | void rgb_led_set_all( int rgb ) 48 | { 49 | #ifdef RGB_FILTER_ENABLE 50 | // deconstruct the colour into components 51 | int g = rgb>>16; 52 | int r = (rgb&0x0000FF00)>>8; 53 | int b = rgb & 0xff; 54 | 55 | // filter individual colors 56 | lpf( &r_filt, r , RGB_FILTER_TIME); 57 | lpf( &g_filt, g , RGB_FILTER_TIME); 58 | lpf( &b_filt, b , RGB_FILTER_TIME); 59 | 60 | int temp = RGB( r_filt , g_filt , b_filt ); 61 | 62 | for ( int i = 0 ; i < RGB_LED_NUMBER ; i++) 63 | rgb_led_value[i] = temp; 64 | 65 | #else 66 | for ( int i = 0 ; i < RGB_LED_NUMBER ; i++) 67 | rgb_led_value[i] = rgb; 68 | #endif 69 | } 70 | 71 | // set an individual led brightness 72 | void rgb_led_set_one( int led_number , int rgb ) 73 | { 74 | rgb_led_value[led_number] = rgb; 75 | } 76 | 77 | // flashes between 2 colours, duty cycle 1 - 15 78 | void rgb_ledflash( int color1 , int color2 , uint32_t period , int duty ) 79 | { 80 | if ( gettime() % period > (period*duty)>>4 ) 81 | { 82 | rgb_led_set_all( color1 ); 83 | } 84 | else 85 | { 86 | rgb_led_set_all( color2 ); 87 | } 88 | 89 | } 90 | 91 | 92 | 93 | 94 | 95 | // speed of movement 96 | float KR_SPEED = 0.005f * DOWNSAMPLE; 97 | 98 | float kr_position = 0; 99 | int kr_dir = 0; 100 | 101 | // knight rider style led movement 102 | void rgb_knight_rider( void) 103 | { 104 | if ( kr_dir ) 105 | { 106 | kr_position+= KR_SPEED; 107 | if ( kr_position > RGB_LED_NUMBER - 1 ) 108 | kr_dir =!kr_dir; 109 | } 110 | else 111 | { 112 | kr_position-= KR_SPEED; 113 | if ( kr_position < 0 ) 114 | kr_dir =!kr_dir; 115 | } 116 | 117 | // calculate led value 118 | for ( int i = 0 ; i < RGB_LED_NUMBER ; i++) 119 | { 120 | float led_bright = fabsf( (float) i - kr_position); 121 | if ( led_bright > 1.0f) led_bright = 1.0f; 122 | led_bright = 1.0f - led_bright; 123 | 124 | // set a green background as well, 32 brightness 125 | rgb_led_set_one( i , RGB( (led_bright*255.0f) , (32.0f-led_bright*32.0f) , 0) ); 126 | 127 | } 128 | 129 | } 130 | 131 | 132 | // 2 led flasher 133 | void rgb_ledflash_twin( int color1 , int color2 , uint32_t period ) 134 | { 135 | if ( gettime() % period > (period/2) ) 136 | { 137 | for ( int i = 0 ; i < RGB_LED_NUMBER ; i++) 138 | { 139 | if( (i/2)*2 == i ) rgb_led_set_one( i ,color1 ); 140 | else rgb_led_set_one( i ,color2 ); 141 | } 142 | } 143 | else 144 | { 145 | for ( int i = 0 ; i < RGB_LED_NUMBER ; i++) 146 | { 147 | if( (i/2)*2 == i ) rgb_led_set_one( i ,color2 ); 148 | else rgb_led_set_one( i ,color1 ); 149 | } 150 | } 151 | 152 | } 153 | 154 | 155 | // main function 156 | void rgb_led_lvc( void) 157 | { 158 | rgb_loopcount++; 159 | if ( rgb_loopcount > DOWNSAMPLE ) 160 | { 161 | rgb_loopcount = 0; 162 | // led flash logic 163 | if ( lowbatt ) 164 | { 165 | //rgb_led_set_all( RGB( 255 , 0 , 0 ) ); 166 | rgb_ledflash ( RGB( 255 , 0 , 0 ), RGB( 255 , 32 , 0 ) ,500000 , 8); 167 | } 168 | else 169 | { 170 | if ( rxmode == RXMODE_BIND) 171 | { 172 | // bind mode 173 | //rgb_ledflash ( RGB( 0 , 0 , 255 ), RGB( 0 , 128 , 0 ), 1000000, 12); 174 | // rgb_ledflash_twin( RGB( 0 , 0 , 255 ), RGB( 0 , 128 , 0 ), 1000000); 175 | rgb_led_set_all( RGB_VALUE_BEFORE_BIND ); 176 | // rgb_knight_rider(); 177 | } 178 | else 179 | {// non bind 180 | if ( failsafe) 181 | { 182 | // failsafe flash 183 | rgb_ledflash ( RGB( 0 , 128 , 0 ) , RGB( 0 , 0 , 128 ) ,500000, 8); 184 | //rgb_led_set_all( RGB( 0 , 128 , 128 ) ); 185 | } 186 | else 187 | { 188 | /* 189 | #ifdef GESTURES2_ENABLE 190 | // flashing for gestures 191 | if (ledcommand) 192 | { 193 | if (!ledcommandtime) 194 | ledcommandtime = gettime(); 195 | if (gettime() - ledcommandtime > 500000) 196 | { 197 | ledcommand = 0; 198 | ledcommandtime = 0; 199 | } 200 | ledflash(100000, 8); 201 | } 202 | else 203 | 204 | #endif // end gesture led flash 205 | */ 206 | if ( aux[LEDS_ON] ) 207 | 208 | rgb_led_set_all( RGB_VALUE_INFLIGHT_ON ); 209 | else 210 | rgb_led_set_all( RGB_VALUE_INFLIGHT_OFF ); 211 | } 212 | } 213 | 214 | } 215 | 216 | // send data to leds 217 | for (int i = 0 ; i < RGB_LED_NUMBER ; i++) 218 | { 219 | rgb_send( rgb_led_value[i] ); 220 | } 221 | 222 | } 223 | } 224 | 225 | #endif 226 | -------------------------------------------------------------------------------- /Silverware/src/drv_dshot.h: -------------------------------------------------------------------------------- 1 | //set the dshot pins based on defines in hardware.h 2 | 3 | // motor0 4 | #ifdef MOTOR0_PIN_PB1 5 | #define DSHOT_PIN_0 GPIO_Pin_1 6 | #define DSHOT_PORT_0 GPIOB 7 | #endif 8 | 9 | #ifdef MOTOR0_PIN_PB0 10 | #define DSHOT_PIN_0 GPIO_Pin_0 11 | #define DSHOT_PORT_0 GPIOB 12 | #endif 13 | 14 | #ifdef MOTOR0_PIN_PA0 15 | #define DSHOT_PIN_0 GPIO_Pin_0 16 | #define DSHOT_PORT_0 GPIOA 17 | #endif 18 | 19 | #ifdef MOTOR0_PIN_PA1 20 | #define DSHOT_PIN_0 GPIO_Pin_1 21 | #define DSHOT_PORT_0 GPIOA 22 | #endif 23 | 24 | #ifdef MOTOR0_PIN_PA2 25 | #define DSHOT_PIN_0 GPIO_Pin_2 26 | #define DSHOT_PORT_0 GPIOA 27 | #endif 28 | 29 | #ifdef MOTOR0_PIN_PA3 30 | #define DSHOT_PIN_0 GPIO_Pin_3 31 | #define DSHOT_PORT_0 GPIOA 32 | #endif 33 | 34 | #ifdef MOTOR0_PIN_PA4 35 | #define DSHOT_PIN_0 GPIO_Pin_4 36 | #define DSHOT_PORT_0 GPIOA 37 | #endif 38 | 39 | #ifdef MOTOR0_PIN_PA5 40 | #define DSHOT_PIN_0 GPIO_Pin_5 41 | #define DSHOT_PORT_0 GPIOA 42 | #endif 43 | 44 | #ifdef MOTOR0_PIN_PA6 45 | #define DSHOT_PIN_0 GPIO_Pin_6 46 | #define DSHOT_PORT_0 GPIOA 47 | #endif 48 | 49 | #ifdef MOTOR0_PIN_PA7 50 | #define DSHOT_PIN_0 GPIO_Pin_7 51 | #define DSHOT_PORT_0 GPIOA 52 | #endif 53 | 54 | #ifdef MOTOR0_PIN_PA8 55 | #define DSHOT_PIN_0 GPIO_Pin_8 56 | #define DSHOT_PORT_0 GPIOA 57 | #endif 58 | 59 | #ifdef MOTOR0_PIN_PA9 60 | #define DSHOT_PIN_0 GPIO_Pin_9 61 | #define DSHOT_PORT_0 GPIOA 62 | #endif 63 | 64 | #ifdef MOTOR0_PIN_PA10 65 | #define DSHOT_PIN_0 GPIO_Pin_10 66 | #define DSHOT_PORT_0 GPIOA 67 | #endif 68 | 69 | #ifdef MOTOR0_PIN_PA11 70 | #define DSHOT_PIN_0 GPIO_Pin_11 71 | #define DSHOT_PORT_0 GPIOA 72 | #endif 73 | 74 | // Motor1 75 | 76 | #ifdef MOTOR1_PIN_PB1 77 | #define DSHOT_PIN_1 GPIO_Pin_1 78 | #define DSHOT_PORT_1 GPIOB 79 | #endif 80 | 81 | #ifdef MOTOR1_PIN_PB0 82 | #define DSHOT_PIN_1 GPIO_Pin_0 83 | #define DSHOT_PORT_1 GPIOB 84 | #endif 85 | 86 | #ifdef MOTOR1_PIN_PA0 87 | #define DSHOT_PIN_1 GPIO_Pin_0 88 | #define DSHOT_PORT_1 GPIOA 89 | #endif 90 | 91 | #ifdef MOTOR1_PIN_PA1 92 | #define DSHOT_PIN_1 GPIO_Pin_1 93 | #define DSHOT_PORT_1 GPIOA 94 | #endif 95 | 96 | #ifdef MOTOR1_PIN_PA2 97 | #define DSHOT_PIN_1 GPIO_Pin_2 98 | #define DSHOT_PORT_1 GPIOA 99 | #endif 100 | 101 | #ifdef MOTOR1_PIN_PA3 102 | #define DSHOT_PIN_1 GPIO_Pin_3 103 | #define DSHOT_PORT_1 GPIOA 104 | #endif 105 | 106 | #ifdef MOTOR1_PIN_PA4 107 | #define DSHOT_PIN_1 GPIO_Pin_4 108 | #define DSHOT_PORT_1 GPIOA 109 | #endif 110 | 111 | #ifdef MOTOR1_PIN_PA5 112 | #define DSHOT_PIN_1 GPIO_Pin_5 113 | #define DSHOT_PORT_1 GPIOA 114 | #endif 115 | 116 | #ifdef MOTOR1_PIN_PA6 117 | #define DSHOT_PIN_1 GPIO_Pin_6 118 | #define DSHOT_PORT_1 GPIOA 119 | #endif 120 | 121 | #ifdef MOTOR1_PIN_PA7 122 | #define DSHOT_PIN_1 GPIO_Pin_7 123 | #define DSHOT_PORT_1 GPIOA 124 | #endif 125 | 126 | #ifdef MOTOR1_PIN_PA8 127 | #define DSHOT_PIN_1 GPIO_Pin_8 128 | #define DSHOT_PORT_1 GPIOA 129 | #endif 130 | 131 | #ifdef MOTOR1_PIN_PA9 132 | #define DSHOT_PIN_1 GPIO_Pin_9 133 | #define DSHOT_PORT_1 GPIOA 134 | #endif 135 | 136 | #ifdef MOTOR1_PIN_PA10 137 | #define DSHOT_PIN_1 GPIO_Pin_10 138 | #define DSHOT_PORT_1 GPIOA 139 | #endif 140 | 141 | #ifdef MOTOR1_PIN_PA11 142 | #define DSHOT_PIN_1 GPIO_Pin_11 143 | #define DSHOT_PORT_1 GPIOA 144 | #endif 145 | // motor2 146 | 147 | 148 | #ifdef MOTOR2_PIN_PB1 149 | #define DSHOT_PIN_2 GPIO_Pin_1 150 | #define DSHOT_PORT_2 GPIOB 151 | #endif 152 | 153 | #ifdef MOTOR2_PIN_PB0 154 | #define DSHOT_PIN_2 GPIO_Pin_0 155 | #define DSHOT_PORT_2 GPIOB 156 | #endif 157 | 158 | #ifdef MOTOR2_PIN_PA0 159 | #define DSHOT_PIN_2 GPIO_Pin_0 160 | #define DSHOT_PORT_2 GPIOA 161 | #endif 162 | 163 | #ifdef MOTOR2_PIN_PA1 164 | #define DSHOT_PIN_2 GPIO_Pin_1 165 | #define DSHOT_PORT_2 GPIOA 166 | #endif 167 | 168 | #ifdef MOTOR2_PIN_PA2 169 | #define DSHOT_PIN_2 GPIO_Pin_2 170 | #define DSHOT_PORT_2 GPIOA 171 | #endif 172 | 173 | #ifdef MOTOR2_PIN_PA3 174 | #define DSHOT_PIN_2 GPIO_Pin_3 175 | #define DSHOT_PORT_2 GPIOA 176 | #endif 177 | 178 | #ifdef MOTOR2_PIN_PA4 179 | #define DSHOT_PIN_2 GPIO_Pin_4 180 | #define DSHOT_PORT_2 GPIOA 181 | #endif 182 | 183 | #ifdef MOTOR2_PIN_PA5 184 | #define DSHOT_PIN_2 GPIO_Pin_5 185 | #define DSHOT_PORT_2 GPIOA 186 | #endif 187 | 188 | #ifdef MOTOR2_PIN_PA6 189 | #define DSHOT_PIN_2 GPIO_Pin_6 190 | #define DSHOT_PORT_2 GPIOA 191 | #endif 192 | 193 | #ifdef MOTOR2_PIN_PA7 194 | #define DSHOT_PIN_2 GPIO_Pin_7 195 | #define DSHOT_PORT_2 GPIOA 196 | #endif 197 | 198 | #ifdef MOTOR2_PIN_PA8 199 | #define DSHOT_PIN_2 GPIO_Pin_8 200 | #define DSHOT_PORT_2 GPIOA 201 | #endif 202 | 203 | #ifdef MOTOR2_PIN_PA9 204 | #define DSHOT_PIN_2 GPIO_Pin_9 205 | #define DSHOT_PORT_2 GPIOA 206 | #endif 207 | 208 | #ifdef MOTOR2_PIN_PA10 209 | #define DSHOT_PIN_2 GPIO_Pin_10 210 | #define DSHOT_PORT_2 GPIOA 211 | #endif 212 | 213 | #ifdef MOTOR2_PIN_PA11 214 | #define DSHOT_PIN_2 GPIO_Pin_11 215 | #define DSHOT_PORT_2 GPIOA 216 | #endif 217 | 218 | // motor 3 219 | 220 | #ifdef MOTOR3_PIN_PB1 221 | #define DSHOT_PIN_3 GPIO_Pin_1 222 | #define DSHOT_PORT_3 GPIOB 223 | #endif 224 | 225 | #ifdef MOTOR3_PIN_PB0 226 | #define DSHOT_PIN_3 GPIO_Pin_0 227 | #define DSHOT_PORT_3 GPIOB 228 | #endif 229 | 230 | #ifdef MOTOR3_PIN_PA0 231 | #define DSHOT_PIN_3 GPIO_Pin_0 232 | #define DSHOT_PORT_3 GPIOA 233 | #endif 234 | 235 | #ifdef MOTOR3_PIN_PA1 236 | #define DSHOT_PIN_3 GPIO_Pin_1 237 | #define DSHOT_PORT_3 GPIOA 238 | #endif 239 | 240 | #ifdef MOTOR3_PIN_PA2 241 | #define DSHOT_PIN_3 GPIO_Pin_2 242 | #define DSHOT_PORT_3 GPIOA 243 | #endif 244 | 245 | #ifdef MOTOR3_PIN_PA3 246 | #define DSHOT_PIN_3 GPIO_Pin_3 247 | #define DSHOT_PORT_3 GPIOA 248 | #endif 249 | 250 | #ifdef MOTOR3_PIN_PA4 251 | #define DSHOT_PIN_3 GPIO_Pin_4 252 | #define DSHOT_PORT_3 GPIOA 253 | #endif 254 | 255 | #ifdef MOTOR3_PIN_PA5 256 | #define DSHOT_PIN_3 GPIO_Pin_5 257 | #define DSHOT_PORT_3 GPIOA 258 | #endif 259 | 260 | #ifdef MOTOR3_PIN_PA6 261 | #define DSHOT_PIN_3 GPIO_Pin_6 262 | #define DSHOT_PORT_3 GPIOA 263 | #endif 264 | 265 | #ifdef MOTOR3_PIN_PA7 266 | #define DSHOT_PIN_3 GPIO_Pin_7 267 | #define DSHOT_PORT_3 GPIOA 268 | #endif 269 | 270 | #ifdef MOTOR3_PIN_PA8 271 | #define DSHOT_PIN_3 GPIO_Pin_8 272 | #define DSHOT_PORT_3 GPIOA 273 | #endif 274 | 275 | #ifdef MOTOR3_PIN_PA9 276 | #define DSHOT_PIN_3 GPIO_Pin_9 277 | #define DSHOT_PORT_3 GPIOA 278 | #endif 279 | 280 | #ifdef MOTOR3_PIN_PA10 281 | #define DSHOT_PIN_3 GPIO_Pin_10 282 | #define DSHOT_PORT_3 GPIOA 283 | #endif 284 | 285 | #ifdef MOTOR3_PIN_PA11 286 | #define DSHOT_PIN_3 GPIO_Pin_11 287 | #define DSHOT_PORT_3 GPIOA 288 | #endif 289 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/src/stm32f0xx_misc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_misc.c 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file provides all the miscellaneous firmware functions (add-on 8 | * to CMSIS functions). 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 | /* Includes ------------------------------------------------------------------*/ 30 | #include "stm32f0xx_misc.h" 31 | 32 | /** @addtogroup STM32F0xx_StdPeriph_Driver 33 | * @{ 34 | */ 35 | 36 | /** @defgroup MISC 37 | * @brief MISC driver modules 38 | * @{ 39 | */ 40 | 41 | /* Private typedef -----------------------------------------------------------*/ 42 | /* Private define ------------------------------------------------------------*/ 43 | /* Private macro -------------------------------------------------------------*/ 44 | /* Private variables ---------------------------------------------------------*/ 45 | /* Private function prototypes -----------------------------------------------*/ 46 | /* Private functions ---------------------------------------------------------*/ 47 | 48 | /** @defgroup MISC_Private_Functions 49 | * @{ 50 | */ 51 | /** 52 | * 53 | @verbatim 54 | ******************************************************************************* 55 | ##### Interrupts configuration functions ##### 56 | ******************************************************************************* 57 | [..] This section provide functions allowing to configure the NVIC interrupts 58 | (IRQ). The Cortex-M0 exceptions are managed by CMSIS functions. 59 | (#) Enable and Configure the priority of the selected IRQ Channels. 60 | The priority can be 0..3. 61 | 62 | -@- Lower priority values gives higher priority. 63 | -@- Priority Order: 64 | (#@) Lowest priority. 65 | (#@) Lowest hardware priority (IRQn position). 66 | 67 | @endverbatim 68 | */ 69 | 70 | /** 71 | * @brief Initializes the NVIC peripheral according to the specified 72 | * parameters in the NVIC_InitStruct. 73 | * @param NVIC_InitStruct: pointer to a NVIC_InitTypeDef structure that contains 74 | * the configuration information for the specified NVIC peripheral. 75 | * @retval None 76 | */ 77 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct) 78 | { 79 | uint32_t tmppriority = 0x00; 80 | 81 | /* Check the parameters */ 82 | assert_param(IS_FUNCTIONAL_STATE(NVIC_InitStruct->NVIC_IRQChannelCmd)); 83 | assert_param(IS_NVIC_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelPriority)); 84 | 85 | if (NVIC_InitStruct->NVIC_IRQChannelCmd != DISABLE) 86 | { 87 | /* Compute the Corresponding IRQ Priority --------------------------------*/ 88 | tmppriority = NVIC->IP[NVIC_InitStruct->NVIC_IRQChannel >> 0x02]; 89 | tmppriority &= (uint32_t)(~(((uint32_t)0xFF) << ((NVIC_InitStruct->NVIC_IRQChannel & 0x03) * 8))); 90 | tmppriority |= (uint32_t)((((uint32_t)NVIC_InitStruct->NVIC_IRQChannelPriority << 6) & 0xFF) << ((NVIC_InitStruct->NVIC_IRQChannel & 0x03) * 8)); 91 | 92 | NVIC->IP[NVIC_InitStruct->NVIC_IRQChannel >> 0x02] = tmppriority; 93 | 94 | /* Enable the Selected IRQ Channels --------------------------------------*/ 95 | NVIC->ISER[0] = (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); 96 | } 97 | else 98 | { 99 | /* Disable the Selected IRQ Channels -------------------------------------*/ 100 | NVIC->ICER[0] = (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); 101 | } 102 | } 103 | 104 | /** 105 | * @brief Selects the condition for the system to enter low power mode. 106 | * @param LowPowerMode: Specifies the new mode for the system to enter low power mode. 107 | * This parameter can be one of the following values: 108 | * @arg NVIC_LP_SEVONPEND: Low Power SEV on Pend. 109 | * @arg NVIC_LP_SLEEPDEEP: Low Power DEEPSLEEP request. 110 | * @arg NVIC_LP_SLEEPONEXIT: Low Power Sleep on Exit. 111 | * @param NewState: new state of LP condition. 112 | * This parameter can be: ENABLE or DISABLE. 113 | * @retval None 114 | */ 115 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState) 116 | { 117 | /* Check the parameters */ 118 | assert_param(IS_NVIC_LP(LowPowerMode)); 119 | 120 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 121 | 122 | if (NewState != DISABLE) 123 | { 124 | SCB->SCR |= LowPowerMode; 125 | } 126 | else 127 | { 128 | SCB->SCR &= (uint32_t)(~(uint32_t)LowPowerMode); 129 | } 130 | } 131 | 132 | /** 133 | * @brief Configures the SysTick clock source. 134 | * @param SysTick_CLKSource: specifies the SysTick clock source. 135 | * This parameter can be one of the following values: 136 | * @arg SysTick_CLKSource_HCLK_Div8: AHB clock divided by 8 selected as SysTick clock source. 137 | * @arg SysTick_CLKSource_HCLK: AHB clock selected as SysTick clock source. 138 | * @retval None 139 | */ 140 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource) 141 | { 142 | /* Check the parameters */ 143 | assert_param(IS_SYSTICK_CLK_SOURCE(SysTick_CLKSource)); 144 | 145 | if (SysTick_CLKSource == SysTick_CLKSource_HCLK) 146 | { 147 | SysTick->CTRL |= SysTick_CLKSource_HCLK; 148 | } 149 | else 150 | { 151 | SysTick->CTRL &= SysTick_CLKSource_HCLK_Div8; 152 | } 153 | } 154 | 155 | /** 156 | * @} 157 | */ 158 | 159 | /** 160 | * @} 161 | */ 162 | 163 | /** 164 | * @} 165 | */ 166 | 167 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 168 | -------------------------------------------------------------------------------- /Silverware/src/gestures.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "drv_time.h" 4 | #include "gestures.h" 5 | #include "config.h" 6 | 7 | #define STICKMAX 0.5f 8 | #define STICKCENTER 0.25f 9 | 10 | 11 | #define GMACRO_LEFT (rx[0] < - STICKMAX || rx[2] < - STICKMAX) 12 | #define GMACRO_RIGHT (rx[0] > STICKMAX || rx[2] > STICKMAX) 13 | #define GMACRO_XCENTER (fabsf(rx[0]) < STICKCENTER && fabsf(rx[2]) < STICKCENTER ) 14 | 15 | 16 | #define GMACRO_DOWN (rx[1] < - STICKMAX) 17 | #define GMACRO_UP (rx[1] > STICKMAX) 18 | 19 | #define GMACRO_PITCHCENTER (fabsf(rx[1]) < STICKCENTER) 20 | 21 | 22 | #define GESTURE_CENTER 0 23 | #define GESTURE_CENTER_IDLE 12 24 | #define GESTURE_LEFT 1 25 | #define GESTURE_RIGHT 2 26 | #define GESTURE_DOWN 3 27 | #define GESTURE_UP 4 28 | #define GESTURE_OTHER 127 29 | #define GESTURE_LONG 255 30 | 31 | #define GESTURETIME_MIN 100e3 32 | #define GESTURETIME_MAX 500e3 33 | #define GESTURETIME_IDLE 1000e3 34 | 35 | 36 | 37 | 38 | int gesture_start; 39 | int lastgesture; 40 | int setgesture; 41 | static unsigned gesturetime; 42 | 43 | extern int onground; 44 | extern float rx[]; 45 | 46 | 47 | int gestures2() 48 | { 49 | if (onground) 50 | { 51 | if (GMACRO_XCENTER && GMACRO_PITCHCENTER) 52 | { 53 | gesture_start = GESTURE_CENTER; 54 | } 55 | else if (GMACRO_LEFT && GMACRO_PITCHCENTER) 56 | { 57 | gesture_start = GESTURE_LEFT; 58 | } 59 | else if (GMACRO_RIGHT && GMACRO_PITCHCENTER) 60 | { 61 | gesture_start = GESTURE_RIGHT; 62 | } 63 | else if (GMACRO_DOWN && GMACRO_XCENTER) 64 | { 65 | gesture_start = GESTURE_DOWN; 66 | } 67 | else if (GMACRO_UP && GMACRO_XCENTER) 68 | { 69 | gesture_start = GESTURE_UP; 70 | } 71 | else 72 | { 73 | // gesture_start = GESTURE_OTHER; 74 | } 75 | 76 | unsigned long time = gettime(); 77 | 78 | if (gesture_start != lastgesture) 79 | { 80 | gesturetime = time; 81 | } 82 | 83 | 84 | if (time - gesturetime > GESTURETIME_MIN) 85 | { 86 | if ((gesture_start == GESTURE_CENTER) && (time - gesturetime > GESTURETIME_IDLE)) 87 | { 88 | setgesture = GESTURE_CENTER_IDLE; 89 | } 90 | else if (time - gesturetime > GESTURETIME_MAX) 91 | { 92 | if ((gesture_start != GESTURE_OTHER)) 93 | setgesture = GESTURE_LONG; 94 | } 95 | 96 | else 97 | setgesture = gesture_start; 98 | 99 | } 100 | 101 | 102 | lastgesture = gesture_start; 103 | 104 | 105 | 106 | return gesture_sequence(setgesture); 107 | 108 | } 109 | else 110 | { 111 | setgesture = GESTURE_OTHER; 112 | lastgesture = GESTURE_OTHER; 113 | } 114 | 115 | return 0; 116 | } 117 | 118 | #define GSIZE 7 119 | 120 | 121 | uint8_t gbuffer[GSIZE]; 122 | 123 | // L L D 124 | const uint8_t command1[GSIZE] = { 125 | GESTURE_CENTER_IDLE, GESTURE_LEFT, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER 126 | }; 127 | 128 | // R R D 129 | const uint8_t command2[GSIZE] = { 130 | GESTURE_CENTER_IDLE, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER 131 | }; 132 | 133 | // D D D 134 | const uint8_t command3[GSIZE] = { 135 | GESTURE_CENTER_IDLE, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER 136 | }; 137 | 138 | #ifdef PID_GESTURE_TUNING 139 | // U D U - Next PID term 140 | const uint8_t command4[GSIZE] = { 141 | GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_UP, GESTURE_CENTER 142 | }; 143 | 144 | // U D D - Next PID Axis 145 | const uint8_t command5[GSIZE] = { 146 | GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER 147 | }; 148 | 149 | // U D R - Increase value 150 | const uint8_t command6[GSIZE] = { 151 | GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_RIGHT, GESTURE_CENTER 152 | }; 153 | 154 | // U D L - Descrease value 155 | const uint8_t command7[GSIZE] = { 156 | GESTURE_CENTER_IDLE, GESTURE_UP, GESTURE_CENTER, GESTURE_DOWN, GESTURE_CENTER, GESTURE_LEFT, GESTURE_CENTER 157 | }; 158 | #endif 159 | 160 | uint8_t check_command( uint8_t buffer1[] , const uint8_t command[] ) 161 | { 162 | for (int i = 0; i < GSIZE; i++) 163 | { 164 | if( buffer1[i] != command[GSIZE - i - 1]) 165 | return 0; 166 | } 167 | return 1; 168 | } 169 | 170 | int gesture_sequence(int currentgesture) 171 | { 172 | 173 | if (currentgesture != gbuffer[0]) 174 | { // add to queue 175 | 176 | for (int i = GSIZE; i >= 1; i--) 177 | { 178 | gbuffer[i] = gbuffer[i - 1]; 179 | 180 | } 181 | gbuffer[0] = currentgesture; 182 | 183 | 184 | // check commands 185 | 186 | if (check_command ( &gbuffer[0] , &command1[0] ) ) 187 | { 188 | // command 1 189 | 190 | //change buffer so it does not trigger again 191 | gbuffer[1] = GESTURE_OTHER; 192 | return GESTURE_LLD; 193 | } 194 | 195 | 196 | if (check_command ( &gbuffer[0] , &command2[0] )) 197 | { 198 | // command 2 199 | 200 | //change buffer so it does not trigger again 201 | gbuffer[1] = GESTURE_OTHER; 202 | return GESTURE_RRD; 203 | } 204 | 205 | 206 | if (check_command ( &gbuffer[0] , &command3[0] )) 207 | { 208 | // command 3 209 | 210 | //change buffer so it does not trigger again 211 | gbuffer[1] = GESTURE_OTHER; 212 | return GESTURE_DDD; 213 | } 214 | #ifdef PID_GESTURE_TUNING 215 | if (check_command ( &gbuffer[0] , &command4[0] )) 216 | { 217 | // command 4 218 | 219 | //change buffer so it does not trigger again 220 | gbuffer[1] = GESTURE_OTHER; 221 | return GESTURE_UDU; 222 | } 223 | 224 | if (check_command ( &gbuffer[0] , &command5[0] )) 225 | { 226 | // command 5 227 | 228 | //change buffer so it does not trigger again 229 | gbuffer[1] = GESTURE_OTHER; 230 | return GESTURE_UDD; 231 | } 232 | 233 | if (check_command ( &gbuffer[0] , &command6[0] )) 234 | { 235 | // command 6 236 | 237 | //change buffer so it does not trigger again 238 | gbuffer[1] = GESTURE_OTHER; 239 | return GESTURE_UDR; 240 | } 241 | if (check_command ( &gbuffer[0] , &command7[0] )) 242 | { 243 | // command 7 244 | 245 | //change buffer so it does not trigger again 246 | gbuffer[1] = GESTURE_OTHER; 247 | return GESTURE_UDL; 248 | } 249 | #endif 250 | 251 | } 252 | 253 | return GESTURE_NONE; 254 | } 255 | -------------------------------------------------------------------------------- /Silverware/src/rx_h7_protocol.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 stawel 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | #include "binary.h" 26 | #include "drv_spi.h" 27 | 28 | #include "project.h" 29 | #include "xn297.h" 30 | #include "drv_time.h" 31 | #include 32 | #include "config.h" 33 | #include "defines.h" 34 | 35 | #include "rx_bayang.h" 36 | #include "util.h" 37 | 38 | 39 | #ifdef RX_H7_PROTOCOL 40 | 41 | extern float rx[4]; 42 | 43 | extern char aux[AUXNUMBER]; 44 | extern char lastaux[AUXNUMBER]; 45 | extern char auxchange[AUXNUMBER]; 46 | 47 | 48 | #define H7_FLIP_MASK 0x80 // right shoulder (3D flip switch), resets after aileron or elevator has moved and came back to neutral 49 | #define H7_F_S_MASK 0x01 50 | #define H7_FLAG_VIDEO 0x10 51 | 52 | #define PACKET_SIZE 9 // packets have 9-byte payload 53 | #define SKIPCHANNELTIME 28000 54 | 55 | 56 | int failsafe = 0; 57 | int rxdata[PACKET_SIZE]; 58 | int rxmode = 0; 59 | 60 | 61 | void writeregs(const uint8_t data[], uint8_t size) { 62 | 63 | spi_cson(); 64 | for (uint8_t i = 0; i < size; i++) { 65 | spi_sendbyte(data[i]); 66 | } 67 | spi_csoff(); 68 | delay(1000); 69 | } 70 | 71 | static const uint8_t bbcal[6] = { 0x3f, 0x4c, 0x84, 0x6F, 0x9c, 0x20 }; 72 | static const uint8_t rfcal[8] = { 0x3e, 0xc9, 220, 0x80, 0x61, 0xbb, 0xab, 0x9c }; 73 | static const uint8_t demodcal[6] = { 0x39, 0x0b, 0xdf, 0xc4, 0xa7, 0x03 }; 74 | static int rxaddress[5] = {0xcc, 0xcc, 0xcc, 0xcc, 0xcc}; 75 | //{0xb2, 0xbe, 0x00, 0xcc, 0xcc}; 76 | 77 | 78 | 79 | static const uint8_t H7_freq[] = { 80 | 0x02, 0x48, 0x0C, 0x3e, 0x16, 0x34, 0x20, 0x2A, 81 | 0x2A, 0x20, 0x34, 0x16, 0x3e, 0x0c, 0x48, 0x02 82 | }; 83 | 84 | int channeloffset; 85 | int channel; 86 | 87 | 88 | 89 | void rx_init() { 90 | /* 91 | writeregs(bbcal, sizeof(bbcal)); 92 | writeregs(rfcal, sizeof(rfcal)); 93 | writeregs(demodcal, sizeof(demodcal)); 94 | */ 95 | xn_writerxaddress(rxaddress); 96 | 97 | xn_writereg( EN_AA, 0); // aa disabled 98 | xn_writereg( EN_RXADDR, 1); // pipe 0 only 99 | xn_writereg( RF_SETUP, B00000001); // lna high current on ( better performance ) 100 | xn_writereg( RX_PW_P0, PACKET_SIZE); // payload size 101 | xn_writereg( SETUP_RETR, 0); // no retransmissions ( redundant?) 102 | xn_writereg( SETUP_AW, 3); // address size (5 bits) 103 | xn_command( FLUSH_RX); 104 | xn_writereg( RF_CH, 22); // bind channel 105 | xn_writereg(0, B00001111); // power up, crc enabled 106 | 107 | } 108 | 109 | static char checkpacket() { 110 | int status = xn_readreg( 7 ); 111 | if ((status & B00001110) != B00001110) { 112 | // rx fifo not empty 113 | return 2; 114 | } 115 | 116 | return 0; 117 | } 118 | 119 | 120 | uint8_t checksum_offset = 0; 121 | 122 | uint8_t calc_checksum( void) 123 | { 124 | uint8_t result=checksum_offset; 125 | for(uint8_t i=0; i<8; i++) 126 | result += rxdata[i]; 127 | return result & 0xFF; 128 | } 129 | 130 | void nextchannel(void) 131 | { 132 | channel++; 133 | if(channel > 15) channel = 0; 134 | xn_writereg(0x25, H7_freq[channel]+ channeloffset ); // Set channel frequency 135 | } 136 | 137 | 138 | int decode_h7(void) { 139 | if (rxdata[8] != calc_checksum() ) return 0; 140 | 141 | rx[3] = 0.0045000f * (225 - rxdata[0]); 142 | 143 | rx[1] = ( ((int)rxdata[3]) - 112) * 0.00888888f; 144 | 145 | rx[0] = ( ((int)rxdata[2]) - 112) * 0.00888888f; // roll 146 | 147 | rx[2] = (-((int)rxdata[1]) + 112) * 0.00888888f; 148 | 149 | //rxdata[4] L-R: default:32, (63..1) 150 | //rxdata[5] F-B: default:32, (1..63) 151 | //rxdata[6] default:0, 1: F/S, 128: flip 152 | 153 | aux[0] = (rxdata[6] & H7_FLIP_MASK)?1:0; 154 | 155 | aux[1] = (rxdata[6] & H7_FLAG_VIDEO)?1:0; 156 | 157 | aux[2] = (rxdata[6] & H7_F_S_MASK)?1:0; //?? 158 | 159 | 160 | #ifndef DISABLE_EXPO 161 | rx[0] = rcexpo ( rx[0] , EXPO_XY ); 162 | rx[1] = rcexpo ( rx[1] , EXPO_XY ); 163 | rx[2] = rcexpo ( rx[2] , EXPO_YAW ); 164 | #endif 165 | 166 | for ( int i = 0 ; i < AUXNUMBER - 2 ; i++) 167 | { 168 | auxchange[i] = 0; 169 | if ( lastaux[i] != aux[i] ) auxchange[i] = 1; 170 | lastaux[i] = aux[i]; 171 | } 172 | 173 | return 1; 174 | } 175 | 176 | static unsigned long failsafetime; 177 | static unsigned long lastrxtime; 178 | 179 | #ifdef DEBUG 180 | int failcount = 0; 181 | int chan[16]; 182 | #endif 183 | 184 | void checkrx(void) { 185 | if (checkpacket()) 186 | { 187 | unsigned long time = gettime(); 188 | xn_readpayload(rxdata, PACKET_SIZE); 189 | if (rxmode == RXMODE_BIND) { // rx startup , bind mode 190 | if (rxdata[0] == 0x20) { // bind packet received 191 | rxaddress[0] = rxdata[4]; 192 | rxaddress[1] = rxdata[5]; 193 | rxaddress[2] = 0; 194 | rxmode = RXMODE_NORMAL; 195 | xn_writerxaddress(rxaddress); 196 | 197 | channeloffset = (((rxdata[7] & 0xf0)>>4) + (rxdata[7] & 0x0f)) % 8; 198 | xn_command( FLUSH_RX); 199 | nextchannel(); 200 | checksum_offset = rxdata[7]; 201 | } 202 | } else { // normal mode 203 | if ( decode_h7() ) 204 | { 205 | failsafetime = time; 206 | lastrxtime = failsafetime; 207 | failsafe = 0; 208 | #ifdef DEBUG 209 | chan[channel]++; 210 | #endif 211 | nextchannel(); 212 | } 213 | else 214 | { 215 | #ifdef DEBUG 216 | failcount++; 217 | #endif 218 | } 219 | } // end normal rx mode 220 | 221 | } // end packet received 222 | 223 | unsigned long time = gettime(); 224 | 225 | if ( time - lastrxtime > SKIPCHANNELTIME && rxmode != RXMODE_BIND) 226 | { 227 | nextchannel(); 228 | lastrxtime= time; 229 | } 230 | 231 | 232 | if (time - failsafetime > FAILSAFETIME) { // failsafe 233 | failsafe = 1; 234 | rx[0] = 0; 235 | rx[1] = 0; 236 | rx[2] = 0; 237 | rx[3] = 0; 238 | } 239 | } 240 | 241 | // end H7 proto 242 | #endif 243 | 244 | -------------------------------------------------------------------------------- /Silverware/src/binary.h: -------------------------------------------------------------------------------- 1 | 2 | // Binary 8 bit defines 3 | // PUBLIC DOMAIN 4 | // by silverx 5 | 6 | // no guarantees are made for the correctness of this file 7 | 8 | // the original idea comes from the arduino ide 9 | 10 | 11 | // obtained by this code 12 | /* 13 | #include 14 | 15 | using namespace std; 16 | 17 | int main() 18 | { 19 | for ( int i = 0 ; i < 256 ; i++) 20 | { 21 | cout << "#define B"; 22 | for ( int x = 7 ; x >= 0 ; x--) 23 | { 24 | if ( (i>>x)&1UL) cout << "1"; 25 | else cout << "0"; 26 | } 27 | cout << " "; 28 | cout << i; 29 | cout << endl; 30 | } 31 | return 0; 32 | } 33 | */ 34 | 35 | 36 | #define B00000000 0 37 | #define B00000001 1 38 | #define B00000010 2 39 | #define B00000011 3 40 | #define B00000100 4 41 | #define B00000101 5 42 | #define B00000110 6 43 | #define B00000111 7 44 | #define B00001000 8 45 | #define B00001001 9 46 | #define B00001010 10 47 | #define B00001011 11 48 | #define B00001100 12 49 | #define B00001101 13 50 | #define B00001110 14 51 | #define B00001111 15 52 | #define B00010000 16 53 | #define B00010001 17 54 | #define B00010010 18 55 | #define B00010011 19 56 | #define B00010100 20 57 | #define B00010101 21 58 | #define B00010110 22 59 | #define B00010111 23 60 | #define B00011000 24 61 | #define B00011001 25 62 | #define B00011010 26 63 | #define B00011011 27 64 | #define B00011100 28 65 | #define B00011101 29 66 | #define B00011110 30 67 | #define B00011111 31 68 | #define B00100000 32 69 | #define B00100001 33 70 | #define B00100010 34 71 | #define B00100011 35 72 | #define B00100100 36 73 | #define B00100101 37 74 | #define B00100110 38 75 | #define B00100111 39 76 | #define B00101000 40 77 | #define B00101001 41 78 | #define B00101010 42 79 | #define B00101011 43 80 | #define B00101100 44 81 | #define B00101101 45 82 | #define B00101110 46 83 | #define B00101111 47 84 | #define B00110000 48 85 | #define B00110001 49 86 | #define B00110010 50 87 | #define B00110011 51 88 | #define B00110100 52 89 | #define B00110101 53 90 | #define B00110110 54 91 | #define B00110111 55 92 | #define B00111000 56 93 | #define B00111001 57 94 | #define B00111010 58 95 | #define B00111011 59 96 | #define B00111100 60 97 | #define B00111101 61 98 | #define B00111110 62 99 | #define B00111111 63 100 | #define B01000000 64 101 | #define B01000001 65 102 | #define B01000010 66 103 | #define B01000011 67 104 | #define B01000100 68 105 | #define B01000101 69 106 | #define B01000110 70 107 | #define B01000111 71 108 | #define B01001000 72 109 | #define B01001001 73 110 | #define B01001010 74 111 | #define B01001011 75 112 | #define B01001100 76 113 | #define B01001101 77 114 | #define B01001110 78 115 | #define B01001111 79 116 | #define B01010000 80 117 | #define B01010001 81 118 | #define B01010010 82 119 | #define B01010011 83 120 | #define B01010100 84 121 | #define B01010101 85 122 | #define B01010110 86 123 | #define B01010111 87 124 | #define B01011000 88 125 | #define B01011001 89 126 | #define B01011010 90 127 | #define B01011011 91 128 | #define B01011100 92 129 | #define B01011101 93 130 | #define B01011110 94 131 | #define B01011111 95 132 | #define B01100000 96 133 | #define B01100001 97 134 | #define B01100010 98 135 | #define B01100011 99 136 | #define B01100100 100 137 | #define B01100101 101 138 | #define B01100110 102 139 | #define B01100111 103 140 | #define B01101000 104 141 | #define B01101001 105 142 | #define B01101010 106 143 | #define B01101011 107 144 | #define B01101100 108 145 | #define B01101101 109 146 | #define B01101110 110 147 | #define B01101111 111 148 | #define B01110000 112 149 | #define B01110001 113 150 | #define B01110010 114 151 | #define B01110011 115 152 | #define B01110100 116 153 | #define B01110101 117 154 | #define B01110110 118 155 | #define B01110111 119 156 | #define B01111000 120 157 | #define B01111001 121 158 | #define B01111010 122 159 | #define B01111011 123 160 | #define B01111100 124 161 | #define B01111101 125 162 | #define B01111110 126 163 | #define B01111111 127 164 | #define B10000000 128 165 | #define B10000001 129 166 | #define B10000010 130 167 | #define B10000011 131 168 | #define B10000100 132 169 | #define B10000101 133 170 | #define B10000110 134 171 | #define B10000111 135 172 | #define B10001000 136 173 | #define B10001001 137 174 | #define B10001010 138 175 | #define B10001011 139 176 | #define B10001100 140 177 | #define B10001101 141 178 | #define B10001110 142 179 | #define B10001111 143 180 | #define B10010000 144 181 | #define B10010001 145 182 | #define B10010010 146 183 | #define B10010011 147 184 | #define B10010100 148 185 | #define B10010101 149 186 | #define B10010110 150 187 | #define B10010111 151 188 | #define B10011000 152 189 | #define B10011001 153 190 | #define B10011010 154 191 | #define B10011011 155 192 | #define B10011100 156 193 | #define B10011101 157 194 | #define B10011110 158 195 | #define B10011111 159 196 | #define B10100000 160 197 | #define B10100001 161 198 | #define B10100010 162 199 | #define B10100011 163 200 | #define B10100100 164 201 | #define B10100101 165 202 | #define B10100110 166 203 | #define B10100111 167 204 | #define B10101000 168 205 | #define B10101001 169 206 | #define B10101010 170 207 | #define B10101011 171 208 | #define B10101100 172 209 | #define B10101101 173 210 | #define B10101110 174 211 | #define B10101111 175 212 | #define B10110000 176 213 | #define B10110001 177 214 | #define B10110010 178 215 | #define B10110011 179 216 | #define B10110100 180 217 | #define B10110101 181 218 | #define B10110110 182 219 | #define B10110111 183 220 | #define B10111000 184 221 | #define B10111001 185 222 | #define B10111010 186 223 | #define B10111011 187 224 | #define B10111100 188 225 | #define B10111101 189 226 | #define B10111110 190 227 | #define B10111111 191 228 | #define B11000000 192 229 | #define B11000001 193 230 | #define B11000010 194 231 | #define B11000011 195 232 | #define B11000100 196 233 | #define B11000101 197 234 | #define B11000110 198 235 | #define B11000111 199 236 | #define B11001000 200 237 | #define B11001001 201 238 | #define B11001010 202 239 | #define B11001011 203 240 | #define B11001100 204 241 | #define B11001101 205 242 | #define B11001110 206 243 | #define B11001111 207 244 | #define B11010000 208 245 | #define B11010001 209 246 | #define B11010010 210 247 | #define B11010011 211 248 | #define B11010100 212 249 | #define B11010101 213 250 | #define B11010110 214 251 | #define B11010111 215 252 | #define B11011000 216 253 | #define B11011001 217 254 | #define B11011010 218 255 | #define B11011011 219 256 | #define B11011100 220 257 | #define B11011101 221 258 | #define B11011110 222 259 | #define B11011111 223 260 | #define B11100000 224 261 | #define B11100001 225 262 | #define B11100010 226 263 | #define B11100011 227 264 | #define B11100100 228 265 | #define B11100101 229 266 | #define B11100110 230 267 | #define B11100111 231 268 | #define B11101000 232 269 | #define B11101001 233 270 | #define B11101010 234 271 | #define B11101011 235 272 | #define B11101100 236 273 | #define B11101101 237 274 | #define B11101110 238 275 | #define B11101111 239 276 | #define B11110000 240 277 | #define B11110001 241 278 | #define B11110010 242 279 | #define B11110011 243 280 | #define B11110100 244 281 | #define B11110101 245 282 | #define B11110110 246 283 | #define B11110111 247 284 | #define B11111000 248 285 | #define B11111001 249 286 | #define B11111010 250 287 | #define B11111011 251 288 | #define B11111100 252 289 | #define B11111101 253 290 | #define B11111110 254 291 | #define B11111111 255 292 | 293 | 294 | -------------------------------------------------------------------------------- /Libraries/STM32F0xx_StdPeriph_Driver/inc/stm32f0xx_pwr.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f0xx_pwr.h 4 | * @author MCD Application Team 5 | * @version V1.3.0 6 | * @date 16-January-2014 7 | * @brief This file contains all the functions prototypes for the PWR firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2014 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 __STM32F0XX_PWR_H 31 | #define __STM32F0XX_PWR_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f0xx.h" 39 | 40 | /** @addtogroup STM32F0xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup PWR 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | 50 | /* Exported constants --------------------------------------------------------*/ 51 | 52 | /** @defgroup PWR_Exported_Constants 53 | * @{ 54 | */ 55 | 56 | /** @defgroup PWR_PVD_detection_level 57 | * @brief This parameters are only applicable for STM32F051 and STM32F072 devices 58 | * @{ 59 | */ 60 | 61 | #define PWR_PVDLevel_0 PWR_CR_PLS_LEV0 62 | #define PWR_PVDLevel_1 PWR_CR_PLS_LEV1 63 | #define PWR_PVDLevel_2 PWR_CR_PLS_LEV2 64 | #define PWR_PVDLevel_3 PWR_CR_PLS_LEV3 65 | #define PWR_PVDLevel_4 PWR_CR_PLS_LEV4 66 | #define PWR_PVDLevel_5 PWR_CR_PLS_LEV5 67 | #define PWR_PVDLevel_6 PWR_CR_PLS_LEV6 68 | #define PWR_PVDLevel_7 PWR_CR_PLS_LEV7 69 | 70 | #define IS_PWR_PVD_LEVEL(LEVEL) (((LEVEL) == PWR_PVDLevel_0) || ((LEVEL) == PWR_PVDLevel_1)|| \ 71 | ((LEVEL) == PWR_PVDLevel_2) || ((LEVEL) == PWR_PVDLevel_3)|| \ 72 | ((LEVEL) == PWR_PVDLevel_4) || ((LEVEL) == PWR_PVDLevel_5)|| \ 73 | ((LEVEL) == PWR_PVDLevel_6) || ((LEVEL) == PWR_PVDLevel_7)) 74 | /** 75 | * @} 76 | */ 77 | 78 | /** @defgroup PWR_WakeUp_Pins 79 | * @{ 80 | */ 81 | 82 | #define PWR_WakeUpPin_1 PWR_CSR_EWUP1 83 | #define PWR_WakeUpPin_2 PWR_CSR_EWUP2 84 | #define PWR_WakeUpPin_3 PWR_CSR_EWUP3 /*!< only applicable for STM32F072 devices */ 85 | #define PWR_WakeUpPin_4 PWR_CSR_EWUP4 /*!< only applicable for STM32F072 devices */ 86 | #define PWR_WakeUpPin_5 PWR_CSR_EWUP5 /*!< only applicable for STM32F072 devices */ 87 | #define PWR_WakeUpPin_6 PWR_CSR_EWUP6 /*!< only applicable for STM32F072 devices */ 88 | #define PWR_WakeUpPin_7 PWR_CSR_EWUP7 /*!< only applicable for STM32F072 devices */ 89 | #define PWR_WakeUpPin_8 PWR_CSR_EWUP8 /*!< only applicable for STM32F072 devices */ 90 | #define IS_PWR_WAKEUP_PIN(PIN) (((PIN) == PWR_WakeUpPin_1) || ((PIN) == PWR_WakeUpPin_2) || \ 91 | ((PIN) == PWR_WakeUpPin_3) || ((PIN) == PWR_WakeUpPin_4) || \ 92 | ((PIN) == PWR_WakeUpPin_5) || ((PIN) == PWR_WakeUpPin_6) || \ 93 | ((PIN) == PWR_WakeUpPin_7) || ((PIN) == PWR_WakeUpPin_8)) 94 | /** 95 | * @} 96 | */ 97 | 98 | 99 | /** @defgroup PWR_Regulator_state_is_Sleep_STOP_mode 100 | * @{ 101 | */ 102 | 103 | #define PWR_Regulator_ON ((uint32_t)0x00000000) 104 | #define PWR_Regulator_LowPower PWR_CR_LPSDSR 105 | #define IS_PWR_REGULATOR(REGULATOR) (((REGULATOR) == PWR_Regulator_ON) || \ 106 | ((REGULATOR) == PWR_Regulator_LowPower)) 107 | /** 108 | * @} 109 | */ 110 | 111 | /** @defgroup PWR_SLEEP_mode_entry 112 | * @{ 113 | */ 114 | 115 | #define PWR_SLEEPEntry_WFI ((uint8_t)0x01) 116 | #define PWR_SLEEPEntry_WFE ((uint8_t)0x02) 117 | #define IS_PWR_SLEEP_ENTRY(ENTRY) (((ENTRY) == PWR_SLEEPEntry_WFI) || ((ENTRY) == PWR_SLEEPEntry_WFE)) 118 | 119 | /** 120 | * @} 121 | */ 122 | 123 | /** @defgroup PWR_STOP_mode_entry 124 | * @{ 125 | */ 126 | 127 | #define PWR_STOPEntry_WFI ((uint8_t)0x01) 128 | #define PWR_STOPEntry_WFE ((uint8_t)0x02) 129 | #define PWR_STOPEntry_SLEEPONEXIT ((uint8_t)0x03) 130 | #define IS_PWR_STOP_ENTRY(ENTRY) (((ENTRY) == PWR_STOPEntry_WFI) || ((ENTRY) == PWR_STOPEntry_WFE) ||\ 131 | ((ENTRY) == PWR_STOPEntry_SLEEPONEXIT)) 132 | 133 | /** 134 | * @} 135 | */ 136 | 137 | /** @defgroup PWR_Flag 138 | * @{ 139 | */ 140 | 141 | #define PWR_FLAG_WU PWR_CSR_WUF 142 | #define PWR_FLAG_SB PWR_CSR_SBF 143 | #define PWR_FLAG_PVDO PWR_CSR_PVDO /*!< Not applicable for STM32F030 devices */ 144 | #define PWR_FLAG_VREFINTRDY PWR_CSR_VREFINTRDYF 145 | 146 | #define IS_PWR_GET_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB) || \ 147 | ((FLAG) == PWR_FLAG_PVDO) || ((FLAG) == PWR_FLAG_VREFINTRDY)) 148 | 149 | #define IS_PWR_CLEAR_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB)) 150 | /** 151 | * @} 152 | */ 153 | 154 | /** 155 | * @} 156 | */ 157 | 158 | /* Exported macro ------------------------------------------------------------*/ 159 | /* Exported functions ------------------------------------------------------- */ 160 | 161 | /* Function used to set the PWR configuration to the default reset state ******/ 162 | void PWR_DeInit(void); 163 | 164 | /* Backup Domain Access function **********************************************/ 165 | void PWR_BackupAccessCmd(FunctionalState NewState); 166 | 167 | /* PVD configuration functions ************************************************/ 168 | void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel); /*!< only applicable for STM32F051 and STM32F072 devices */ 169 | void PWR_PVDCmd(FunctionalState NewState); /*!< only applicable for STM32F051 and STM32F072 devices */ 170 | 171 | /* WakeUp pins configuration functions ****************************************/ 172 | void PWR_WakeUpPinCmd(uint32_t PWR_WakeUpPin, FunctionalState NewState); 173 | 174 | /* Low Power modes configuration functions ************************************/ 175 | void PWR_EnterSleepMode(uint8_t PWR_SLEEPEntry); 176 | void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry); 177 | void PWR_EnterSTANDBYMode(void); 178 | 179 | /* Flags management functions *************************************************/ 180 | FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG); 181 | void PWR_ClearFlag(uint32_t PWR_FLAG); 182 | 183 | #ifdef __cplusplus 184 | } 185 | #endif 186 | 187 | #endif /* __STM32F0XX_PWR_H */ 188 | 189 | /** 190 | * @} 191 | */ 192 | 193 | /** 194 | * @} 195 | */ 196 | 197 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 198 | -------------------------------------------------------------------------------- /Silverware/src/config.h: -------------------------------------------------------------------------------- 1 | 2 | #include "defines.h" 3 | #include "hardware.h" 4 | 5 | // pids in pid.c 6 | 7 | // rate in deg/sec 8 | // for acro mode 9 | #define MAX_RATE 360.0 10 | #define MAX_RATEYAW 360.0 11 | 12 | // max angle for level mode 13 | #define MAX_ANGLE_HI 55.0f 14 | 15 | #define LOW_RATES_MULTI 0.5f 16 | 17 | // disable inbuilt expo functions 18 | #define DISABLE_EXPO 19 | 20 | // use if your tx has no expo function 21 | // also comment out DISABLE_EXPO to use 22 | // 0.00 to 1.00 , 0 = no exp 23 | // positive = less sensitive near center 24 | #define EXPO_XY 0.65 25 | #define EXPO_YAW 0.5 26 | 27 | 28 | 29 | // battery saver 30 | // do not start software if battery is too low 31 | // flashes 2 times repeatedly at startup 32 | //#define STOP_LOWBATTERY 33 | 34 | // voltage to start warning 35 | // volts 36 | #define VBATTLOW 3.5 37 | 38 | // compensation for battery voltage vs throttle drop 39 | #define VDROP_FACTOR 0.7 40 | // calculate above factor automatically 41 | #define AUTO_VDROP_FACTOR 42 | 43 | // voltage hysteresys 44 | // in volts 45 | #define HYST 0.10 46 | 47 | 48 | 49 | // lower throttle when battery below treshold 50 | //#define LVC_LOWER_THROTTLE 51 | #define LVC_LOWER_THROTTLE_VOLTAGE 3.30 52 | #define LVC_LOWER_THROTTLE_VOLTAGE_RAW 2.70 53 | #define LVC_LOWER_THROTTLE_KP 3.0 54 | 55 | 56 | 57 | // Gyro LPF filter frequency 58 | // gyro filter 0 = 250hz delay 0.97mS 59 | // gyro filter 1 = 184hz delay 2.9mS 60 | // gyro filter 2 = 92hz delay 3.9mS 61 | // gyro filter 3 = 41hz delay 5.9mS (Default) 62 | // gyro filter 4 = 20hz 63 | // gyro filter 5 = 10hz 64 | // gyro filter 6 = 5hz 65 | // gyro filter 7 = 3600hz delay 0.17mS 66 | #define GYRO_LOW_PASS_FILTER 3 67 | 68 | 69 | // software gyro lpf ( iir ) 70 | // set only one below 71 | //#define SOFT_LPF_1ST_023HZ 72 | //#define SOFT_LPF_1ST_043HZ 73 | //#define SOFT_LPF_1ST_100HZ 74 | //#define SOFT_LPF_2ND_043HZ 75 | //#define SOFT_LPF_2ND_088HZ 76 | //#define SOFT_LPF_4TH_088HZ 77 | //#define SOFT_LPF_4TH_160HZ 78 | //#define SOFT_LPF_4TH_250HZ 79 | #define SOFT_LPF_1ST_HZ 100 80 | //#define SOFT_LPF_2ST_HZ 100 81 | //#define SOFT_LPF_NONE 82 | 83 | 84 | 85 | 86 | // switch function selection 87 | 88 | // H8 protocol channels 89 | // CH_FLIP - flip, CH_HEADFREE - headfree, CH_RTH - headingreturn 90 | // CH_EXPERT , CH_INV (inv h101 tx) 91 | // CH_RLL_TRIM , CH_PIT_TRIM - trim buttons pitch, roll 92 | 93 | 94 | // DEVO channels (bayang protocol) 95 | // DEVO_CHAN_5 - DEVO_CHAN_10 96 | 97 | // Multiprotocol can use MULTI_CHAN_5 - MULTI_CHAN_10 (bayang protocol) 98 | 99 | // CH_ON - on always ( all protocols) 100 | // CH_OFF - off always ( all protocols) 101 | 102 | // rates / expert mode 103 | #define RATES CH_EXPERT 104 | 105 | #define LEVELMODE CH_AUX1 106 | 107 | #define STARTFLIP CH_FLIP 108 | 109 | #define LEDS_ON CH_ON 110 | 111 | // switch for fpv / other, requires fet 112 | // comment out to disable 113 | //#define FPV_ON CH_ON 114 | 115 | // aux1 channel starts on if this is defined, otherwise off. 116 | #define AUX1_START_ON 117 | 118 | // improves reception and enables trims if used 119 | // trims are incompatible with DEVO TX when used 120 | //#define USE_STOCK_TX 121 | 122 | // automatically remove center bias ( needs throttle off for 1 second ) 123 | //#define STOCK_TX_AUTOCENTER 124 | 125 | // enable motor filter - select one 126 | // motorfilter1: hanning 3 sample fir filter 127 | // motorfilter2: 1st lpf, 0.2 - 0.6 , 0.6 = less filtering 128 | //#define MOTOR_FILTER 129 | #define MOTOR_FILTER2_ALPHA 0.3 130 | 131 | // clip feedforward attempts to resolve issues that occur near full throttle 132 | //#define CLIP_FF 133 | 134 | // pwm frequency for motor control 135 | // a higher frequency makes the motors more linear 136 | // in Hz 137 | #define PWMFREQ 16000 138 | 139 | // motor curve to use 140 | // the pwm frequency has to be set independently 141 | // 720motors - use 8khz and curve none. 142 | #define MOTOR_CURVE_NONE 143 | //#define MOTOR_CURVE_6MM_490HZ 144 | //#define MOTOR_CURVE_85MM_8KHZ 145 | //#define MOTOR_CURVE_85MM_32KHZ 146 | //#define BOLDCLASH_716MM_8K 147 | //#define BOLDCLASH_716MM_24K 148 | 149 | // a filter which makes throttle feel faster 150 | //#define THROTTLE_TRANSIENT_COMPENSATION 151 | 152 | // lost quad beeps using motors (30 sec timeout) 153 | //#define MOTOR_BEEPS 154 | 155 | // throttle angle compensation in level mode 156 | // comment out to disable 157 | //#define AUTO_THROTTLE 158 | 159 | // enable auto lower throttle near max throttle to keep control 160 | // mix3 works better with brushless 161 | // comment out to disable 162 | //#define MIX_LOWER_THROTTLE 163 | //#define MIX_INCREASE_THROTTLE 164 | 165 | //#define MIX_LOWER_THROTTLE_3 166 | //#define MIX_INCREASE_THROTTLE_3 167 | 168 | // Radio protocol selection 169 | // select only one 170 | //#define RX_CG023_PROTOCOL 171 | //#define RX_H7_PROTOCOL 172 | //#define RX_BAYANG_PROTOCOL 173 | #define RX_BAYANG_PROTOCOL_TELEMETRY 174 | //#define RX_BAYANG_PROTOCOL_BLE_BEACON 175 | //#define RX_BAYANG_BLE_APP 176 | //#define RX_CX10BLUE_PROTOCOL 177 | //#define RX_SBUS 178 | 179 | // 0 - 7 - power for telemetry 180 | #define TX_POWER 4 181 | 182 | 183 | // Flash saving features 184 | //#define DISABLE_FLIP_SEQUENCER 185 | //#define DISABLE_GESTURES2 186 | 187 | // led brightness in-flight ( solid lights only) 188 | // 0- 15 range 189 | #define LED_BRIGHTNESS 15 190 | 191 | 192 | // external buzzer - pins in hardware.h 193 | //#define BUZZER_ENABLE 194 | 195 | 196 | 197 | // Comment out to disable pid tuning gestures 198 | #define PID_GESTURE_TUNING 199 | #define COMBINE_PITCH_ROLL_PID_TUNING 200 | 201 | // flash save method 202 | // flash_save 1: pids + accel calibration 203 | // flash_save 2: accel calibration to option bytes 204 | #define FLASH_SAVE1 205 | //#define FLASH_SAVE2 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | //################################## 215 | // debug / other things 216 | // this should not be usually changed 217 | 218 | 219 | // level mode "manual" trims ( in degrees) 220 | // pitch positive forward 221 | // roll positive right 222 | #define TRIM_PITCH 0.0 223 | #define TRIM_ROLL 0.0 224 | 225 | // disable motors for testing 226 | //#define NOMOTORS 227 | 228 | // throttle direct to motors for thrust measure 229 | // #define MOTORS_TO_THROTTLE 230 | 231 | // loop time in uS 232 | // this affects soft gyro lpf frequency if used 233 | #define LOOPTIME 1000 234 | 235 | // failsafe time in uS 236 | #define FAILSAFETIME 1000000 // one second 237 | 238 | // max rate used by level pid ( limit ) 239 | #define LEVEL_MAX_RATE 360 240 | 241 | // invert yaw pid for hubsan motors 242 | //#define INVERT_YAW_PID 243 | 244 | // debug things ( debug struct and other) 245 | //#define DEBUG 246 | 247 | // rxdebug structure 248 | //#define RXDEBUG 249 | 250 | // enable motors if pitch / roll controls off center (at zero throttle) 251 | // possible values: 0 / 1 252 | // use in acro build only 253 | #define ENABLESTIX 0 254 | #define ENABLESTIX_TRESHOLD 0.3 255 | #define ENABLESTIX_TIMEOUT 1e6 256 | 257 | // overclock to 64Mhz 258 | //#define ENABLE_OVERCLOCK 259 | 260 | 261 | // limit minimum motor output to a value (0.0 - 1.0) 262 | //#define MOTOR_MIN_ENABLE 263 | #define MOTOR_MIN_VALUE 0.05 264 | 265 | 266 | 267 | 268 | 269 | #pragma diag_warning 1035 , 177 , 4017 270 | #pragma diag_error 260 271 | 272 | //--fpmode=fast 273 | 274 | 275 | 276 | 277 | 278 | // define logic - do not change 279 | /////////////// 280 | 281 | // used for pwm calculations 282 | #ifdef ENABLE_OVERCLOCK 283 | #define SYS_CLOCK_FREQ_HZ 64000000 284 | #else 285 | #define SYS_CLOCK_FREQ_HZ 48000000 286 | #endif 287 | 288 | 289 | 290 | #ifdef MOTOR_BEEPS 291 | #ifdef USE_ESC_DRIVER 292 | #warning "MOTOR BEEPS_WORKS WITH BRUSHED MOTORS ONLY" 293 | #endif 294 | #endif 295 | 296 | 297 | 298 | // for the ble beacon to work after in-flight reset 299 | #ifdef RX_BAYANG_PROTOCOL_BLE_BEACON 300 | #undef STOP_LOWBATTERY 301 | #endif 302 | 303 | 304 | // gcc warnings in main.c 305 | -------------------------------------------------------------------------------- /Silverware/src/drv_hw_i2c.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 silverx 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | 26 | 27 | #include "project.h" 28 | #include "drv_hw_i2c.h" 29 | #include "drv_time.h" 30 | #include "config.h" 31 | 32 | #define HW_I2C_ADDRESS SOFTI2C_GYRO_ADDRESS 33 | 34 | // pins for hw i2c , select one only 35 | // select pins PB6 and PB7 36 | // OR select pins PA9 and PA10 37 | //#define HW_I2C_PINS_PB67 38 | //#define HW_I2C_PINS_PA910 39 | 40 | 41 | // digital filter 0 - 15 ( 0 - off ) 42 | #define HW_I2C_DIGITAL_FILTER 15 43 | 44 | 45 | // 100Khz (slow) 46 | //#define HW_I2C_TIMINGREG 0x10805e89 47 | 48 | // 200Khz 49 | //#define HW_I2C_TIMINGREG 0x2060083e 50 | 51 | // 400khz (fast) 52 | //#define HW_I2C_TIMINGREG 0x00901850 53 | 54 | // 1000Khz (fast+) 55 | //#define HW_I2C_TIMINGREG 0x00700818 56 | 57 | // 1000+Khz (overclock i2c) 58 | //#define HW_I2C_TIMINGREG 0x00400615 59 | 60 | // 100Khz (slow) (overclock 64Mhz) 61 | //#define HW_I2C_TIMINGREG 0x10d05880 62 | 63 | // 400khz (fast) (overclock 64Mhz) 64 | //#define HW_I2C_TIMINGREG 0x00c0216c 65 | 66 | // 1000Khz (fast+) (overclock 64Mhz) 67 | //#define HW_I2C_TIMINGREG 0x00900b22 68 | 69 | 70 | #ifdef HW_I2C_SPEED_FAST_OC 71 | // 1000+Khz (overclock i2c) 72 | #define HW_I2C_TIMINGREG 0x00400615 73 | #endif 74 | 75 | #ifdef HW_I2C_SPEED_FAST2 76 | // 1000Khz (fast+) 77 | #define HW_I2C_TIMINGREG 0x00700818 78 | #endif 79 | 80 | #ifdef HW_I2C_SPEED_FAST 81 | // 400khz (fast) 82 | #define HW_I2C_TIMINGREG 0x00901850 83 | #endif 84 | 85 | #ifdef HW_I2C_SPEED_SLOW1 86 | // 200Khz 87 | #define HW_I2C_TIMINGREG 0x2060083e 88 | #endif 89 | 90 | #ifdef HW_I2C_SPEED_SLOW2 91 | // 100Khz (slow) 92 | #define HW_I2C_TIMINGREG 0x10805e89 93 | #endif 94 | 95 | 96 | #ifndef HW_I2C_PINS_PA910 97 | #ifndef HW_I2C_PINS_PB67 98 | #define HW_I2C_PINS_PB67 99 | #endif 100 | #endif 101 | 102 | // default if not set 103 | #ifndef HW_I2C_TIMINGREG 104 | // 400khz (fast) 105 | #define HW_I2C_TIMINGREG 0x00901850 106 | #endif 107 | 108 | extern int liberror; 109 | 110 | void hw_i2c_init( void) 111 | { 112 | 113 | GPIO_InitTypeDef gpioinitI2C1; 114 | 115 | gpioinitI2C1.GPIO_Mode = GPIO_Mode_AF; 116 | gpioinitI2C1.GPIO_OType = GPIO_OType_OD; 117 | gpioinitI2C1.GPIO_PuPd = GPIO_PuPd_UP; 118 | 119 | 120 | #ifdef HW_I2C_PINS_PB67 121 | gpioinitI2C1.GPIO_Pin = GPIO_Pin_6 | GPIO_Pin_7; 122 | GPIO_Init(GPIOB, &gpioinitI2C1); 123 | #endif 124 | 125 | #ifdef HW_I2C_PINS_PA910 126 | gpioinitI2C1.GPIO_Pin = GPIO_Pin_9 | GPIO_Pin_10; 127 | GPIO_Init(GPIOA, &gpioinitI2C1); 128 | #endif 129 | 130 | 131 | 132 | #ifdef HW_I2C_PINS_PB67 133 | GPIO_PinAFConfig(GPIOB, GPIO_PinSource6, GPIO_AF_1); 134 | GPIO_PinAFConfig(GPIOB, GPIO_PinSource7, GPIO_AF_1); 135 | #endif 136 | 137 | #ifdef HW_I2C_PINS_PA910 138 | GPIO_PinAFConfig(GPIOA, GPIO_PinSource9, GPIO_AF_4); 139 | GPIO_PinAFConfig(GPIOA, GPIO_PinSource10, GPIO_AF_4); 140 | #endif 141 | 142 | 143 | RCC_APB1PeriphClockCmd( RCC_APB1Periph_I2C1, ENABLE); 144 | 145 | #ifdef HW_I2C_PINS_PB67 146 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 147 | SYSCFG_I2CFastModePlusConfig(SYSCFG_I2CFastModePlus_PB6 , ENABLE); 148 | SYSCFG_I2CFastModePlusConfig(SYSCFG_I2CFastModePlus_PB7 , ENABLE); 149 | #endif 150 | 151 | #ifdef HW_I2C_PINS_PA910 152 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_SYSCFG, ENABLE); 153 | SYSCFG_I2CFastModePlusConfig(SYSCFG_I2CFastModePlus_PA9 , ENABLE); 154 | SYSCFG_I2CFastModePlusConfig(SYSCFG_I2CFastModePlus_PA10 , ENABLE); 155 | #endif 156 | 157 | RCC_I2CCLKConfig(RCC_I2C1CLK_SYSCLK); 158 | 159 | I2C_InitTypeDef initI2C1; 160 | 161 | // I2C_StructInit(&initI2C1); 162 | 163 | initI2C1.I2C_Timing = HW_I2C_TIMINGREG; 164 | initI2C1.I2C_AnalogFilter = I2C_AnalogFilter_Enable; 165 | initI2C1.I2C_DigitalFilter = HW_I2C_DIGITAL_FILTER ; 166 | initI2C1.I2C_Mode = I2C_Mode_I2C; 167 | initI2C1.I2C_OwnAddress1 = 0xAB; 168 | initI2C1.I2C_Ack = I2C_Ack_Enable; 169 | initI2C1.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; 170 | 171 | I2C_Init(I2C1, &initI2C1); 172 | I2C_Cmd(I2C1, ENABLE); 173 | 174 | } 175 | 176 | //#define I2C_TIMEOUT 50000 177 | //#define I2C_CONDITION i2c_timeout > I2C_TIMEOUT 178 | 179 | #define I2C_CONDITION ((i2c_timeout>>13)) 180 | 181 | int hw_i2c_sendheader( int reg, int bytes) 182 | { 183 | 184 | unsigned int i2c_timeout = 0; 185 | //check i2c ready 186 | while(I2C_GetFlagStatus(I2C1, I2C_FLAG_BUSY) == SET) 187 | { 188 | i2c_timeout++; 189 | if(I2C_CONDITION) 190 | { 191 | liberror++; 192 | return 0; 193 | } 194 | } 195 | 196 | // start transfer 197 | I2C_TransferHandling(I2C1, HW_I2C_ADDRESS<<1, bytes, I2C_SoftEnd_Mode, I2C_Generate_Start_Write); 198 | 199 | //i2c_timeout = 0; 200 | // wait for address to be sent 201 | while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TXIS) == RESET) 202 | { 203 | i2c_timeout++; 204 | if(I2C_CONDITION) 205 | { 206 | liberror++; 207 | return 0; 208 | } 209 | } 210 | 211 | // send next byte (register location) 212 | I2C_SendData(I2C1, (uint8_t)reg); 213 | 214 | //i2c_timeout = 0; 215 | // wait until last data sent 216 | while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TXE) == RESET) 217 | { 218 | i2c_timeout++; 219 | if(I2C_CONDITION) 220 | { 221 | liberror++; 222 | return 0; 223 | } 224 | } 225 | 226 | return 1; 227 | } 228 | 229 | 230 | 231 | void hw_i2c_writereg( int reg ,int data) 232 | { 233 | 234 | unsigned int i2c_timeout = 0; 235 | 236 | // send start + writeaddress + register location, common send+receive 237 | hw_i2c_sendheader( reg,2 ); 238 | // send register value 239 | I2C_SendData(I2C1, (uint8_t) data); 240 | // wait for finish 241 | while(I2C_GetFlagStatus(I2C1, I2C_FLAG_TC) == RESET) 242 | { 243 | i2c_timeout++; 244 | if(I2C_CONDITION) 245 | { 246 | liberror++; 247 | return; 248 | } 249 | } 250 | 251 | // send stop - end transaction 252 | I2C_GenerateSTOP(I2C1, ENABLE); 253 | 254 | 255 | return; 256 | } 257 | 258 | 259 | 260 | int hw_i2c_readdata( int reg, int *data, int size ) 261 | { 262 | 263 | static uint8_t i = 0; 264 | unsigned int i2c_timeout = 0; 265 | 266 | // send start + writeaddress + register location, common send+receive 267 | hw_i2c_sendheader( reg, 1 ); 268 | 269 | //send restart + readaddress 270 | I2C_TransferHandling(I2C1, HW_I2C_ADDRESS<<1 , size, I2C_AutoEnd_Mode, I2C_Generate_Start_Read); 271 | 272 | //wait for data 273 | for(i = 0; i
© COPYRIGHT 2014 STMicroelectronics
18 | * 19 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 20 | * You may not use this file except in compliance with the License. 21 | * You may obtain a copy of the License at: 22 | * 23 | * http://www.st.com/software_license_agreement_liberty_v2 24 | * 25 | * Unless required by applicable law or agreed to in writing, software 26 | * distributed under the License is distributed on an "AS IS" BASIS, 27 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 28 | * See the License for the specific language governing permissions and 29 | * limitations under the License. 30 | * 31 | ****************************************************************************** 32 | */ 33 | 34 | /* Includes ------------------------------------------------------------------*/ 35 | #include "stm32f0xx_dbgmcu.h" 36 | 37 | /** @addtogroup STM32F0xx_StdPeriph_Driver 38 | * @{ 39 | */ 40 | 41 | /** @defgroup DBGMCU 42 | * @brief DBGMCU driver modules 43 | * @{ 44 | */ 45 | 46 | /* Private typedef -----------------------------------------------------------*/ 47 | /* Private define ------------------------------------------------------------*/ 48 | #define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF) 49 | 50 | /* Private macro -------------------------------------------------------------*/ 51 | /* Private variables ---------------------------------------------------------*/ 52 | /* Private function prototypes -----------------------------------------------*/ 53 | /* Private functions ---------------------------------------------------------*/ 54 | 55 | /** @defgroup DBGMCU_Private_Functions 56 | * @{ 57 | */ 58 | 59 | 60 | /** @defgroup DBGMCU_Group1 Device and Revision ID management functions 61 | * @brief Device and Revision ID management functions 62 | * 63 | @verbatim 64 | ============================================================================== 65 | ##### Device and Revision ID management functions ##### 66 | ============================================================================== 67 | 68 | @endverbatim 69 | * @{ 70 | */ 71 | 72 | /** 73 | * @brief Returns the device revision identifier. 74 | * @param None 75 | * @retval Device revision identifier 76 | */ 77 | uint32_t DBGMCU_GetREVID(void) 78 | { 79 | return(DBGMCU->IDCODE >> 16); 80 | } 81 | 82 | /** 83 | * @brief Returns the device identifier. 84 | * @param None 85 | * @retval Device identifier 86 | */ 87 | uint32_t DBGMCU_GetDEVID(void) 88 | { 89 | return(DBGMCU->IDCODE & IDCODE_DEVID_MASK); 90 | } 91 | 92 | /** 93 | * @} 94 | */ 95 | 96 | /** @defgroup DBGMCU_Group2 Peripherals Configuration functions 97 | * @brief Peripherals Configuration 98 | * 99 | @verbatim 100 | ============================================================================== 101 | ##### Peripherals Configuration functions ##### 102 | ============================================================================== 103 | 104 | @endverbatim 105 | * @{ 106 | */ 107 | 108 | /** 109 | * @brief Configures low power mode behavior when the MCU is in Debug mode. 110 | * @param DBGMCU_Periph: specifies the low power mode. 111 | * This parameter can be any combination of the following values: 112 | * @arg DBGMCU_STOP: Keep debugger connection during STOP mode 113 | * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode 114 | * @param NewState: new state of the specified low power mode in Debug mode. 115 | * This parameter can be: ENABLE or DISABLE. 116 | * @retval None 117 | */ 118 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState) 119 | { 120 | /* Check the parameters */ 121 | assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph)); 122 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 123 | 124 | if (NewState != DISABLE) 125 | { 126 | DBGMCU->CR |= DBGMCU_Periph; 127 | } 128 | else 129 | { 130 | DBGMCU->CR &= ~DBGMCU_Periph; 131 | } 132 | } 133 | 134 | 135 | /** 136 | * @brief Configures APB1 peripheral behavior when the MCU is in Debug mode. 137 | * @param DBGMCU_Periph: specifies the APB1 peripheral. 138 | * This parameter can be any combination of the following values: 139 | * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted, 140 | * not applicable for STM32F030 devices 141 | * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted 142 | * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted 143 | * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted, 144 | * applicable only for STM32F072 devices 145 | * @arg DBGMCU_TIM14_STOP: TIM14 counter stopped when Core is halted 146 | * @arg DBGMCU_RTC_STOP: RTC Calendar and Wakeup counter stopped 147 | * when Core is halted. 148 | * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted 149 | * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted 150 | * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped 151 | * when Core is halted 152 | * @arg DBGMCU_CAN1_STOP: Debug CAN1 stopped when Core is halted, 153 | * applicable only for STM32F042 and STM32F072 devices 154 | * @param NewState: new state of the specified APB1 peripheral in Debug mode. 155 | * This parameter can be: ENABLE or DISABLE. 156 | * @retval None 157 | */ 158 | void DBGMCU_APB1PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) 159 | { 160 | /* Check the parameters */ 161 | assert_param(IS_DBGMCU_APB1PERIPH(DBGMCU_Periph)); 162 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 163 | 164 | if (NewState != DISABLE) 165 | { 166 | DBGMCU->APB1FZ |= DBGMCU_Periph; 167 | } 168 | else 169 | { 170 | DBGMCU->APB1FZ &= ~DBGMCU_Periph; 171 | } 172 | } 173 | 174 | /** 175 | * @brief Configures APB2 peripheral behavior when the MCU is in Debug mode. 176 | * @param DBGMCU_Periph: specifies the APB2 peripheral. 177 | * This parameter can be any combination of the following values: 178 | * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted 179 | * @arg DBGMCU_TIM15_STOP: TIM15 counter stopped when Core is halted 180 | * @arg DBGMCU_TIM16_STOP: TIM16 counter stopped when Core is halted 181 | * @arg DBGMCU_TIM17_STOP: TIM17 counter stopped when Core is halted 182 | * @param NewState: new state of the specified APB2 peripheral in Debug mode. 183 | * This parameter can be: ENABLE or DISABLE. 184 | * @retval None 185 | */ 186 | void DBGMCU_APB2PeriphConfig(uint32_t DBGMCU_Periph, FunctionalState NewState) 187 | { 188 | /* Check the parameters */ 189 | assert_param(IS_DBGMCU_APB2PERIPH(DBGMCU_Periph)); 190 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 191 | 192 | if (NewState != DISABLE) 193 | { 194 | DBGMCU->APB2FZ |= DBGMCU_Periph; 195 | } 196 | else 197 | { 198 | DBGMCU->APB2FZ &= ~DBGMCU_Periph; 199 | } 200 | } 201 | 202 | /** 203 | * @} 204 | */ 205 | 206 | /** 207 | * @} 208 | */ 209 | 210 | /** 211 | * @} 212 | */ 213 | 214 | /** 215 | * @} 216 | */ 217 | 218 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 219 | -------------------------------------------------------------------------------- /Silverware/src/rx_cx10blue_protocol.c: -------------------------------------------------------------------------------- 1 | /* 2 | The MIT License (MIT) 3 | 4 | Copyright (c) 2016 silverx 5 | 6 | Permission is hereby granted, free of charge, to any person obtaining a copy 7 | of this software and associated documentation files (the "Software"), to deal 8 | in the Software without restriction, including without limitation the rights 9 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 10 | copies of the Software, and to permit persons to whom the Software is 11 | furnished to do so, subject to the following conditions: 12 | 13 | The above copyright notice and this permission notice shall be included in 14 | all copies or substantial portions of the Software. 15 | 16 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 17 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 18 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 19 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 20 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 21 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 22 | THE SOFTWARE. 23 | */ 24 | 25 | 26 | // cx10 protocol is work in progress 27 | 28 | #include "binary.h" 29 | #include "drv_spi.h" 30 | 31 | #include "project.h" 32 | #include "xn297.h" 33 | #include "drv_time.h" 34 | #include 35 | #include "config.h" 36 | #include "defines.h" 37 | 38 | #include "rx_bayang.h" 39 | 40 | #include "util.h" 41 | 42 | #ifdef RX_CX10BLUE_PROTOCOL 43 | 44 | #define PAYLOAD_LENGHT 19 45 | 46 | extern float rx[4]; 47 | extern char aux[AUXNUMBER]; 48 | extern char lastaux[AUXNUMBER]; 49 | extern char auxchange[AUXNUMBER]; 50 | 51 | int rxmode = 0; 52 | int failsafe = 0; 53 | 54 | void writeregs ( const uint8_t data[] , uint8_t size ) 55 | { 56 | spi_cson(); 57 | for ( uint8_t i = 0 ; i < size ; i++) 58 | { 59 | spi_sendbyte( data[i]); 60 | } 61 | spi_csoff(); 62 | delay(1000); 63 | } 64 | 65 | 66 | uint8_t bbcal[6] = { 0x3f , 0x4c , 0x84 , 0x6F , 0x9c , 0x20 }; 67 | uint8_t rfcal[8] = { 0x3e , 0xc9 , 220 , 0x80 , 0x61 , 0xbb , 0xab , 0x9c }; 68 | uint8_t demodcal[6] = { 0x39 , 0x0b , 0xdf , 0xc4 , 0xa7 , 0x03}; 69 | 70 | void rx_init() 71 | { 72 | /* 73 | writeregs( bbcal , sizeof(bbcal) ); 74 | writeregs( rfcal , sizeof(rfcal) ); 75 | writeregs( demodcal , sizeof(demodcal) ); 76 | */ 77 | int rxaddress[5] = {0xCC,0xCC,0xCC,0xCC,0xCC}; 78 | 79 | xn_writerxaddress( rxaddress); 80 | 81 | xn_writetxaddress( rxaddress); 82 | 83 | xn_writereg( EN_AA , 0 ); // aa disabled 84 | xn_writereg( EN_RXADDR , 1 ); // pipe 0 only 85 | // xn_writereg( RF_SETUP , B00000001); // lna high current on ( better performance ) 86 | xn_writereg( RF_SETUP , B00000111); 87 | xn_writereg( RX_PW_P0 , PAYLOAD_LENGHT ); // payload size 88 | xn_writereg( SETUP_RETR , 0 ); // no retransmissions ( redundant?) 89 | xn_writereg( SETUP_AW , 3 ); // address size (5 bits) 90 | xn_command( FLUSH_RX); 91 | xn_writereg( RF_CH , 2 ); // bind channel 92 | xn_writereg( 0 , B00001111 ); // power up, crc enabled 93 | 94 | 95 | 96 | } 97 | 98 | 99 | static char checkpacket() 100 | { 101 | int status = xn_readreg( 7 ); 102 | 103 | if ( status&(1< 3 ) chan = 0; 172 | xn_writereg(0x25, rfchannel[chan] ); 173 | } 174 | 175 | 176 | 177 | 178 | #ifdef RXDEBUG 179 | struct rxdebug 180 | { 181 | unsigned long packettime; 182 | int failcount; 183 | int packetpersecond; 184 | int channelcount[4]; 185 | } 186 | rxdebug; 187 | int packetrx; 188 | unsigned long lastrxtime; 189 | unsigned long secondtimer; 190 | #warning "RX debug enabled" 191 | 192 | #endif 193 | 194 | 195 | void checkrx( void) 196 | { 197 | int packetreceived = checkpacket(); 198 | int pass = 0; 199 | if ( packetreceived ) 200 | { 201 | if ( rxmode == 0) 202 | { // rx startup , bind mode 203 | xn_readpayload( rxdata , 15); 204 | 205 | if ( rxdata[0] == 0xAA ) 206 | {// bind packet 207 | 208 | unsigned int temp = rxdata[2];//&0x2F; 209 | 210 | rfchannel[0] = ( (uint8_t) rxdata[1] & 0x0F) + 0x03; 211 | rfchannel[1] = ( (uint8_t) rxdata[1] >> 4) + 0x16; 212 | rfchannel[2] = ( (uint8_t) temp & 0x0F) + 0x2D; 213 | rfchannel[3] = ( (uint8_t) temp >> 4); 214 | 215 | rxdata[9] = 1; 216 | for ( int i = 200; i!=0; i--) 217 | { 218 | // sent confirmation to tx 219 | 220 | xn_writereg( 0 , B00001110 ); 221 | delay(130); 222 | 223 | xn_writepayload( rxdata , PAYLOAD_LENGHT ); 224 | /* 225 | int status; 226 | status = 0; 227 | int txcount = 0; 228 | while( !(status&B00100000) && txcount < 0x100 ) 229 | { 230 | status = xn_command(NOP); 231 | delay(10); 232 | txcount++; 233 | } 234 | */ 235 | delay(1000); 236 | xn_writereg( 0 , B00001111 ); 237 | //xn_writereg( STATUS , B00100000 ); 238 | delay(1000); 239 | } 240 | rxmode = RXMODE_NORMAL; 241 | 242 | nextchannel(); 243 | extern unsigned long lastlooptime; 244 | lastlooptime = gettime(); 245 | #ifdef SERIAL 246 | printf( " BIND \n"); 247 | #endif 248 | } 249 | } 250 | else 251 | { // normal rx mode 252 | #ifdef RXDEBUG 253 | rxdebug.packettime = gettime() - lastrxtime; 254 | #endif 255 | 256 | 257 | lastrxtime = gettime(); 258 | xn_readpayload( rxdata , PAYLOAD_LENGHT); 259 | pass = decodepacket(); 260 | 261 | if (pass) 262 | { 263 | 264 | #ifdef RXDEBUG 265 | packetrx++; 266 | rxdebug.channelcount[chan]++; 267 | #endif 268 | failsafetime = lastrxtime; 269 | failsafe = 0; 270 | nextchannel(); 271 | 272 | } 273 | else 274 | { 275 | #ifdef RXDEBUG 276 | rxdebug.failcount++; 277 | #endif 278 | } 279 | 280 | }// end normal rx mode 281 | 282 | }// end packet received 283 | 284 | unsigned long time = gettime(); 285 | 286 | if( time - lastrxtime > 20000 && rxmode != RXMODE_BIND) 287 | {// channel with no reception 288 | lastrxtime = time; 289 | nextchannel(); 290 | 291 | } 292 | if( time - failsafetime > FAILSAFETIME ) 293 | {// failsafe 294 | failsafe = 1; 295 | rx[0] = 0; 296 | rx[1] = 0; 297 | rx[2] = 0; 298 | rx[3] = 0; 299 | } 300 | #ifdef RXDEBUG 301 | if ( gettime() - secondtimer > 1000000) 302 | { 303 | rxdebug.packetpersecond = packetrx; 304 | packetrx = 0; 305 | secondtimer = gettime(); 306 | } 307 | #endif 308 | 309 | } 310 | 311 | // end bayang protocol 312 | #endif 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | --------------------------------------------------------------------------------