├── stateTracker.cpp ├── global.cpp ├── cmsis_boot ├── stm32f10x.h ├── system_stm32f10x.h ├── stm32f10x_conf.h └── startup │ └── startup_stm32f10x_md.c ├── drv ├── comms │ ├── generate.bat │ ├── protocol.proto │ ├── protocol.pb.h │ ├── communicator.hpp │ ├── communicator.cpp │ └── config.proto ├── led │ ├── led.hpp │ └── led.cpp ├── settings │ ├── settings.hpp │ └── settings.cpp ├── mpu6050 │ ├── mpu6050_registers.hpp │ ├── mpu.hpp │ └── mpu.cpp └── vesc │ ├── crc.h │ ├── vesc.hpp │ └── vesc.cpp ├── stm_lib ├── src │ ├── stm32f10x_i2c.c │ ├── stm32f10x_flash.c │ ├── stm32f10x_usart.c │ ├── stm32f10x_crc.c │ ├── stm32f10x_iwdg.c │ ├── misc.c │ └── stm32f10x_exti.c └── inc │ ├── stm32f10x_crc.h │ ├── stm32f10x_iwdg.h │ ├── stm32f10x_exti.h │ └── misc.h ├── PINS_txt.txt ├── io ├── serial.hpp ├── pwm_out.hpp ├── i2c.hpp ├── genericOut.hpp ├── genericOut.cpp ├── usart.hpp ├── pwm_out.cpp ├── i2c.cpp └── usart.cpp ├── guards ├── guard.hpp ├── footpadGuard.hpp ├── angleGuard.hpp └── footpadGuard.cpp ├── ledController.hpp ├── .gitignore ├── arduino.h ├── imu ├── imu.hpp ├── MadgwickAHRS.hpp ├── imu.cpp └── MadgwickAHRS.cpp ├── lpf.cpp ├── pid.hpp ├── syscalls └── syscalls.c ├── utils.h ├── stateTracker.hpp ├── global.h ├── arduino.cpp ├── README.md ├── lpf.hpp ├── balanceController.hpp ├── Makefile.bak ├── link.ld ├── Makefile ├── ledController.cpp ├── boardController.hpp ├── BalancingController.coproj └── main.cpp /stateTracker.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /global.cpp: -------------------------------------------------------------------------------- 1 | #include "global.h" 2 | -------------------------------------------------------------------------------- /cmsis_boot/stm32f10x.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blezalex/WheelBoardMain/HEAD/cmsis_boot/stm32f10x.h -------------------------------------------------------------------------------- /drv/comms/generate.bat: -------------------------------------------------------------------------------- 1 | set prev_dir=%cd% 2 | cd %~dp0 3 | protoc --nanopb_out=. protocol.proto 4 | cd %prev_dir% -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_i2c.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blezalex/WheelBoardMain/HEAD/stm_lib/src/stm32f10x_i2c.c -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_flash.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blezalex/WheelBoardMain/HEAD/stm_lib/src/stm32f10x_flash.c -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_usart.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/blezalex/WheelBoardMain/HEAD/stm_lib/src/stm32f10x_usart.c -------------------------------------------------------------------------------- /PINS_txt.txt: -------------------------------------------------------------------------------- 1 | https://www.readytoflyquads.com/the-flip32 2 | 3 | RC1,RC2 - FOOT PAD2 4 | PWM1 - motor out 5 | RT - bluetooth RX, TX 6 | -------------------------------------------------------------------------------- /drv/led/led.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define LED_COUNT 20 6 | 7 | void led_set_color(int led_idx, uint32_t color); 8 | void led_init(); 9 | -------------------------------------------------------------------------------- /io/serial.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | class SerialStream { 5 | public: 6 | int16_t read() { 7 | return -1; 8 | } 9 | }; 10 | 11 | SerialStream Serial; 12 | -------------------------------------------------------------------------------- /guards/guard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "global.h" 3 | 4 | class Guard { 5 | public: 6 | virtual void Update() { }; 7 | virtual bool CanStart() = 0; 8 | virtual bool MustStop() = 0; 9 | }; 10 | -------------------------------------------------------------------------------- /ledController.hpp: -------------------------------------------------------------------------------- 1 | 2 | 3 | void led_controller_init(); 4 | void led_controller_startup_animation(); 5 | void led_controller_set_state(float speed, float tilt, float battery_level); 6 | void led_controller_update(); 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode/* 2 | BalancingController/Debug/* 3 | BalancingController/BalancingController.elf.xcodeproj/project.pbxproj 4 | BalancingController.comemgui 5 | BalancingController.comarker 6 | BalancingController.cogui 7 | 8 | logs/* 9 | build/* 10 | nanopb/* -------------------------------------------------------------------------------- /io/pwm_out.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "global.h" 4 | 5 | class PwmOut { 6 | public: 7 | PwmOut() {} 8 | 9 | void init(uint16_t val); 10 | 11 | void set(uint16_t val); 12 | uint16_t get(); 13 | 14 | private: 15 | DISALLOW_COPY_AND_ASSIGN(PwmOut); 16 | }; 17 | -------------------------------------------------------------------------------- /io/i2c.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "../cmsis_boot/stm32f10x.h" 3 | 4 | void i2c_init(); 5 | 6 | uint8_t i2c_readReg(uint8_t hwAddr, uint8_t rAddr); 7 | 8 | void i2c_read_reg_to_buf(uint8_t hwAddr, uint8_t rAddr, uint8_t* buf, uint8_t size); 9 | 10 | void i2c_writeReg(uint8_t hwAddr, uint8_t wAddr, uint8_t value); 11 | 12 | void i2c_DmaRead(uint8_t hwAddr, uint8_t rAddr, DMA_Channel_TypeDef* DMAy_Channelx, uint16_t dataNumber); 13 | -------------------------------------------------------------------------------- /drv/settings/settings.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "drv/comms/config.pb.h" 4 | #include "io/usart.hpp" 5 | 6 | bool saveSettingsToFlash(const Config& config); 7 | 8 | bool readSettingsFromFlash(Config* config); 9 | 10 | bool readSettingsFromBuffer(Config* config, const uint8_t* data, uint32_t data_len); 11 | 12 | int32_t saveProtoToBuffer(uint8_t* buffer, int16_t max_size, const pb_msgdesc_t* fields, const void *src_struct, Usart* log = nullptr); 13 | -------------------------------------------------------------------------------- /arduino.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define min(a,b) ((a)<(b)?(a):(b)) 6 | #define max(a,b) ((a)>(b)?(a):(b)) 7 | #define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt))) 8 | 9 | 10 | void initArduino(); 11 | 12 | uint16_t half_millis(); 13 | 14 | uint32_t millis32(); 15 | 16 | void delay(uint16_t time); 17 | 18 | inline float fmap(float x, float in_min, float in_max, float out_min, float out_max) { 19 | return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min; 20 | } 21 | -------------------------------------------------------------------------------- /io/genericOut.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include 4 | #include "global.h" 5 | 6 | class GenericOut { 7 | public: 8 | GenericOut(uint32_t RCC_APB2Periph, GPIO_TypeDef* port, uint16_t pin, bool inverted); 9 | void init(bool open_drain = false); 10 | bool getState(); 11 | void setState(bool enabled); 12 | void toggle(); 13 | 14 | private: 15 | uint32_t RCC_APB2Periph_; 16 | GPIO_TypeDef* port_; 17 | uint16_t pin_; 18 | bool inverted_; 19 | 20 | DISALLOW_COPY_AND_ASSIGN(GenericOut); 21 | }; 22 | -------------------------------------------------------------------------------- /guards/footpadGuard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "../global.h" 4 | #include "guard.hpp" 5 | #include "lpf.hpp" 6 | #include "drv/comms/config.pb.h" 7 | 8 | class FootpadGuard : public Guard { 9 | public: 10 | FootpadGuard(const Config_FootPadSettings* settings); 11 | 12 | bool CanStart(); 13 | bool MustStop(); 14 | 15 | void Update(); 16 | 17 | uint16_t getLevel(int i) { 18 | return (uint16_t)padLevelFilter[i].getVal(); 19 | } 20 | 21 | private: 22 | LPF padLevelFilter[2]; 23 | 24 | const Config_FootPadSettings* settings_; 25 | uint16_t stop_request_timestamp_ = 0; 26 | bool stop_requested_ = false; 27 | bool seen_booth_off = false; 28 | 29 | DISALLOW_COPY_AND_ASSIGN(FootpadGuard); 30 | }; 31 | 32 | -------------------------------------------------------------------------------- /drv/comms/protocol.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | // CMD format: 4 | // Byte 0: Fixed 0x1 protocol header/version 5 | // Byte 1: packet length 3-255 (inclues all transfered bytes: header, len, 6 | // msgId, msgBody and CRC16) 7 | // Byte 2: message id 8 | // Byte 3-n message body 9 | // Byte n-1 CRC16 10 | 11 | enum RequestId { 12 | MSG_NONE = 0; 13 | READ_CONFIG = 1; 14 | WRITE_CONFIG = 2; 15 | GET_STATS = 3; 16 | CALLIBRATE_ACC = 4; 17 | SAVE_CONFIG = 5; 18 | GET_DEBUG_BUFFER = 6; 19 | SET_DEBUG_STREAM_ID = 7; 20 | TOGGLE_PASSTHROUGH = 8; 21 | GET_CONFIG_DESCRIPTOR = 9; 22 | } 23 | 24 | enum ReplyId { 25 | NO_REPLY = 0; 26 | GENERIC_OK = 1; 27 | GENERIC_FAIL = 2; 28 | STATS = 3; 29 | CONFIG = 4; 30 | CRC_MISMATCH = 5; 31 | DEBUG_BUFFER = 6; 32 | CONFIG_DESCRIPTOR = 7; 33 | } -------------------------------------------------------------------------------- /imu/imu.hpp: -------------------------------------------------------------------------------- 1 | 2 | #ifndef IMU_H 3 | #define IMU_H 4 | 5 | #include "Arduino.h" 6 | #include "global.h" 7 | #include "drv/mpu6050/mpu.hpp" 8 | #include "MadgwickAHRS.hpp" 9 | #include "drv/comms/config.pb.h" 10 | 11 | #define MADGWICK 12 | 13 | class IMU { 14 | public: 15 | IMU(const Config* config) 16 | #ifndef MADGWICK 17 | : accCompensatedVector_{ 0, 0, ACC_1G }, config_(config) { 18 | #else 19 | : config_(config) { 20 | mw_.begin(1000); 21 | #endif 22 | } 23 | void compute(const MpuUpdate& update, bool init = false); 24 | 25 | // deg 26 | volatile float angles[2]; 27 | // deg/sec 28 | volatile float rates[3]; 29 | private: 30 | 31 | #ifdef MADGWICK 32 | Madgwick mw_; 33 | #else 34 | float accCompensatedVector_[3]; 35 | #endif 36 | 37 | const Config* config_; 38 | DISALLOW_COPY_AND_ASSIGN(IMU); 39 | }; 40 | 41 | #endif 42 | -------------------------------------------------------------------------------- /guards/angleGuard.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include "global.h" 4 | #include "guard.hpp" 5 | #include "imu/imu.hpp" 6 | 7 | class AngleGuard : public Guard { 8 | public: 9 | AngleGuard(const IMU& imu, const Config_BalancingConfig* balance_settings) 10 | : imu_(imu), balance_settings_(balance_settings) { 11 | } 12 | 13 | bool CanStart() { 14 | return abs(imu_.angles[ANGLE_STEER]) < balance_settings_->max_start_angle_steer && abs(imu_.angles[ANGLE_DRIVE]) < START_ANGLE_DRIVE; 15 | } 16 | 17 | bool MustStop() { 18 | return abs(imu_.angles[ANGLE_DRIVE]) > balance_settings_->shutoff_angle_drive // back/forward tilt too high (accelerate/stop angle) 19 | || abs(imu_.angles[ANGLE_STEER]) > balance_settings_->shutoff_angle_steer; // left/right tilt is too high 20 | } 21 | 22 | private: 23 | const IMU& imu_; 24 | const Config_BalancingConfig* balance_settings_; 25 | 26 | DISALLOW_COPY_AND_ASSIGN(AngleGuard); 27 | }; 28 | -------------------------------------------------------------------------------- /drv/mpu6050/mpu6050_registers.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define MPU6050_PWR_MGMT_1 0x6B 4 | #define MPU6050_CONFIG 0x1A 5 | #define MPU6050_GYRO_CONFIG 0x1B 6 | #define MPU6050_ACCEL_CONFIG 0x1C 7 | #define MPU6050_SMPLRT_DIV 0x19 8 | #define MPU6050_INT_PIN_CFG 0x37 9 | #define MPU6050_INT_ENABLE 0x38 10 | #define MPU6050_ACCEL_XOUT_H 0x3Bu 11 | 12 | #define MPU6050_CLOCK_INTERNAL 0x00 13 | #define MPU6050_CLOCK_PLL_XGYRO 0x01 14 | #define MPU6050_CLOCK_PLL_YGYRO 0x02 15 | #define MPU6050_CLOCK_PLL_ZGYRO 0x03 16 | #define MPU6050_CLOCK_PLL_EXT32K 0x04 17 | #define MPU6050_CLOCK_PLL_EXT19M 0x05 18 | #define MPU6050_CLOCK_KEEP_RESET 0x07 19 | 20 | #define MPU6050_GYRO_FS_250 0x00<<3 21 | #define MPU6050_GYRO_FS_500 0x01<<3 22 | #define MPU6050_GYRO_FS_1000 0x02<<3 23 | #define MPU6050_GYRO_FS_2000 0x03<<3 24 | 25 | #define MPU6050_ACCEL_FS_2 0x00<<3 26 | #define MPU6050_ACCEL_FS_4 0x01<<3 27 | #define MPU6050_ACCEL_FS_8 0x02<<3 28 | #define MPU6050_ACCEL_FS_16 0x03<<3 29 | -------------------------------------------------------------------------------- /io/genericOut.cpp: -------------------------------------------------------------------------------- 1 | #include "genericOut.hpp" 2 | #include "stm_lib/inc/stm32f10x_rcc.h" 3 | #include "global.h" 4 | 5 | 6 | GenericOut::GenericOut(uint32_t RCC_APB2Periph, GPIO_TypeDef* port, uint16_t pin, bool inverted) 7 | : RCC_APB2Periph_(RCC_APB2Periph), port_(port), pin_(pin), inverted_(inverted) { 8 | 9 | } 10 | 11 | void GenericOut::init(bool open_drain) { 12 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_, ENABLE); 13 | 14 | setState(0); 15 | 16 | /* GPIO configuration */ 17 | GPIO_InitTypeDef GPIO_InitStructure; 18 | GPIO_InitStructure.GPIO_Pin = pin_; 19 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 20 | GPIO_InitStructure.GPIO_Mode = open_drain ? GPIO_Mode_Out_OD : GPIO_Mode_Out_PP; 21 | GPIO_Init(port_, &GPIO_InitStructure); 22 | } 23 | 24 | void GenericOut::setState(bool enabled) { 25 | if (enabled ^ inverted_) { 26 | port_->BSRR = pin_; 27 | } 28 | else { 29 | port_->BRR = pin_; 30 | } 31 | } 32 | 33 | void GenericOut::toggle() { 34 | setState(!getState()); 35 | } 36 | 37 | bool GenericOut::getState() { 38 | return GPIO_ReadOutputDataBit(port_, pin_) ^ inverted_; 39 | } 40 | -------------------------------------------------------------------------------- /io/usart.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | #include "../global.h" 4 | 5 | #define TX_BUFFER_SIZE 512 6 | #define RX_BUFFER_SIZE 256 7 | 8 | class Usart { 9 | public: 10 | Usart(USART_TypeDef * device) : device_(device), rxEmpty(true) { 11 | 12 | } 13 | bool Init(USART_TypeDef * device, uint32_t baud); 14 | int32_t Send(const uint8_t* data, int32_t size); 15 | int32_t Send(const char* data, int32_t size) { return Send((const uint8_t*)data, size); } 16 | 17 | void SendWithWait(const uint8_t* data, int32_t size); 18 | 19 | int32_t TxBufferFreeCapacity(); 20 | int32_t Read(uint8_t* data, int32_t max_size); 21 | 22 | bool HasData(); 23 | void handleIRQ(); 24 | 25 | private: 26 | void SendCurrentByteFromBuffer(); 27 | 28 | private: 29 | USART_TypeDef * device_; 30 | 31 | uint8_t txBuffer[TX_BUFFER_SIZE]; 32 | volatile int32_t txBufferReadIdx = 0; 33 | volatile int32_t txBufferWriteIdx = 0; 34 | volatile bool txStarted = 0; 35 | 36 | uint8_t rxBuffer[RX_BUFFER_SIZE]; 37 | volatile int32_t rxBufferReadIdx = 0; 38 | volatile int32_t rxBufferWriteIdx = 0; 39 | volatile bool rxEmpty = 0; 40 | 41 | DISALLOW_COPY_AND_ASSIGN(Usart); 42 | }; 43 | 44 | extern Usart Serial1; 45 | extern Usart Serial2; 46 | -------------------------------------------------------------------------------- /lpf.cpp: -------------------------------------------------------------------------------- 1 | #include "lpf.hpp" 2 | #include 3 | 4 | #define PI 3.1415926 5 | 6 | // based on http://www.ti.com/lit/an/slaa447/slaa447.pdf 7 | void biquad_butterworth_init(float cutoffFreqHz, float samplingFreqHz, float* filter_params) { 8 | float Q = 0.707; 9 | 10 | float W0 = 2 * PI * cutoffFreqHz / samplingFreqHz; 11 | float cosW0 = cos(W0); 12 | float alpha = sin(W0) / (2 * Q); 13 | 14 | float b0 = (1 - cosW0) / 2; 15 | float b1 = (1 - cosW0); 16 | float b2 = (1 - cosW0) / 2; 17 | 18 | float a0 = 1 + alpha; 19 | float a1 = -2 * cosW0; 20 | float a2 = 1 - alpha; 21 | 22 | // normalize by a0 23 | b0 /= a0; 24 | b1 /= a0; 25 | b2 /= a0; 26 | 27 | a1 /= -a0; 28 | a2 /= -a0; 29 | 30 | filter_params[0] = b0; 31 | filter_params[1] = b1; 32 | filter_params[2] = b2; 33 | 34 | filter_params[3] = a1; 35 | filter_params[4] = a2; 36 | } 37 | 38 | // state xi-1, xi-2, yi-1, yi-2 39 | float biquad_compute(float in, float* params, float* state) { 40 | float result = params[0] * in + params[1] * state[0] + params[2] * state[1] + params[3] * state[2] + params[4] * state[3]; 41 | 42 | // Propagate X state 43 | state[1] = state[0]; 44 | state[0] = in; 45 | 46 | // Propagate Y state 47 | state[3] = state[2]; 48 | state[2] = result; 49 | 50 | return result; 51 | } 52 | -------------------------------------------------------------------------------- /pid.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _PID_H 2 | #define _PID_H 3 | #include "global.h" 4 | #include "drv/comms/config.pb.h" 5 | 6 | class PidController { 7 | public: 8 | PidController(const Config_PidConfig* params) 9 | : _params(params) { 10 | reset(); 11 | } 12 | 13 | float compute(float error) { 14 | float de = error - _prev_error; 15 | _prev_error = error; 16 | 17 | return compute(error, de); 18 | } 19 | 20 | float compute(float error, float de) { 21 | // Cumulative sumI changes slowly, it is OK to use I value from previous iteration here. 22 | float output = error * _params->p + de * _params->d + applyExpoPoly(_sumI,_params->i_expo); 23 | 24 | float ierror = constrain(error, -_params->max_i, _params->max_i); 25 | ierror = error * _params->i; 26 | // Accumulate I unless windup is detected. 27 | if ((output < 1 && output > -1) || 28 | (output > 1 && ierror < 0) || 29 | (output < -1 && ierror > 0)) { 30 | _sumI += ierror; 31 | _sumI = constrain(_sumI, -1, 1); // limit to range 32 | } 33 | 34 | return output; 35 | } 36 | 37 | void reset() { 38 | resetI(); 39 | _prev_error = 0; 40 | } 41 | 42 | void resetI() { 43 | _sumI = 0; 44 | } 45 | 46 | private: 47 | const Config_PidConfig* _params; 48 | float _sumI; 49 | float _prev_error; 50 | 51 | DISALLOW_COPY_AND_ASSIGN(PidController); 52 | }; 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /io/pwm_out.cpp: -------------------------------------------------------------------------------- 1 | #include "pwm_out.hpp" 2 | #include "stm32f10x.h" 3 | #include "stm32f10x_tim.h" 4 | #include "stm32f10x_rcc.h" 5 | #include "stm32f10x_gpio.h" 6 | 7 | void PwmOut::init(uint16_t val) { 8 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); 9 | GPIO_InitTypeDef GPIO_InitStructure; 10 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_8; 11 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 12 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 13 | GPIO_Init(GPIOA, &GPIO_InitStructure); 14 | 15 | 16 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE); 17 | 18 | TIM_TimeBaseInitTypeDef TimerBaseInit; 19 | TIM_TimeBaseStructInit(&TimerBaseInit); 20 | 21 | TimerBaseInit.TIM_Prescaler = SystemCoreClock / 1000000 - 1; // 1us tick ; 22 | TimerBaseInit.TIM_Period = 2500; // 400hz 23 | TimerBaseInit.TIM_CounterMode = TIM_CounterMode_Up; 24 | TimerBaseInit.TIM_ClockDivision = TIM_CKD_DIV1; 25 | TIM_TimeBaseInit(TIM1,&TimerBaseInit); 26 | TIM_Cmd(TIM1, ENABLE); 27 | 28 | TIM_OCInitTypeDef OC_Config; 29 | TIM_OCStructInit(&OC_Config); 30 | OC_Config.TIM_OCMode = TIM_OCMode_PWM1; 31 | OC_Config.TIM_Pulse = val; 32 | OC_Config.TIM_OutputState = TIM_OutputState_Enable; 33 | OC_Config.TIM_OCPolarity = TIM_OCPolarity_High; 34 | TIM_OC1Init(TIM1, &OC_Config); 35 | 36 | TIM_OC1PreloadConfig(TIM1, TIM_OCPreload_Enable); 37 | 38 | TIM_CtrlPWMOutputs(TIM1, ENABLE); 39 | } 40 | 41 | void PwmOut::set(uint16_t val) { 42 | TIM1->CCR1 = val; 43 | } 44 | 45 | uint16_t PwmOut::get() { 46 | return TIM1->CCR1; 47 | } 48 | -------------------------------------------------------------------------------- /drv/comms/protocol.pb.h: -------------------------------------------------------------------------------- 1 | /* Automatically generated nanopb header */ 2 | /* Generated by nanopb-0.4.9-dev */ 3 | 4 | #ifndef PB_DRV_COMMS_PROTOCOL_PB_H_INCLUDED 5 | #define PB_DRV_COMMS_PROTOCOL_PB_H_INCLUDED 6 | #include 7 | 8 | #if PB_PROTO_HEADER_VERSION != 40 9 | #error Regenerate this file with the current version of nanopb generator. 10 | #endif 11 | 12 | /* Enum definitions */ 13 | typedef enum _RequestId { 14 | RequestId_MSG_NONE = 0, 15 | RequestId_READ_CONFIG = 1, 16 | RequestId_WRITE_CONFIG = 2, 17 | RequestId_GET_STATS = 3, 18 | RequestId_CALLIBRATE_ACC = 4, 19 | RequestId_SAVE_CONFIG = 5, 20 | RequestId_GET_DEBUG_BUFFER = 6, 21 | RequestId_SET_DEBUG_STREAM_ID = 7, 22 | RequestId_TOGGLE_PASSTHROUGH = 8, 23 | RequestId_GET_CONFIG_DESCRIPTOR = 9 24 | } RequestId; 25 | 26 | typedef enum _ReplyId { 27 | ReplyId_NO_REPLY = 0, 28 | ReplyId_GENERIC_OK = 1, 29 | ReplyId_GENERIC_FAIL = 2, 30 | ReplyId_STATS = 3, 31 | ReplyId_CONFIG = 4, 32 | ReplyId_CRC_MISMATCH = 5, 33 | ReplyId_DEBUG_BUFFER = 6, 34 | ReplyId_CONFIG_DESCRIPTOR = 7 35 | } ReplyId; 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | /* Helper constants for enums */ 42 | #define _RequestId_MIN RequestId_MSG_NONE 43 | #define _RequestId_MAX RequestId_GET_CONFIG_DESCRIPTOR 44 | #define _RequestId_ARRAYSIZE ((RequestId)(RequestId_GET_CONFIG_DESCRIPTOR+1)) 45 | 46 | #define _ReplyId_MIN ReplyId_NO_REPLY 47 | #define _ReplyId_MAX ReplyId_CONFIG_DESCRIPTOR 48 | #define _ReplyId_ARRAYSIZE ((ReplyId)(ReplyId_CONFIG_DESCRIPTOR+1)) 49 | 50 | 51 | #ifdef __cplusplus 52 | } /* extern "C" */ 53 | #endif 54 | 55 | #endif 56 | -------------------------------------------------------------------------------- /syscalls/syscalls.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************//***** 2 | * @file stdio.c 3 | * @brief Implementation of newlib syscall 4 | ********************************************************************************/ 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #undef errno 12 | extern int errno; 13 | extern int _end; 14 | 15 | __attribute__ ((used)) 16 | caddr_t _sbrk ( int incr ) 17 | { 18 | static unsigned char *heap = NULL; 19 | unsigned char *prev_heap; 20 | 21 | if (heap == NULL) { 22 | heap = (unsigned char *)&_end; 23 | } 24 | prev_heap = heap; 25 | 26 | heap += incr; 27 | 28 | return (caddr_t) prev_heap; 29 | } 30 | 31 | __attribute__ ((used)) 32 | int link(char *old, char *new) { 33 | return -1; 34 | } 35 | 36 | __attribute__ ((used)) 37 | int _close(int file) 38 | { 39 | return -1; 40 | } 41 | 42 | __attribute__ ((used)) 43 | int _fstat(int file, struct stat *st) 44 | { 45 | st->st_mode = S_IFCHR; 46 | return 0; 47 | } 48 | 49 | __attribute__ ((used)) 50 | int _isatty(int file) 51 | { 52 | return 1; 53 | } 54 | 55 | __attribute__ ((used)) 56 | int _lseek(int file, int ptr, int dir) 57 | { 58 | return 0; 59 | } 60 | __attribute__ ((used)) 61 | int _read(int file, char *ptr, int len) 62 | { 63 | return 0; 64 | } 65 | __attribute__ ((used)) 66 | int _write(int file, char *ptr, int len) 67 | { 68 | return len; 69 | } 70 | 71 | __attribute__ ((used)) 72 | void abort(void) 73 | { 74 | /* Abort called */ 75 | while(1); 76 | } 77 | 78 | /* --------------------------------- End Of File ------------------------------ */ 79 | -------------------------------------------------------------------------------- /drv/comms/communicator.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #include "../../global.h" 6 | #include "../../stm_lib/inc/stm32f10x_crc.h" 7 | #include "../../io/usart.hpp" 8 | #include "protocol.pb.h" 9 | 10 | #define kHeaderSize 4 // Id + 2 byte msg len, 1 bytes padding 11 | #define kSuffixSize 2 // CRC 12 | #define kMetadataSize (kHeaderSize + kSuffixSize) 13 | #define kMsgTimeoutMs 1000u // 1 second 14 | 15 | class Communicator { 16 | public: 17 | void Init(Usart* comms_channel) { comms_ = comms_channel; } 18 | 19 | void SendMsg(uint8_t msg_id, const uint8_t* data, uint32_t data_len); 20 | 21 | void SendMsg(uint8_t msg_id); 22 | 23 | // returns msg_id 24 | int update(); 25 | 26 | const uint8_t* data() { 27 | return rx_data + kHeaderSize; 28 | } 29 | 30 | const uint32_t data_len() { 31 | return expected_msg_len() - kMetadataSize; 32 | } 33 | 34 | private: 35 | // buffer_pos_ always points to non-written yet memory. bufferpos = 1 means have only one byte with index 0 36 | int32_t buffer_pos_ = 0; 37 | 38 | uint8_t expected_msg_id() { 39 | return rx_data[0]; 40 | } 41 | 42 | uint32_t expected_msg_len() { 43 | return (static_cast(rx_data[2]) << 8) | rx_data[1]; 44 | } 45 | 46 | uint8_t expected_msg_crc_lo() { 47 | return rx_data[expected_msg_len() - 2]; 48 | } 49 | 50 | uint8_t expected_msg_crc_hi() { 51 | return rx_data[expected_msg_len() - 1]; 52 | } 53 | 54 | uint8_t rx_data[512]; 55 | 56 | uint16_t last_uart_data_time_ = 0; 57 | 58 | // previous update processed a message. buffer_pos_ still poits to its end or some bytes past it. move data if past. 59 | bool move_message_ = false; 60 | 61 | Usart* comms_ = nullptr; 62 | }; 63 | -------------------------------------------------------------------------------- /utils.h: -------------------------------------------------------------------------------- 1 | #ifndef IMU_UTILS_H_ 2 | #define IMU_UTILS_H_ 3 | 4 | #include 5 | #include 6 | 7 | inline void ComputeRotationMatrix(float matrix[][3], float roll, float pitch, float yaw) { 8 | float cosR = cos(roll); 9 | float sinR = sin(roll); 10 | 11 | float cosP = cos(pitch); 12 | float sinP = sin(pitch); 13 | 14 | float cosY = cos(yaw); 15 | float sinY = sin(yaw); 16 | 17 | matrix[0][0] = cosP * cosY; 18 | matrix[0][1] = cosP * sinY; 19 | matrix[0][2] = -sinP; 20 | 21 | matrix[1][0] = sinR * sinP * cosY - cosR * sinY ; 22 | matrix[1][1] = cosR * cosY + sinR * sinP * sinY; 23 | matrix[1][2] = sinR * cosP; 24 | 25 | matrix[2][0] = sinR * sinY + cosR * sinP * cosY; 26 | matrix[2][1] = -sinR * cosY + cosR * sinP * sinY; 27 | matrix[2][2] = cosR * cosP; 28 | } 29 | 30 | 31 | 32 | inline void RotateVectorUsingMatrix(float vector[], float matrix[][3]) { 33 | float tmpCopy[3]; 34 | 35 | tmpCopy[0] = vector[0] * matrix[0][0] + vector[1] * matrix[1][0] + vector[2] * matrix[2][0]; 36 | tmpCopy[1] = vector[0] * matrix[0][1] + vector[1] * matrix[1][1] + vector[2] * matrix[2][1]; 37 | tmpCopy[2] = vector[0] * matrix[0][2] + vector[1] * matrix[1][2] + vector[2] * matrix[2][2]; 38 | 39 | for (int i = 0; i < 3; i++) { 40 | vector[i] = tmpCopy[i]; 41 | } 42 | } 43 | 44 | inline void RotateVectorUsingMatrix(int16_t vector[], float matrix[][3]) { 45 | float tmpCopy[3]; 46 | 47 | tmpCopy[0] = vector[0] * matrix[0][0] + vector[1] * matrix[1][0] + vector[2] * matrix[2][0]; 48 | tmpCopy[1] = vector[0] * matrix[0][1] + vector[1] * matrix[1][1] + vector[2] * matrix[2][1]; 49 | tmpCopy[2] = vector[0] * matrix[0][2] + vector[1] * matrix[1][2] + vector[2] * matrix[2][2]; 50 | 51 | for (int i = 0; i < 3; i++) { 52 | vector[i] = tmpCopy[i]; 53 | } 54 | } 55 | 56 | #endif /* IMU_UTILS_H_ */ 57 | -------------------------------------------------------------------------------- /stateTracker.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "global.h" 3 | #include "guards/guard.hpp" 4 | #include "arduino.h" 5 | 6 | enum class State { 7 | Stopped, 8 | FirstIteration, 9 | Starting, 10 | Running 11 | }; 12 | 13 | class StateTracker { 14 | public: 15 | StateTracker(Guard** guards, int guards_count) 16 | : state_(State::Stopped), guards_(guards), guards_count_(guards_count) { } 17 | 18 | State update() { 19 | for (int i = 0; i < guards_count_; i++) { 20 | Guard* g = guards_[i]; 21 | g->Update(); 22 | } 23 | 24 | switch (state_) { 25 | case State::Starting: handleStartingState(); break; 26 | case State::FirstIteration: handleFirstInterationState(); break; 27 | case State::Stopped: handleStoppedState(); break; 28 | } 29 | 30 | if (state_ != State::Stopped) { 31 | for (int i = 0; i < guards_count_; i++) { 32 | if (guards_[i]->MustStop()) { 33 | state_ = State::Stopped; 34 | break; 35 | } 36 | } 37 | } 38 | 39 | return state_; 40 | } 41 | 42 | float start_progress(){ 43 | return start_progress_; 44 | } 45 | 46 | private: 47 | void handleStartingState() { 48 | uint16_t durationSinceStart = (half_millis() - start_timestamp_) / 2; 49 | if (durationSinceStart > START_DURATION) { 50 | state_ = State::Running; 51 | start_progress_ = 1; 52 | } 53 | start_progress_ = durationSinceStart / (float)START_DURATION; 54 | } 55 | 56 | void handleFirstInterationState() { 57 | state_ = State::Starting; 58 | } 59 | 60 | void handleStoppedState() { 61 | for (int i = 0; i < guards_count_; i++) { 62 | if (!guards_[i]->CanStart()) { 63 | return; 64 | } 65 | } 66 | 67 | state_ = State::FirstIteration; 68 | start_timestamp_ = half_millis(); 69 | start_progress_ = 0; 70 | } 71 | 72 | 73 | private: 74 | State state_; 75 | uint16_t start_timestamp_; 76 | float start_progress_; // [0:1] interval 77 | Guard** guards_; 78 | int guards_count_; 79 | 80 | DISALLOW_COPY_AND_ASSIGN(StateTracker); 81 | }; 82 | -------------------------------------------------------------------------------- /global.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | #include "cmsis_boot/stm32f10x.h" 7 | #include "arduino.h" 8 | 9 | //#define REV5 10 | 11 | //***** Apply board orientation transform *** // 12 | // IMU is using right hand coordinate system, it tracks gravity vector and calculates angle between gravity vector and the board frame. 13 | // Normal orientation - X forward, Y right Z down 14 | 15 | //#define BOARD_ROTATION_MACRO BOARD_ROTATION_UPSIDE_DOWN_X 16 | 17 | #define BOARD_ROTATION_UPSIDE_DOWN_X(XYZ) XYZ[1]*=-1; XYZ[2]*=-1; // rotated 180 deg around X axis 18 | 19 | 20 | // ****** start mode settings *********/ 21 | #define START_MAX_POWER 0.7 // 70% 22 | #define START_DURATION 300 // in ms. Time to bring board from tilted to balanced. 23 | 24 | #define START_ANGLE_DRIVE 12 25 | 26 | #define MIN_MOTOR_CMD 990 27 | #define MAX_MOTOR_CMD 2010 28 | #define NEUTRAL_MOTOR_CMD 1500 29 | 30 | // Configure brake current on invalid pulse with 100ms timeout 31 | #define BRAKE_MOTOR_CMD -2 32 | 33 | #define MOTOR_CMD_RANGE (MAX_MOTOR_CMD - NEUTRAL_MOTOR_CMD) 34 | 35 | #define ANGLE_DRIVE 1 36 | #define ANGLE_STEER 0 37 | 38 | 39 | #define DISALLOW_COPY_AND_ASSIGN(TypeName) \ 40 | TypeName(const TypeName&); \ 41 | void operator=(const TypeName&) 42 | 43 | 44 | //inline int sgn(float val) { return (0 < val) - (val < 0); } 45 | inline int sgn(float val) { return val >= 0 ? 1 : -1; } // returns 1 for 0 and up, -1 for less than zero. 46 | 47 | inline float applyExpoReal(float x, float k) { return sgn(x) * powf(fabsf(x), 1+k); } 48 | 49 | constexpr float E = 2.71828; 50 | 51 | inline float applyExpoNatural(float x, float k) { 52 | float absx = fabsf(x); 53 | return sgn(x) * (powf(E, k*absx) - 1) / (powf(E, k) - 1) ; 54 | } 55 | 56 | inline float applyExpoPoly(float x, float k) { 57 | float absx = fabsf(x); 58 | return sgn(x) * absx/(1+k*(1-absx)); 59 | } 60 | 61 | constexpr inline float deg_to_rad(float angle) { 62 | return angle * M_PI / 180; 63 | } 64 | -------------------------------------------------------------------------------- /drv/settings/settings.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "settings.hpp" 3 | #include "drv/comms/config.pb.h" 4 | #include 5 | #include 6 | #include "stm32f10x_flash.h" 7 | 8 | #define CONFIG_SIZE_MAX 512 9 | #define CONFIG_FLASH_PAGE_ADDR (FLASH_BASE + 128*1024 - CONFIG_SIZE_MAX) 10 | 11 | 12 | int32_t saveProtoToBuffer(uint8_t* buffer, int16_t max_size, const pb_msgdesc_t* fields, const void *src_struct, Usart* log ) { 13 | pb_ostream_t stream = pb_ostream_from_buffer(buffer, max_size); 14 | if (!pb_encode(&stream, fields, src_struct)) { 15 | if (log != nullptr) 16 | { 17 | log->Send(PB_GET_ERROR(&stream), strlen(PB_GET_ERROR(&stream))); 18 | } 19 | return -1; 20 | } 21 | 22 | return stream.bytes_written; 23 | } 24 | 25 | uint8_t scratch[CONFIG_SIZE_MAX]; 26 | 27 | bool saveSettingsToFlash(const Config& config) { 28 | int32_t size = saveProtoToBuffer(scratch, sizeof(scratch), Config_fields, &config); 29 | if (size < 0) 30 | return false; 31 | 32 | FLASH_Unlock(); 33 | if (FLASH_ErasePage(CONFIG_FLASH_PAGE_ADDR) != FLASH_Status::FLASH_COMPLETE) 34 | return false; 35 | 36 | if (FLASH_ProgramWord((uint32_t)CONFIG_FLASH_PAGE_ADDR, size) != FLASH_Status::FLASH_COMPLETE) 37 | return false; 38 | 39 | int size_round_up = (size + 3) / 4 * 4; 40 | for (int i = 0; i < size_round_up/ 4; i++) { 41 | // +4 for size block 42 | if (FLASH_ProgramWord((uint32_t)CONFIG_FLASH_PAGE_ADDR + i*4 + 4, ((uint32_t*)scratch)[i]) != FLASH_Status::FLASH_COMPLETE) 43 | return false; 44 | } 45 | 46 | FLASH_Lock(); 47 | return true; 48 | } 49 | 50 | bool readSettingsFromBuffer(Config* config, const uint8_t* data, uint32_t data_len) { 51 | pb_istream_t stream = pb_istream_from_buffer(data, data_len); 52 | 53 | return pb_decode(&stream, Config_fields, config); 54 | } 55 | 56 | bool readSettingsFromFlash(Config* config) { 57 | uint32_t size = *(uint32_t*)CONFIG_FLASH_PAGE_ADDR; 58 | return readSettingsFromBuffer(config, (const uint8_t*)(CONFIG_FLASH_PAGE_ADDR + 4), size); 59 | } 60 | 61 | 62 | -------------------------------------------------------------------------------- /drv/mpu6050/mpu.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | #define MPU6050_INT_Exti EXTI_Line13 6 | 7 | #define MPU6050_LPF_256HZ 0 8 | #define MPU6050_LPF_188HZ 1 9 | #define MPU6050_LPF_98HZ 2 10 | #define MPU6050_LPF_42HZ 3 11 | #define MPU6050_LPF_20HZ 4 12 | #define MPU6050_LPF_10HZ 5 13 | #define MPU6050_LPF_5HZ 6 14 | 15 | void mpuHandleDataReady(); 16 | 17 | struct MpuUpdate { 18 | int16_t gyro[3]; 19 | int16_t acc[3]; 20 | }; 21 | 22 | class UpdateListener { 23 | public: 24 | virtual void processUpdate(const MpuUpdate& update) = 0; 25 | }; 26 | 27 | 28 | #define ACC_1G 2048 29 | 30 | #define GYRO_CALIBRATION_ITERATIONS_REQUIRED 2048 31 | #define GYRO_CALIBRATION_MAX_DIFF 50 32 | 33 | class Mpu { 34 | public: 35 | Mpu() 36 | : listener_(nullptr), 37 | accZeroOffsets_{0,0,0}, 38 | gyroZeroOffsets_{0,0,0} 39 | {} 40 | 41 | void init(uint8_t accLowPassFilterValue); 42 | inline void handleRawData(uint8_t* data); 43 | 44 | void setListener(UpdateListener* listener) { 45 | listener_ = listener; 46 | } 47 | 48 | inline bool calibrationComplete() { 49 | return gyroCalibrationIterationsLeft_ <= 0; 50 | } 51 | 52 | inline bool accCalibrationComplete() { 53 | return accCalibrationIterationsLeft_ <= 0; 54 | } 55 | 56 | void callibrateAcc(); 57 | 58 | void applyAccZeroOffsets(int16_t* offsets_x_y_z) { 59 | for (int i = 0; i < 3; i++) { 60 | accZeroOffsets_[i] = offsets_x_y_z[i]; 61 | } 62 | } 63 | 64 | const int16_t* getAccOffsets() { 65 | return accZeroOffsets_; 66 | } 67 | 68 | private: 69 | inline void handleGyroData(int16_t* gyro, uint8_t* rawData); 70 | inline void handleAccData(int16_t* acc, int16_t* gyro, uint8_t* rawData); 71 | 72 | UpdateListener* listener_; 73 | 74 | int16_t accZeroOffsets_[3]; 75 | 76 | volatile int32_t gyroCalibrationIterationsLeft_; 77 | volatile int32_t accCalibrationIterationsLeft_; 78 | 79 | int32_t gyroCalibrationAccumulator_[3]; 80 | int16_t gyroZeroOffsets_[3]; // calculated internally 81 | }; 82 | 83 | extern Mpu accGyro; 84 | -------------------------------------------------------------------------------- /arduino.cpp: -------------------------------------------------------------------------------- 1 | #include "stm32f10x_tim.h" 2 | #include "stm32f10x_rcc.h" 3 | #include "misc.h" 4 | 5 | 6 | #define MILLIS_TIMER TIM2 7 | #define MILLIS_TIMER_PERIPH RCC_APB1Periph_TIM2 8 | 9 | 10 | static volatile uint32_t millis_time_; 11 | 12 | void initArduino() { 13 | millis_time_ = 0; 14 | 15 | RCC_APB1PeriphClockCmd(MILLIS_TIMER_PERIPH, ENABLE); 16 | 17 | TIM_TimeBaseInitTypeDef TimerBaseInit; 18 | TIM_TimeBaseStructInit(&TimerBaseInit); 19 | 20 | TimerBaseInit.TIM_Prescaler = SystemCoreClock / 2 / 1000 - 1; // 0.5 ms tick ; 21 | 22 | TimerBaseInit.TIM_Period = 0xFFFF; 23 | TimerBaseInit.TIM_CounterMode = TIM_CounterMode_Up; 24 | TimerBaseInit.TIM_ClockDivision = TIM_CKD_DIV2; // HAS NO EFFECT!! 25 | TIM_TimeBaseInit(MILLIS_TIMER,&TimerBaseInit); 26 | 27 | TIM_Cmd(MILLIS_TIMER, ENABLE); 28 | 29 | NVIC_InitTypeDef nvicStructure; 30 | nvicStructure.NVIC_IRQChannel = TIM2_IRQn; 31 | nvicStructure.NVIC_IRQChannelPreemptionPriority = 0; 32 | nvicStructure.NVIC_IRQChannelSubPriority = 0; 33 | nvicStructure.NVIC_IRQChannelCmd = ENABLE; 34 | NVIC_Init(&nvicStructure); 35 | 36 | TIM_ClearITPendingBit(MILLIS_TIMER, TIM_IT_Update); 37 | TIM_ITConfig(MILLIS_TIMER, TIM_IT_Update, ENABLE ); 38 | } 39 | 40 | 41 | extern "C" void TIM2_IRQHandler() { 42 | if (TIM_GetITStatus(MILLIS_TIMER, TIM_IT_Update) != RESET) { 43 | millis_time_++; 44 | TIM_ClearITPendingBit(MILLIS_TIMER, TIM_IT_Update); 45 | } 46 | } 47 | 48 | uint16_t half_millis() { 49 | return TIM2->CNT; 50 | } 51 | 52 | 53 | // Returns uint32 version of millis time. 54 | // Assuming TIM2_IRQHandler never gets preempted. 55 | uint32_t millis32() { 56 | uint16_t enter_cnt; 57 | uint32_t high_bytes; 58 | do { 59 | enter_cnt = TIM2->CNT; 60 | bool it_pending = TIM_GetITStatus(MILLIS_TIMER, TIM_IT_Update) != RESET; 61 | // it_pending may be set to true only if this code is called from an interrupt of the same priority as TIM2_IRQHandler, otherwise this code would have been stopped 62 | // prior to calling it_pending. 63 | high_bytes = millis_time_ + it_pending; 64 | } while (enter_cnt > TIM2->CNT); // overflow occurred during the loop execution. 65 | return ((high_bytes << 16) | enter_cnt) / 2; 66 | } 67 | 68 | void delay(uint16_t time) { 69 | uint16_t start_time = half_millis(); 70 | 71 | while ((uint16_t)(half_millis() - start_time) < time*2); 72 | } 73 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # WheelBoard Controller 2 | Balancing controller for DIY mono wheeled skateboard. 3 | 4 | [![](https://img.youtube.com/vi/-kyArozNmkw/0.jpg)](https://www.youtube.com/watch?v=-kyArozNmkw) 5 | 6 | [![](https://img.youtube.com/vi/4-E7deaGyFs/0.jpg)](https://www.youtube.com/watch?v=4-E7deaGyFs) 7 | 8 | 9 | Supported hardware: 10 | CPU: STM32F103CBT6 11 | [Flip32](http://www.readytoflyquads.com/the-flip32) or Naze32 control boards. 12 | 13 | I run it with: [VESC ESC](http://vedder.se/2015/01/vesc-open-source-esc/). 14 | My vesc config is below 15 | 16 | ## Wiring: 17 | 18 | * PWM1 - main balancing signal. Wire it to VESC PPM input. 19 | * RC_CH1, RC_CH2 - optional input for force sensors. These register pressure on foot pads and allow board to start only when both pads are pressed. 20 | * BUZZER - optional buzzer, lets you know when you reach speed limit. 21 | 22 | #### FootPad sensors 23 | I use 4 (2 on each side) [force sensing resistors](https://www.pololu.com/product/1645). 24 | Connect two on each side in parallel. Then one wire to 3.3v, the other to RC_CH1. Add a pull down 3k resistor from RC_CH1 to ground. (Same for other side, RC_CH2). 25 | 26 | ### Optional: 27 | * USART1 RX, TX - goes to a bluetooth module (NON-BLE, BLE wont work), baud 115200, no parity, 8bits, 1 stop 28 | Phone app https://github.com/blezalex/WheelBoardAndroid 29 | * RC_CH3, RC_CH4 - USART2 RX/TX - goes to VESC RX/TX (gets various stats from vesc) 30 | 31 | ## My Setup: 32 | * Main board Flip32 Rev5 33 | * ESC [VESC](http://diyelectricskateboard.com/diy-electric-skateboard-kits-parts/vesc-the-best-electric-skateboard-esc/) 34 | * [Hub motor with tire 36v 500W 11in](https://www.aliexpress.com/item/11inch-350w-500w-wide-tire-hub-motor-phub-44/32536443206.html) 35 | * 13s 5000mAh lipo 36 | 37 | ## VESC Settings 38 | 39 | * FOC 40 | * All current limits 60A 41 | * APP: USART and PPM 42 | * Control Type: Current 43 | * Median Filter: False 44 | * Safe Start: False 45 | * Positive/Negative ramping time 0.02 46 | 47 | #### Mapping 48 | * Start: 1ms 49 | * End: 2ms 50 | * Center: 1.5010ms 51 | * Input Deadband: 0% 52 | 53 | #### Throttle curve 54 | no expos 55 | 56 | 57 | # Frame build log: http://imgur.com/a/ALTTR 58 | ![current frame](http://i.imgur.com/aFqdWp5.jpg) 59 | 60 | # Warning 61 | Use at your own risk. No guarantees. This software is not bug-free. Self balancing vehicles are inherently unstable. 62 | 63 | -------------------------------------------------------------------------------- /cmsis_boot/system_stm32f10x.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f10x.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief CMSIS Cortex-M3 Device Peripheral Access Layer System Header File. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /** @addtogroup CMSIS 23 | * @{ 24 | */ 25 | 26 | /** @addtogroup stm32f10x_system 27 | * @{ 28 | */ 29 | 30 | /** 31 | * @brief Define to prevent recursive inclusion 32 | */ 33 | #ifndef __SYSTEM_STM32F10X_H 34 | #define __SYSTEM_STM32F10X_H 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | /** @addtogroup STM32F10x_System_Includes 41 | * @{ 42 | */ 43 | 44 | /** 45 | * @} 46 | */ 47 | 48 | 49 | /** @addtogroup STM32F10x_System_Exported_types 50 | * @{ 51 | */ 52 | 53 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /** @addtogroup STM32F10x_System_Exported_Constants 60 | * @{ 61 | */ 62 | 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @addtogroup STM32F10x_System_Exported_Macros 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @addtogroup STM32F10x_System_Exported_Functions 76 | * @{ 77 | */ 78 | 79 | extern void SystemInit(void); 80 | extern void SystemCoreClockUpdate(void); 81 | /** 82 | * @} 83 | */ 84 | 85 | #ifdef __cplusplus 86 | } 87 | #endif 88 | 89 | #endif /*__SYSTEM_STM32F10X_H */ 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 99 | -------------------------------------------------------------------------------- /stm_lib/inc/stm32f10x_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_crc.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_CRC_H 25 | #define __STM32F10x_CRC_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup CRC 39 | * @{ 40 | */ 41 | 42 | /** @defgroup CRC_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup CRC_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** 55 | * @} 56 | */ 57 | 58 | /** @defgroup CRC_Exported_Macros 59 | * @{ 60 | */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup CRC_Exported_Functions 67 | * @{ 68 | */ 69 | 70 | void CRC_ResetDR(void); 71 | uint32_t CRC_CalcCRC(uint32_t Data); 72 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 73 | uint32_t CRC_GetCRC(void); 74 | void CRC_SetIDRegister(uint8_t IDValue); 75 | uint8_t CRC_GetIDRegister(void); 76 | 77 | #ifdef __cplusplus 78 | } 79 | #endif 80 | 81 | #endif /* __STM32F10x_CRC_H */ 82 | /** 83 | * @} 84 | */ 85 | 86 | /** 87 | * @} 88 | */ 89 | 90 | /** 91 | * @} 92 | */ 93 | 94 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 95 | -------------------------------------------------------------------------------- /drv/vesc/crc.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | const uint16_t crc16_table[] = { 5 | 0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50a5, 0x60c6, 0x70e7, 0x8108, 6 | 0x9129, 0xa14a, 0xb16b, 0xc18c, 0xd1ad, 0xe1ce, 0xf1ef, 0x1231, 0x0210, 7 | 0x3273, 0x2252, 0x52b5, 0x4294, 0x72f7, 0x62d6, 0x9339, 0x8318, 0xb37b, 8 | 0xa35a, 0xd3bd, 0xc39c, 0xf3ff, 0xe3de, 0x2462, 0x3443, 0x0420, 0x1401, 9 | 0x64e6, 0x74c7, 0x44a4, 0x5485, 0xa56a, 0xb54b, 0x8528, 0x9509, 0xe5ee, 10 | 0xf5cf, 0xc5ac, 0xd58d, 0x3653, 0x2672, 0x1611, 0x0630, 0x76d7, 0x66f6, 11 | 0x5695, 0x46b4, 0xb75b, 0xa77a, 0x9719, 0x8738, 0xf7df, 0xe7fe, 0xd79d, 12 | 0xc7bc, 0x48c4, 0x58e5, 0x6886, 0x78a7, 0x0840, 0x1861, 0x2802, 0x3823, 13 | 0xc9cc, 0xd9ed, 0xe98e, 0xf9af, 0x8948, 0x9969, 0xa90a, 0xb92b, 0x5af5, 14 | 0x4ad4, 0x7ab7, 0x6a96, 0x1a71, 0x0a50, 0x3a33, 0x2a12, 0xdbfd, 0xcbdc, 15 | 0xfbbf, 0xeb9e, 0x9b79, 0x8b58, 0xbb3b, 0xab1a, 0x6ca6, 0x7c87, 0x4ce4, 16 | 0x5cc5, 0x2c22, 0x3c03, 0x0c60, 0x1c41, 0xedae, 0xfd8f, 0xcdec, 0xddcd, 17 | 0xad2a, 0xbd0b, 0x8d68, 0x9d49, 0x7e97, 0x6eb6, 0x5ed5, 0x4ef4, 0x3e13, 18 | 0x2e32, 0x1e51, 0x0e70, 0xff9f, 0xefbe, 0xdfdd, 0xcffc, 0xbf1b, 0xaf3a, 19 | 0x9f59, 0x8f78, 0x9188, 0x81a9, 0xb1ca, 0xa1eb, 0xd10c, 0xc12d, 0xf14e, 20 | 0xe16f, 0x1080, 0x00a1, 0x30c2, 0x20e3, 0x5004, 0x4025, 0x7046, 0x6067, 21 | 0x83b9, 0x9398, 0xa3fb, 0xb3da, 0xc33d, 0xd31c, 0xe37f, 0xf35e, 0x02b1, 22 | 0x1290, 0x22f3, 0x32d2, 0x4235, 0x5214, 0x6277, 0x7256, 0xb5ea, 0xa5cb, 23 | 0x95a8, 0x8589, 0xf56e, 0xe54f, 0xd52c, 0xc50d, 0x34e2, 0x24c3, 0x14a0, 24 | 0x0481, 0x7466, 0x6447, 0x5424, 0x4405, 0xa7db, 0xb7fa, 0x8799, 0x97b8, 25 | 0xe75f, 0xf77e, 0xc71d, 0xd73c, 0x26d3, 0x36f2, 0x0691, 0x16b0, 0x6657, 26 | 0x7676, 0x4615, 0x5634, 0xd94c, 0xc96d, 0xf90e, 0xe92f, 0x99c8, 0x89e9, 27 | 0xb98a, 0xa9ab, 0x5844, 0x4865, 0x7806, 0x6827, 0x18c0, 0x08e1, 0x3882, 28 | 0x28a3, 0xcb7d, 0xdb5c, 0xeb3f, 0xfb1e, 0x8bf9, 0x9bd8, 0xabbb, 0xbb9a, 29 | 0x4a75, 0x5a54, 0x6a37, 0x7a16, 0x0af1, 0x1ad0, 0x2ab3, 0x3a92, 0xfd2e, 30 | 0xed0f, 0xdd6c, 0xcd4d, 0xbdaa, 0xad8b, 0x9de8, 0x8dc9, 0x7c26, 0x6c07, 31 | 0x5c64, 0x4c45, 0x3ca2, 0x2c83, 0x1ce0, 0x0cc1, 0xef1f, 0xff3e, 0xcf5d, 32 | 0xdf7c, 0xaf9b, 0xbfba, 0x8fd9, 0x9ff8, 0x6e17, 0x7e36, 0x4e55, 0x5e74, 33 | 0x2e93, 0x3eb2, 0x0ed1, 0x1ef0}; 34 | 35 | uint16_t crc16(const uint8_t *buf, int len) { 36 | uint16_t crc = 0; 37 | for (int i = 0; i < len; i++) { 38 | crc = crc16_table[(((crc >> 8) ^ *buf++) & 0xFF)] ^ (crc << 8); 39 | } 40 | return crc; 41 | } 42 | -------------------------------------------------------------------------------- /imu/MadgwickAHRS.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | //============================================================================================= 5 | // MadgwickAHRS.h 6 | //============================================================================================= 7 | // 8 | // Implementation of Madgwick's IMU and AHRS algorithms. 9 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 10 | // 11 | // From the x-io website "Open-source resources available on this website are 12 | // provided under the GNU General Public Licence unless an alternative licence 13 | // is provided in source." 14 | // 15 | // Date Author Notes 16 | // 29/09/2011 SOH Madgwick Initial release 17 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 18 | // 19 | //============================================================================================= 20 | #ifndef MadgwickAHRS_h 21 | #define MadgwickAHRS_h 22 | #include 23 | 24 | //-------------------------------------------------------------------------------------------- 25 | // Variable declaration 26 | class Madgwick { 27 | private: 28 | static float invSqrt(float x); 29 | float q0; 30 | float q1; 31 | float q2; 32 | float q3; // quaternion of sensor frame relative to auxiliary frame 33 | float invSampleFreq; 34 | float roll; 35 | float pitch; 36 | float yaw; 37 | char anglesComputed; 38 | const float* beta_ptr_; 39 | void computeAngles(); 40 | 41 | //------------------------------------------------------------------------------------------- 42 | // Function declarations 43 | public: 44 | Madgwick(); 45 | void begin(float sampleFrequency) { invSampleFreq = 1.0f / sampleFrequency; } 46 | void updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float beta); 47 | //float getPitch(){return atan2f(2.0f * q2 * q3 - 2.0f * q0 * q1, 2.0f * q0 * q0 + 2.0f * q3 * q3 - 1.0f);}; 48 | //float getRoll(){return -1.0f * asinf(2.0f * q1 * q3 + 2.0f * q0 * q2);}; 49 | //float getYaw(){return atan2f(2.0f * q1 * q2 - 2.0f * q0 * q3, 2.0f * q0 * q0 + 2.0f * q1 * q1 - 1.0f);}; 50 | float getRoll() { 51 | if (!anglesComputed) computeAngles(); 52 | return roll * 57.29578f; 53 | } 54 | float getPitch() { 55 | if (!anglesComputed) computeAngles(); 56 | return pitch * 57.29578f; 57 | } 58 | float getYaw() { 59 | if (!anglesComputed) computeAngles(); 60 | return yaw * 57.29578f + 180.0f; 61 | } 62 | float getRollRadians() { 63 | if (!anglesComputed) computeAngles(); 64 | return roll; 65 | } 66 | float getPitchRadians() { 67 | if (!anglesComputed) computeAngles(); 68 | return pitch; 69 | } 70 | float getYawRadians() { 71 | if (!anglesComputed) computeAngles(); 72 | return yaw; 73 | } 74 | }; 75 | #endif 76 | -------------------------------------------------------------------------------- /lpf.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | #include 5 | 6 | class LPF 7 | { 8 | public: 9 | LPF(const float* rc) 10 | { 11 | _rc = rc; 12 | reset(); 13 | } 14 | 15 | float compute(float value) 16 | { 17 | _prevValue = _prevValue * (1-*_rc) + value * *_rc; 18 | return _prevValue; 19 | } 20 | 21 | float getVal() const { return _prevValue; } 22 | 23 | void reset() { 24 | reset(0); 25 | } 26 | 27 | void reset(float value) { 28 | _prevValue = value; 29 | } 30 | 31 | const float *_rc; 32 | 33 | private: 34 | float _prevValue; 35 | }; 36 | 37 | 38 | void biquad_butterworth_init(float cutoffFreqHz, float samplingFreqHz, float* filter_params); 39 | float biquad_compute(float in, float params[5], float state[4]); 40 | 41 | 42 | 43 | class BiQuadLpf { 44 | public: 45 | explicit BiQuadLpf(const float* frequency_ptr) 46 | : frequency_ptr_(frequency_ptr) { } 47 | 48 | void reset() { 49 | reset(0); 50 | } 51 | 52 | void reset(float value) { 53 | for (int i = 0; i < 4; i++) { 54 | bw_state_[i] = value; 55 | } 56 | } 57 | 58 | float compute(float value) 59 | { 60 | const bool params_changed = prev_rc_ != *frequency_ptr_; 61 | if (params_changed) { 62 | prev_rc_ = *frequency_ptr_; 63 | biquad_butterworth_init(prev_rc_, 1000, bw_params_); 64 | } 65 | 66 | return prev_value_ = biquad_compute(value, bw_params_, bw_state_); 67 | } 68 | 69 | float getVal() const { return prev_value_; } 70 | 71 | 72 | private: 73 | float prev_rc_ = -1; 74 | const float* frequency_ptr_; 75 | float bw_params_[5]; 76 | float bw_state_[4]; 77 | 78 | float prev_value_; 79 | }; 80 | 81 | 82 | static const float kLoopRateMult = 0.001; 83 | 84 | class Ramp { 85 | public: 86 | 87 | 88 | explicit Ramp(const float* raise_rate, const float* drop_rate, float* limit) : raise_rate_(raise_rate), drop_rate_(drop_rate), limit_(limit) { 89 | 90 | } 91 | 92 | void Reset() { 93 | value_ = 0; 94 | } 95 | 96 | float Compute(float new_value) { 97 | const bool new_positive = new_value > 0; 98 | const bool old_positive = value_ > 0; 99 | float max_rate; 100 | 101 | if (new_value == 0) { 102 | max_rate = *drop_rate_; 103 | } 104 | else { 105 | max_rate = new_positive != old_positive || fabsf(new_value) > fabsf(value_) ? *raise_rate_ : *drop_rate_; 106 | } 107 | 108 | max_rate *= kLoopRateMult; 109 | 110 | value_ += constrain((new_value - value_), -max_rate, max_rate ); 111 | value_ = constrain(value_, -*limit_, *limit_); 112 | return value_; 113 | } 114 | 115 | private: 116 | const float* raise_rate_; 117 | const float* drop_rate_; 118 | const float* limit_; 119 | 120 | float value_; 121 | }; 122 | -------------------------------------------------------------------------------- /balanceController.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | 4 | #include "global.h" 5 | #include "pid.hpp" 6 | #include "lpf.hpp" 7 | 8 | 9 | class BalanceController { 10 | public: 11 | BalanceController(const Config* settings) : 12 | settings_(settings), d_lpf_(&settings->balance_settings.balance_d_param_lpf_hz), angle_pid_(&settings->angle_pid), rate_pid_(&settings->rate_pid) { 13 | reset(); 14 | } 15 | 16 | void reset() { 17 | angle_pid_.reset(); 18 | rate_pid_.reset(); 19 | d_lpf_.reset(); 20 | max_D_multiplier_so_far_ = 0; 21 | prev_error_ = 0; 22 | } 23 | 24 | float getPIInput(float* angles, float balance_angle) { 25 | float raw_input = (balance_angle - angles[ANGLE_DRIVE]) / settings_->balance_settings.balance_angle_scaling; 26 | return constrain(raw_input, -1, 1); 27 | } 28 | 29 | float getPInput(float* angles, float balance_angle) { 30 | float p_input = getPIInput(angles, balance_angle); 31 | switch (settings_->balance_settings.expo_type) { 32 | case 0: return applyExpoReal(p_input, settings_->balance_settings.balance_expo); 33 | case 1: return applyExpoNatural(p_input, settings_->balance_settings.balance_expo); 34 | case 2: return applyExpoPoly(p_input, settings_->balance_settings.balance_expo); 35 | default: return p_input; 36 | } 37 | } 38 | 39 | float calcRatePid(float rateRequest, float rate) { 40 | float error = rateRequest * 400 - rate; 41 | float d_term = error - prev_error_; 42 | d_term = constrain(d_term, -settings_->balance_settings.balance_d_param_limiter, settings_->balance_settings.balance_d_param_limiter); 43 | prev_error_ = error; 44 | d_term = d_lpf_.compute(d_term); 45 | 46 | return rate_pid_.compute(error, d_term); 47 | } 48 | 49 | // Compute torque needed while board in normal mode. 50 | // Returns torque request based on current imu and gyro readings. Expected range is [-1:1], 51 | // but not constrained to that range. 52 | float compute(const float* gyro_rates, float* angles, float balance_angle) { 53 | float rateRequest = angle_pid_.compute(getPInput(angles, balance_angle)); 54 | return calcRatePid(rateRequest, -gyro_rates[ANGLE_DRIVE]); 55 | } 56 | 57 | // Compute torque needed while board in starting up phase (coming from one side to balanced state). 58 | // Returns torque request based on current imu and gyro readings. Expected range is [-1:1], 59 | // but not constrained to that range. 60 | float computeStarting(const float* gyro_rates, float* angles, float pid_P_multiplier) { 61 | rate_pid_.resetI(); 62 | angle_pid_.resetI(); 63 | float rateRequest = angle_pid_.compute(getPInput(angles, 0)); 64 | rateRequest *= pid_P_multiplier; 65 | float pid_out = calcRatePid(rateRequest, -gyro_rates[ANGLE_DRIVE]) * pid_P_multiplier; 66 | 67 | return constrain(pid_out, -START_MAX_POWER, START_MAX_POWER); 68 | } 69 | 70 | private: 71 | const Config* settings_; 72 | float max_D_multiplier_so_far_ = 0; 73 | BiQuadLpf d_lpf_; 74 | PidController angle_pid_; 75 | PidController rate_pid_; 76 | float prev_error_ = 0; 77 | }; 78 | -------------------------------------------------------------------------------- /drv/led/led.cpp: -------------------------------------------------------------------------------- 1 | #include "led.hpp" 2 | #include 3 | 4 | #include "stm32f10x_gpio.h" 5 | #include "stm32f10x_rcc.h" 6 | #include "stm32f10x_dma.h" 7 | #include "stm32f10x_tim.h" 8 | 9 | #define BUS_SPEED 800000 // hz 10 | 11 | #define TICKS_PER_BIT (SystemCoreClock / BUS_SPEED - 1) 12 | 13 | #define BIT_ONE (TICKS_PER_BIT*0.8) 14 | #define BIT_ZERO (TICKS_PER_BIT*0.2) 15 | #define RESET_BITS 16 | uint8_t pwm_out_buffer[LED_COUNT*24 + 50]; 17 | 18 | void led_init() { 19 | memset(pwm_out_buffer, 0, sizeof(pwm_out_buffer)); 20 | 21 | 22 | for (int i = 0; i < LED_COUNT; i++) { 23 | led_set_color(i, 0x00A000); 24 | } 25 | 26 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); 27 | 28 | GPIO_InitTypeDef GPIO_InitStructure; 29 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_6; 30 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 31 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 32 | GPIO_Init(GPIOA, &GPIO_InitStructure); 33 | 34 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); 35 | 36 | TIM_TimeBaseInitTypeDef TimerBaseInit; 37 | TIM_TimeBaseStructInit(&TimerBaseInit); 38 | 39 | TimerBaseInit.TIM_Prescaler = 0; 40 | TimerBaseInit.TIM_Period = TICKS_PER_BIT; 41 | TimerBaseInit.TIM_CounterMode = TIM_CounterMode_Up; 42 | TimerBaseInit.TIM_ClockDivision = TIM_CKD_DIV1; 43 | TIM_TimeBaseInit(TIM3,&TimerBaseInit); 44 | 45 | TIM_OCInitTypeDef OC_Config; 46 | TIM_OCStructInit(&OC_Config); 47 | OC_Config.TIM_OCMode = TIM_OCMode_PWM1; 48 | OC_Config.TIM_Pulse = 0; 49 | OC_Config.TIM_OutputState = TIM_OutputState_Enable; 50 | OC_Config.TIM_OCPolarity = TIM_OCPolarity_High; 51 | TIM_OC1Init(TIM3, &OC_Config); 52 | TIM_OC1PreloadConfig(TIM3, TIM_OCPreload_Enable); 53 | 54 | 55 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); 56 | DMA_DeInit(DMA1_Channel6); 57 | 58 | DMA_InitTypeDef DMA_InitStructure; 59 | DMA_StructInit(&DMA_InitStructure); 60 | 61 | DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&TIM3->CCR1; 62 | DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)pwm_out_buffer; 63 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST; 64 | DMA_InitStructure.DMA_BufferSize = sizeof(pwm_out_buffer); 65 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; 66 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; 67 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; 68 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; 69 | DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; 70 | DMA_InitStructure.DMA_Priority = DMA_Priority_Low; 71 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; 72 | DMA_Init(DMA1_Channel6, &DMA_InitStructure); 73 | 74 | DMA_Cmd(DMA1_Channel6, ENABLE); 75 | TIM_DMACmd(TIM3, TIM_DMA_CC1, ENABLE); 76 | TIM_Cmd(TIM3, ENABLE); 77 | } 78 | 79 | 80 | void led_set_color(int led_idx, uint32_t color) { 81 | int led_offset = (led_idx+1)*24; // we set the bits backwards, thus + 1 82 | for (int i = 0; i < 24; i++) { 83 | pwm_out_buffer[led_offset - i] = (color & 1) ? BIT_ONE : BIT_ZERO; 84 | color = color >> 1; 85 | } 86 | } 87 | -------------------------------------------------------------------------------- /Makefile.bak: -------------------------------------------------------------------------------- 1 | 2 | include ../nanopb/extra/nanopb.mk 3 | 4 | BUILD_DIR := build 5 | 6 | .DEFAULT_GOAL := ${BUILD_DIR}/BalancingController.elf 7 | 8 | STM32_KIT=$(wildcard stm_lib/src/*.c) $(wildcard syscalls/*.c) $(wildcard cmsis_boot/*.c) $(wildcard cmsis_boot/*/*.c) 9 | 10 | HDRS := $(wildcard *.h) $(wildcard *.hpp) $(wildcard */*.h) $(wildcard */*.hpp) $(wildcard */*/*.h) $(wildcard */*/*.hpp) $(wildcard */*/*/*.h) $(wildcard */*/*/*.hpp) drv/comms/protocol.pb.h drv/comms/config.pb.h 11 | SRCS := $(wildcard *.cpp) $(wildcard io/*.cpp) $(wildcard imu/*.cpp) $(wildcard guards/*.cpp) $(wildcard drv/vesc/*.cpp) $(wildcard drv/comms/*.cpp) $(wildcard drv/settings/*.cpp) $(wildcard drv/mpu6050/*.cpp) $(wildcard drv/led/*.cpp) ${STM32_KIT} ${NANOPB_CORE} drv/comms/protocol.pb.c drv/comms/config.pb.c 12 | INC:=drv cmsis_boot drv/vesc drv/comms stm_lib/inc cmsis . $(NANOPB_DIR) 13 | INC_PARAMS=$(INC:%=-I%) 14 | 15 | CC = arm-none-eabi-gcc 16 | CXX = arm-none-eabi-gcc 17 | 18 | ARCH = -mcpu=cortex-m3 -mthumb 19 | CFLAGS = ${ARCH} -Wall -ffunction-sections -g -O2 -flto -fno-builtin -c -DSTM32F103CB -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -D__ASSEMBLY__ -DSUPPORT_CPLUSPLUS # -fstack-usage 20 | CPPFLAGS = $(CFLAGS) -std=gnu++11 21 | 22 | OBJS := $(SRCS:%=$(BUILD_DIR)/%.o) 23 | 24 | ifeq ($(OS),Windows_NT) 25 | MKDIR = if not exist "$(DIR)" mkdir "$(DIR)" 26 | else 27 | MKDIR = mkdir -p "$(DIR)" 28 | endif 29 | 30 | create-dir: 31 | $(MKDIR) 32 | 33 | # Build step for C source 34 | ${BUILD_DIR}/%.c.o : %.c ${HDRS} 35 | $(MAKE) create-dir DIR=$(dir $@) 36 | ${CC} $(CFLAGS) ${INC_PARAMS} -c $< -o $@ 37 | 38 | # Build step for C++ source 39 | $(BUILD_DIR)/%.cpp.o: %.cpp ${HDRS} 40 | $(MAKE) create-dir DIR=$(dir $@) 41 | $(CXX) $(CPPFLAGS) $(INC_PARAMS) -c $< -o $@ 42 | 43 | ${BUILD_DIR}/descriptor.pb.bin: drv/comms/config.proto 44 | $(PROTOC) $< --descriptor_set_out=$@ 45 | 46 | ${BUILD_DIR}/descriptor.pb.bin.deflate: ${BUILD_DIR}/descriptor.pb.bin 47 | python -c "import zlib,sys; sys.stdout.buffer.write(zlib.compress(sys.stdin.buffer.read(),9))" < $< > $@ 48 | 49 | ${BUILD_DIR}/descriptor.pb.deflate.o: ${BUILD_DIR}/descriptor.pb.bin.deflate 50 | # arm-none-eabi-ld.exe -r -b binary $< -o $@ 51 | arm-none-eabi-objcopy.exe -I binary -B arm -O 'elf32-littlearm' $< $@ --rename-section=.data=.rodata 52 | 53 | # The final build step. 54 | ${BUILD_DIR}/BalancingController.elf: $(OBJS) link.ld ${BUILD_DIR}/descriptor.pb.deflate.o 55 | $(CC) ${ARCH} -g -flto -Wl,-Map=${BUILD_DIR}/BalancingController.map -O2 -Wl,--gc-sections -Wl,--entry=main -Wl,-T./link.ld -g -o $@ $(OBJS) ${BUILD_DIR}/descriptor.pb.deflate.o -lm -lgcc -lc -lstdc++ 56 | 57 | ${BUILD_DIR}/BalancingController.bin: ${BUILD_DIR}/BalancingController.elf 58 | arm-none-eabi-objcopy -O binary ${BUILD_DIR}/BalancingController.elf $@ 59 | 60 | ${BUILD_DIR}/BalancingController.hex: ${BUILD_DIR}/BalancingController.elf 61 | arm-none-eabi-objcopy -O ihex ${BUILD_DIR}/BalancingController.elf $@ 62 | 63 | 64 | .PHONY: clean program 65 | clean: 66 | rm -r $(BUILD_DIR) 67 | 68 | program: ${BUILD_DIR}/BalancingController.elf 69 | "C:/CooCox/CoIDE/bin\coflash.exe" program STM32F103CB $< --adapter-name=ST-Link --port=SWD --adapter-clk=2000000 --erase=affected --reset=SYSRESETREQ --driver="C:/CooCox/CoIDE/flash/STM32F10x_MD_128.elf" --verify=false 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /link.ld: -------------------------------------------------------------------------------- 1 | OUTPUT_FORMAT ("elf32-littlearm", "elf32-bigarm", "elf32-littlearm") 2 | /* Internal Memory Map*/ 3 | MEMORY 4 | { 5 | rom (rx) : ORIGIN = 0x08000000, LENGTH = 0x00020000 6 | ram (rwx) : ORIGIN = 0x20000000, LENGTH = 0x00005000 7 | } 8 | 9 | _eram = 0x20000000 + 0x00005000; 10 | SECTIONS 11 | { 12 | .text : 13 | { 14 | KEEP(*(.isr_vector)) 15 | *(.text*) 16 | 17 | KEEP(*(.init)) 18 | KEEP(*(.fini)) 19 | 20 | /* .ctors */ 21 | *crtbegin.o(.ctors) 22 | *crtbegin?.o(.ctors) 23 | *(EXCLUDE_FILE(*crtend?.o *crtend.o) .ctors) 24 | *(SORT(.ctors.*)) 25 | *(.ctors) 26 | 27 | /* .dtors */ 28 | *crtbegin.o(.dtors) 29 | *crtbegin?.o(.dtors) 30 | *(EXCLUDE_FILE(*crtend?.o *crtend.o) .dtors) 31 | *(SORT(.dtors.*)) 32 | *(.dtors) 33 | 34 | *(.rodata*) 35 | 36 | KEEP(*(.eh_fram e*)) 37 | } > rom 38 | 39 | .ARM.extab : 40 | { 41 | *(.ARM.extab* .gnu.linkonce.armextab.*) 42 | } > rom 43 | 44 | __exidx_start = .; 45 | .ARM.exidx : 46 | { 47 | *(.ARM.exidx* .gnu.linkonce.armexidx.*) 48 | } > rom 49 | __exidx_end = .; 50 | __etext = .; 51 | 52 | /* _sidata is used in coide startup code */ 53 | _sidata = __etext; 54 | 55 | .data : AT (__etext) 56 | { 57 | __data_start__ = .; 58 | 59 | /* _sdata is used in coide startup code */ 60 | _sdata = __data_start__; 61 | 62 | *(vtable) 63 | *(.data*) 64 | 65 | . = ALIGN(4); 66 | /* preinit data */ 67 | PROVIDE_HIDDEN (__preinit_array_start = .); 68 | KEEP(*(.preinit_array)) 69 | PROVIDE_HIDDEN (__preinit_array_end = .); 70 | 71 | . = ALIGN(4); 72 | /* init data */ 73 | PROVIDE_HIDDEN (__init_array_start = .); 74 | KEEP(*(SORT(.init_array.*))) 75 | KEEP(*(.init_array)) 76 | PROVIDE_HIDDEN (__init_array_end = .); 77 | 78 | . = ALIGN(4); 79 | /* finit data */ 80 | PROVIDE_HIDDEN (__fini_array_start = .); 81 | KEEP(*(SORT(.fini_array.*))) 82 | KEEP(*(.fini_array)) 83 | PROVIDE_HIDDEN (__fini_array_end = .); 84 | 85 | KEEP(*(.jcr*)) 86 | . = ALIGN(4); 87 | /* All data end */ 88 | __data_end__ = .; 89 | 90 | /* _edata is used in coide startup code */ 91 | _edata = __data_end__; 92 | } > ram 93 | 94 | .bss : 95 | { 96 | . = ALIGN(4); 97 | __bss_start__ = .; 98 | _sbss = __bss_start__; 99 | *(.bss*) 100 | *(COMMON) 101 | . = ALIGN(4); 102 | __bss_end__ = .; 103 | _ebss = __bss_end__; 104 | } > ram 105 | 106 | .heap (COPY): 107 | { 108 | __end__ = .; 109 | _end = __end__; 110 | end = __end__; 111 | *(.heap*) 112 | __HeapLimit = .; 113 | } > ram 114 | 115 | /* .stack_dummy section doesn't contains any symbols. It is only 116 | * used for linker to calculate size of stack sections, and assign 117 | * values to stack symbols later */ 118 | .co_stack (NOLOAD): 119 | { 120 | . = ALIGN(8); 121 | *(.co_stack .co_stack.*) 122 | } > ram 123 | 124 | /* Set stack top to end of ram , and stack limit move down by 125 | * size of stack_dummy section */ 126 | __StackTop = ORIGIN(ram ) + LENGTH(ram ); 127 | __StackLimit = __StackTop - SIZEOF(.co_stack); 128 | PROVIDE(__stack = __StackTop); 129 | 130 | /* Check if data + heap + stack exceeds ram limit */ 131 | ASSERT(__StackLimit >= __HeapLimit, "region ram overflowed with stack") 132 | } 133 | -------------------------------------------------------------------------------- /cmsis_boot/stm32f10x_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file RTC/Calendar/stm32f10x_conf.h 4 | * @author MCD Application Team 5 | * @version V3.4.0 6 | * @date 10/15/2010 7 | * @brief Library configuration file. 8 | ****************************************************************************** 9 | * @copy 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2010 STMicroelectronics

19 | */ 20 | 21 | /* Define to prevent recursive inclusion -------------------------------------*/ 22 | #ifndef __STM32F10x_CONF_H 23 | #define __STM32F10x_CONF_H 24 | 25 | /* Includes ------------------------------------------------------------------*/ 26 | /* Uncomment the line below to enable peripheral header file inclusion */ 27 | /* #include "stm32f10x_adc.h" */ 28 | /* #include "stm32f10x_bkp.h" */ 29 | /* #include "stm32f10x_can.h" */ 30 | /* #include "stm32f10x_cec.h" */ 31 | /* #include "stm32f10x_crc.h" */ 32 | /* #include "stm32f10x_dac.h" */ 33 | /* #include "stm32f10x_dbgmcu.h" */ 34 | /* #include "stm32f10x_dma.h" */ 35 | /* #include "stm32f10x_exti.h" */ 36 | /* #include "stm32f10x_flash.h" */ 37 | /* #include "stm32f10x_fsmc.h" */ 38 | /* #include "stm32f10x_gpio.h" */ 39 | /* #include "stm32f10x_i2c.h" */ 40 | /* #include "stm32f10x_iwdg.h" */ 41 | /* #include "stm32f10x_pwr.h" */ 42 | /* #include "stm32f10x_rcc.h" */ 43 | /* #include "stm32f10x_rtc.h" */ 44 | /* #include "stm32f10x_sdio.h" */ 45 | /* #include "stm32f10x_spi.h" */ 46 | /* #include "stm32f10x_tim.h" */ 47 | /* #include "stm32f10x_usart.h" */ 48 | /* #include "stm32f10x_wwdg.h" */ 49 | /* #include "misc.h" */ /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ 50 | 51 | 52 | /* Exported types ------------------------------------------------------------*/ 53 | /* Exported constants --------------------------------------------------------*/ 54 | /* Uncomment the line below to expanse the "assert_param" macro in the 55 | Standard Peripheral Library drivers code */ 56 | /* #define USE_FULL_ASSERT 1 */ 57 | 58 | /* Exported macro ------------------------------------------------------------*/ 59 | #ifdef USE_FULL_ASSERT 60 | 61 | /** 62 | * @brief The assert_param macro is used for function's parameters check. 63 | * @param expr: If expr is false, it calls assert_failed function 64 | * which reports the name of the source file and the source 65 | * line number of the call that failed. 66 | * If expr is true, it returns no value. 67 | * @retval None 68 | */ 69 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 70 | /* Exported functions ------------------------------------------------------- */ 71 | void assert_failed(uint8_t* file, uint32_t line); 72 | #else 73 | #define assert_param(expr) ((void)0) 74 | #endif /* USE_FULL_ASSERT */ 75 | 76 | #endif /* __STM32F10x_CONF_H */ 77 | 78 | /******************* (C) COPYRIGHT 2010 STMicroelectronics *****END OF FILE****/ 79 | -------------------------------------------------------------------------------- /drv/comms/communicator.cpp: -------------------------------------------------------------------------------- 1 | #include "communicator.hpp" 2 | 3 | void Communicator::SendMsg(uint8_t msg_id, const uint8_t* data, uint32_t data_len) { 4 | uint8_t header[kHeaderSize]; 5 | const uint32_t total_len = data_len + kMetadataSize; 6 | header[0] = msg_id; 7 | header[1] = total_len; 8 | header[2] = total_len >> 8; 9 | header[3] = 0; 10 | CRC_ResetDR(); 11 | uint32_t crc32 = CRC_CalcCRC(*(uint32_t*)header); 12 | 13 | uint32_t aligned_blocks = data_len / 4; 14 | if (data_len > 0) { 15 | crc32 = CRC_CalcBlockCRC((uint32_t*)data, aligned_blocks); 16 | } 17 | uint8_t last_block[4] = {0}; 18 | uint32_t remaining_offset = aligned_blocks * 4; 19 | for (uint32_t i = 0; i < data_len - remaining_offset; i++) { 20 | last_block[i] = data[remaining_offset + i]; 21 | } 22 | if (remaining_offset != data_len) { 23 | crc32 = CRC_CalcCRC(*(uint32_t*)last_block); 24 | } 25 | 26 | comms_->Send(header, kHeaderSize); 27 | comms_->SendWithWait(data, data_len); 28 | comms_->SendWithWait((uint8_t*)&crc32, kSuffixSize); 29 | } 30 | 31 | void Communicator::SendMsg(uint8_t msg_id) { 32 | uint8_t header[kHeaderSize] = { 0 }; 33 | header[0] = msg_id; 34 | header[1] = kMetadataSize; 35 | 36 | CRC_ResetDR(); 37 | uint32_t crc32 = CRC_CalcCRC(*(uint32_t*)header); 38 | comms_->Send(header, kHeaderSize); 39 | comms_->Send((uint8_t*)&crc32, kSuffixSize); 40 | } 41 | 42 | int Communicator::update() { 43 | if (!comms_->HasData()) 44 | return false; 45 | 46 | uint16_t time = half_millis(); 47 | if ((uint16_t)(time - last_uart_data_time_) > kMsgTimeoutMs) { 48 | buffer_pos_ = 0; 49 | move_message_ = false; 50 | // comms_->Send("Timeout\n", 8); 51 | } 52 | last_uart_data_time_ = time; 53 | 54 | if (move_message_) { 55 | move_message_ = false; 56 | int bytes_to_move = buffer_pos_ - (int)expected_msg_len(); 57 | 58 | if (bytes_to_move > 0) { 59 | memmove(rx_data, rx_data + expected_msg_len(), bytes_to_move); 60 | buffer_pos_ = bytes_to_move; 61 | } 62 | else { 63 | buffer_pos_ = 0; 64 | } 65 | } 66 | 67 | int32_t received_bytes = comms_->Read(rx_data + buffer_pos_, sizeof(rx_data) - buffer_pos_); 68 | buffer_pos_+= received_bytes; 69 | if (buffer_pos_ > kHeaderSize) { 70 | if (buffer_pos_ >= sizeof(rx_data)) 71 | buffer_pos_ = 0; // too long/invalid 72 | 73 | if (buffer_pos_ >= expected_msg_len()) { 74 | int32_t msg_len = expected_msg_len() - kSuffixSize; 75 | CRC_ResetDR(); 76 | int32_t aligned_blocks = msg_len / 4; 77 | uint32_t crc32; 78 | if (msg_len > 0) { 79 | crc32 = CRC_CalcBlockCRC((uint32_t*)rx_data, aligned_blocks); 80 | } 81 | uint8_t last_block[4] = {0}; 82 | int remaining_offset = aligned_blocks * 4; 83 | for (int i = 0; i < msg_len - remaining_offset; i++) { 84 | last_block[i] = rx_data[remaining_offset + i]; 85 | } 86 | if (remaining_offset != msg_len) { 87 | crc32 = CRC_CalcCRC(*(uint32_t*)last_block); 88 | } 89 | 90 | uint8_t* crc_bytes = (uint8_t*)&crc32; 91 | int msg_id = expected_msg_id(); 92 | if (crc_bytes[0] != expected_msg_crc_lo() || crc_bytes[1] != expected_msg_crc_hi()) 93 | { 94 | msg_id = RequestId_MSG_NONE; 95 | SendMsg(ReplyId_CRC_MISMATCH); 96 | } 97 | 98 | move_message_ = true; 99 | return msg_id; 100 | } 101 | } 102 | 103 | return 0; 104 | } -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | 2 | #TODO must pip install --upgrade protobuf grpcio-tools 3 | 4 | include ../nanopb/extra/nanopb.mk 5 | 6 | BUILD_DIR := build 7 | 8 | .DEFAULT_GOAL := ${BUILD_DIR}/BalancingController.elf 9 | 10 | STM32_KIT=$(wildcard stm_lib/src/*.c) $(wildcard syscalls/*.c) $(wildcard cmsis_boot/*.c) $(wildcard cmsis_boot/*/*.c) 11 | 12 | HDRS := $(wildcard *.h) $(wildcard *.hpp) $(wildcard */*.h) $(wildcard */*.hpp) $(wildcard */*/*.h) $(wildcard */*/*.hpp) $(wildcard */*/*/*.h) $(wildcard */*/*/*.hpp) drv/comms/protocol.pb.h drv/comms/config.pb.h 13 | SRCS := $(wildcard *.cpp) $(wildcard io/*.cpp) $(wildcard imu/*.cpp) $(wildcard guards/*.cpp) $(wildcard drv/vesc/*.cpp) $(wildcard drv/comms/*.cpp) $(wildcard drv/settings/*.cpp) $(wildcard drv/mpu6050/*.cpp) $(wildcard drv/led/*.cpp) ${STM32_KIT} ${NANOPB_CORE} drv/comms/protocol.pb.c drv/comms/config.pb.c 14 | INC:=drv cmsis_boot drv/vesc drv/comms stm_lib/inc cmsis . $(NANOPB_DIR) 15 | INC_PARAMS=$(INC:%=-I%) 16 | 17 | CC = arm-none-eabi-gcc 18 | CXX = arm-none-eabi-gcc 19 | 20 | ARCH = -mcpu=cortex-m3 -mthumb 21 | CFLAGS = ${ARCH} -Wall -ffunction-sections -g -O2 -flto -fno-builtin -c -DSTM32F103CB -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -D__ASSEMBLY__ -DSUPPORT_CPLUSPLUS # -fstack-usage 22 | CPPFLAGS = $(CFLAGS) -std=gnu++11 23 | 24 | OBJS := $(SRCS:%=$(BUILD_DIR)/%.o) 25 | 26 | ifeq ($(OS),Windows_NT) 27 | MKDIR = if not exist "$(DIR)" mkdir "$(DIR)" 28 | else 29 | MKDIR = mkdir -p "$(DIR)" 30 | endif 31 | 32 | create-dir: 33 | $(MKDIR) 34 | 35 | # Build step for C source 36 | ${BUILD_DIR}/%.c.o : %.c ${HDRS} 37 | $(MAKE) create-dir DIR=$(dir $@) 38 | ${CC} $(CFLAGS) ${INC_PARAMS} -c $< -o $@ 39 | 40 | # Build step for C++ source 41 | $(BUILD_DIR)/%.cpp.o: %.cpp ${HDRS} 42 | $(MAKE) create-dir DIR=$(dir $@) 43 | $(CXX) $(CPPFLAGS) $(INC_PARAMS) -c $< -o $@ 44 | 45 | ${BUILD_DIR}/descriptor.pb.bin: drv/comms/config.proto 46 | $(PROTOC) $< --descriptor_set_out=$@ 47 | 48 | ${BUILD_DIR}/descriptor.pb.bin.deflate: ${BUILD_DIR}/descriptor.pb.bin 49 | python3 -c "import zlib,sys; sys.stdout.buffer.write(zlib.compress(sys.stdin.buffer.read(),9))" < $< > $@ 50 | 51 | ${BUILD_DIR}/descriptor.pb.deflate.o: ${BUILD_DIR}/descriptor.pb.bin.deflate 52 | # arm-none-eabi-ld -r -b binary $< -o $@ 53 | arm-none-eabi-objcopy -I binary -B arm -O 'elf32-littlearm' $< $@ --rename-section=.data=.rodata 54 | 55 | # The final build step. 56 | ${BUILD_DIR}/BalancingController.elf: $(OBJS) link.ld ${BUILD_DIR}/descriptor.pb.deflate.o 57 | $(CC) ${ARCH} -g -flto -Wl,-Map=${BUILD_DIR}/BalancingController.map -O2 -Wl,--gc-sections -Wl,--entry=main -Wl,-T./link.ld -g -o $@ $(OBJS) ${BUILD_DIR}/descriptor.pb.deflate.o -lm -lgcc -lc -lstdc++ 58 | 59 | ${BUILD_DIR}/BalancingController.bin: ${BUILD_DIR}/BalancingController.elf 60 | arm-none-eabi-objcopy -O binary ${BUILD_DIR}/BalancingController.elf $@ 61 | 62 | ${BUILD_DIR}/BalancingController.hex: ${BUILD_DIR}/BalancingController.elf 63 | arm-none-eabi-objcopy -O ihex ${BUILD_DIR}/BalancingController.elf $@ 64 | 65 | 66 | .PHONY: clean program 67 | clean: 68 | rm -r $(BUILD_DIR) 69 | 70 | program: ${BUILD_DIR}/BalancingController.elf 71 | "C:/CooCox/CoIDE/bin\coflash.exe" program STM32F103CB $< --adapter-name=ST-Link --port=SWD --adapter-clk=2000000 --erase=affected --reset=SYSRESETREQ --driver="C:/CooCox/CoIDE/flash/STM32F10x_MD_128.elf" --verify=false 72 | 73 | program_openocd: 74 | openocd -f interface/stlink.cfg -f target/stm32f1x.cfg -c "init;targets;halt;flash write_image erase ${BUILD_DIR}/BalancingController.elf;verify_image ${BUILD_DIR}/BalancingController.elf;reset;shutdown" 75 | -------------------------------------------------------------------------------- /drv/vesc/vesc.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include "io/usart.hpp" 3 | #include 4 | 5 | typedef struct { 6 | float v_in; 7 | float temp_mos_filtered; 8 | float temp_motor_filtered; 9 | float avg_motor_current; 10 | float avg_input_current; 11 | float rpm; 12 | float duty_now; 13 | float amp_hours; 14 | float amp_hours_charged; 15 | int32_t tachometer; 16 | int32_t tachometer_abs; 17 | 18 | float v_in_smoothed; 19 | float duty_smoothed; 20 | float erpm_smoothed; 21 | } mc_values; 22 | 23 | class VescComm { 24 | public: 25 | VescComm(Usart* serial) : serial_(serial) { 26 | memset(&mc_values_, 0, sizeof(mc_values)); 27 | } 28 | 29 | void requestStats(); 30 | 31 | void setCurrent(float current); 32 | 33 | void setCurrentBrake(float current); 34 | 35 | int update(); 36 | 37 | 38 | // Communication commands 39 | enum class COMM_PACKET_ID { 40 | COMM_FW_VERSION = 0, 41 | COMM_JUMP_TO_BOOTLOADER, 42 | COMM_ERASE_NEW_APP, 43 | COMM_WRITE_NEW_APP_DATA, 44 | COMM_GET_VALUES, 45 | COMM_SET_DUTY, 46 | COMM_SET_CURRENT, 47 | COMM_SET_CURRENT_BRAKE, 48 | COMM_SET_RPM, 49 | COMM_SET_POS, 50 | COMM_SET_HANDBRAKE, 51 | COMM_SET_DETECT, 52 | COMM_SET_SERVO_POS, 53 | COMM_SET_MCCONF, 54 | COMM_GET_MCCONF, 55 | COMM_GET_MCCONF_DEFAULT, 56 | COMM_SET_APPCONF, 57 | COMM_GET_APPCONF, 58 | COMM_GET_APPCONF_DEFAULT, 59 | COMM_SAMPLE_PRINT, 60 | COMM_TERMINAL_CMD, 61 | COMM_PRINT, 62 | COMM_ROTOR_POSITION, 63 | COMM_EXPERIMENT_SAMPLE, 64 | COMM_DETECT_MOTOR_PARAM, 65 | COMM_DETECT_MOTOR_R_L, 66 | COMM_DETECT_MOTOR_FLUX_LINKAGE, 67 | COMM_DETECT_ENCODER, 68 | COMM_DETECT_HALL_FOC, 69 | COMM_REBOOT, 70 | COMM_ALIVE, 71 | COMM_GET_DECODED_PPM, 72 | COMM_GET_DECODED_ADC, 73 | COMM_GET_DECODED_CHUK, 74 | COMM_FORWARD_CAN, 75 | COMM_SET_CHUCK_DATA, 76 | COMM_CUSTOM_APP_DATA, 77 | COMM_NRF_START_PAIRING, 78 | COMM_GPD_SET_FSW, 79 | COMM_GPD_BUFFER_NOTIFY, 80 | COMM_GPD_BUFFER_SIZE_LEFT, 81 | COMM_GPD_FILL_BUFFER, 82 | COMM_GPD_OUTPUT_SAMPLE, 83 | COMM_GPD_SET_MODE, 84 | COMM_GPD_FILL_BUFFER_INT8, 85 | COMM_GPD_FILL_BUFFER_INT16, 86 | COMM_GPD_SET_BUFFER_INT_SCALE, 87 | COMM_GET_VALUES_SETUP, 88 | COMM_SET_MCCONF_TEMP, 89 | COMM_SET_MCCONF_TEMP_SETUP, 90 | COMM_GET_VALUES_SELECTIVE, 91 | COMM_GET_VALUES_SETUP_SELECTIVE, 92 | COMM_EXT_NRF_PRESENT, 93 | COMM_EXT_NRF_ESB_SET_CH_ADDR, 94 | COMM_EXT_NRF_ESB_SEND_DATA, 95 | COMM_EXT_NRF_ESB_RX_DATA, 96 | COMM_EXT_NRF_SET_ENABLED, 97 | COMM_DETECT_MOTOR_FLUX_LINKAGE_OPENLOOP, 98 | COMM_DETECT_APPLY_ALL_FOC, 99 | COMM_JUMP_TO_BOOTLOADER_ALL_CAN, 100 | COMM_ERASE_NEW_APP_ALL_CAN, 101 | COMM_WRITE_NEW_APP_DATA_ALL_CAN, 102 | COMM_PING_CAN, 103 | COMM_APP_DISABLE_OUTPUT, 104 | COMM_TERMINAL_CMD_SYNC, 105 | COMM_GET_IMU_DATA, 106 | COMM_BM_CONNECT, 107 | COMM_BM_ERASE_FLASH_ALL, 108 | COMM_BM_WRITE_FLASH, 109 | COMM_BM_REBOOT, 110 | COMM_BM_DISCONNECT 111 | } COMM_PACKET_ID; 112 | 113 | 114 | public: 115 | mc_values mc_values_; 116 | 117 | private: 118 | void sendRequest(const uint8_t* payload, int len); 119 | 120 | int expected_msg_len() { 121 | return rx_data_[0] == 2 ? rx_data_[1] : ( rx_data_[1] << 8 | rx_data_[2]); 122 | } 123 | 124 | int actual_header_size() { 125 | return rx_data_[0]; 126 | } 127 | 128 | Usart* serial_; 129 | 130 | // buffer_pos_ always points to non-written yet memory. bufferpos = 1 means have only one byte with index 0 131 | int32_t buffer_pos_ = 0; 132 | 133 | uint8_t rx_data_[256]; 134 | 135 | uint16_t last_uart_data_time_ = 0; 136 | }; 137 | -------------------------------------------------------------------------------- /guards/footpadGuard.cpp: -------------------------------------------------------------------------------- 1 | #include "footpadGuard.hpp" 2 | #include "stm32f10x_adc.h" 3 | #include "stm32f10x_rcc.h" 4 | #include "stm32f10x_gpio.h" 5 | #include "stm32f10x_dma.h" 6 | 7 | volatile uint16_t ADC1_Buffer[2] = { 0 }; 8 | 9 | void initADCs() { 10 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); 11 | 12 | GPIO_InitTypeDef GPIO_InitStructure; 13 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 14 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; 15 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0 | GPIO_Pin_1 ; 16 | GPIO_Init(GPIOA, &GPIO_InitStructure); 17 | 18 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_ADC1, ENABLE); 19 | 20 | ADC_InitTypeDef ADC_InitStructure; 21 | ADC_InitStructure.ADC_Mode = ADC_Mode_Independent; 22 | ADC_InitStructure.ADC_ScanConvMode = ENABLE; 23 | ADC_InitStructure.ADC_ContinuousConvMode = ENABLE; 24 | ADC_InitStructure.ADC_ExternalTrigConv = ADC_ExternalTrigConv_None; 25 | ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; 26 | ADC_InitStructure.ADC_NbrOfChannel = 2; 27 | ADC_Init ( ADC1, &ADC_InitStructure); 28 | 29 | ADC_RegularChannelConfig(ADC1, ADC_Channel_0, 1, ADC_SampleTime_239Cycles5); 30 | ADC_RegularChannelConfig(ADC1, ADC_Channel_1, 2, ADC_SampleTime_239Cycles5); 31 | 32 | ADC_Cmd (ADC1, ENABLE); 33 | 34 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); 35 | DMA_DeInit(DMA1_Channel1); 36 | 37 | DMA_InitTypeDef DMA_InitStructure; 38 | DMA_StructInit(&DMA_InitStructure); 39 | DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; 40 | DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&ADC1_Buffer; 41 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; 42 | DMA_InitStructure.DMA_BufferSize = 2; 43 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; 44 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; 45 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord; 46 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord; 47 | DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; 48 | DMA_InitStructure.DMA_Priority = DMA_Priority_Low; 49 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; 50 | DMA_Init(DMA1_Channel1, &DMA_InitStructure); 51 | 52 | DMA_Cmd(DMA1_Channel1, ENABLE); 53 | 54 | ADC_DMACmd(ADC1, ENABLE); 55 | 56 | ADC_SoftwareStartConvCmd(ADC1, ENABLE); 57 | } 58 | 59 | void FootpadGuard::Update() { 60 | for (int i = 0; i < 2; i++) { 61 | padLevelFilter[0].compute(ADC1_Buffer[0]); 62 | padLevelFilter[1].compute(ADC1_Buffer[1]); 63 | } 64 | 65 | if (!seen_booth_off) { 66 | // check if both off now 67 | if (padLevelFilter[0].getVal() < settings_->min_level_to_continue && padLevelFilter[1].getVal() < settings_->min_level_to_continue) { 68 | seen_booth_off = true; 69 | } 70 | } 71 | } 72 | 73 | FootpadGuard::FootpadGuard(const Config_FootPadSettings* settings) 74 | : settings_(settings), padLevelFilter {&settings->filter_rc, &settings->filter_rc} { 75 | initADCs(); 76 | } 77 | 78 | bool FootpadGuard::CanStart() { 79 | if (!seen_booth_off) { 80 | // sensor failure or still pressed since start. 81 | return false; 82 | } 83 | return padLevelFilter[0].getVal() > settings_->min_level_to_start && padLevelFilter[1].getVal() > settings_->min_level_to_start; 84 | } 85 | 86 | // Stop if one of the footpads below the threshold for at least shutoff_delay_ms 87 | bool FootpadGuard::MustStop() { 88 | bool stop_condition = padLevelFilter[0].getVal() < settings_->min_level_to_continue || padLevelFilter[1].getVal() < settings_->min_level_to_continue; 89 | 90 | if (!stop_condition) { 91 | stop_requested_ = false; 92 | return false; 93 | } 94 | 95 | if (!stop_requested_) { 96 | stop_requested_ = true; 97 | stop_request_timestamp_ = half_millis(); 98 | } 99 | 100 | return (uint16_t)(half_millis() - stop_request_timestamp_)*2 > settings_->shutoff_delay_ms; 101 | } 102 | -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_crc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_crc.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the CRC firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_crc.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup CRC 30 | * @brief CRC driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup CRC_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup CRC_Private_Defines 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup CRC_Private_Macros 51 | * @{ 52 | */ 53 | 54 | /** 55 | * @} 56 | */ 57 | 58 | /** @defgroup CRC_Private_Variables 59 | * @{ 60 | */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup CRC_Private_FunctionPrototypes 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup CRC_Private_Functions 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @brief Resets the CRC Data register (DR). 80 | * @param None 81 | * @retval None 82 | */ 83 | void CRC_ResetDR(void) 84 | { 85 | /* Reset CRC generator */ 86 | CRC->CR = CRC_CR_RESET; 87 | } 88 | 89 | /** 90 | * @brief Computes the 32-bit CRC of a given data word(32-bit). 91 | * @param Data: data word(32-bit) to compute its CRC 92 | * @retval 32-bit CRC 93 | */ 94 | uint32_t CRC_CalcCRC(uint32_t Data) 95 | { 96 | CRC->DR = Data; 97 | 98 | return (CRC->DR); 99 | } 100 | 101 | /** 102 | * @brief Computes the 32-bit CRC of a given buffer of data word(32-bit). 103 | * @param pBuffer: pointer to the buffer containing the data to be computed 104 | * @param BufferLength: length of the buffer to be computed 105 | * @retval 32-bit CRC 106 | */ 107 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength) 108 | { 109 | uint32_t index = 0; 110 | 111 | for(index = 0; index < BufferLength; index++) 112 | { 113 | CRC->DR = pBuffer[index]; 114 | } 115 | return (CRC->DR); 116 | } 117 | 118 | /** 119 | * @brief Returns the current CRC value. 120 | * @param None 121 | * @retval 32-bit CRC 122 | */ 123 | uint32_t CRC_GetCRC(void) 124 | { 125 | return (CRC->DR); 126 | } 127 | 128 | /** 129 | * @brief Stores a 8-bit data in the Independent Data(ID) register. 130 | * @param IDValue: 8-bit value to be stored in the ID register 131 | * @retval None 132 | */ 133 | void CRC_SetIDRegister(uint8_t IDValue) 134 | { 135 | CRC->IDR = IDValue; 136 | } 137 | 138 | /** 139 | * @brief Returns the 8-bit data stored in the Independent Data(ID) register 140 | * @param None 141 | * @retval 8-bit value of the ID register 142 | */ 143 | uint8_t CRC_GetIDRegister(void) 144 | { 145 | return (CRC->IDR); 146 | } 147 | 148 | /** 149 | * @} 150 | */ 151 | 152 | /** 153 | * @} 154 | */ 155 | 156 | /** 157 | * @} 158 | */ 159 | 160 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 161 | -------------------------------------------------------------------------------- /stm_lib/inc/stm32f10x_iwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_iwdg.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the IWDG 8 | * firmware library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_IWDG_H 25 | #define __STM32F10x_IWDG_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup IWDG 39 | * @{ 40 | */ 41 | 42 | /** @defgroup IWDG_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup IWDG_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup IWDG_WriteAccess 55 | * @{ 56 | */ 57 | 58 | #define IWDG_WriteAccess_Enable ((uint16_t)0x5555) 59 | #define IWDG_WriteAccess_Disable ((uint16_t)0x0000) 60 | #define IS_IWDG_WRITE_ACCESS(ACCESS) (((ACCESS) == IWDG_WriteAccess_Enable) || \ 61 | ((ACCESS) == IWDG_WriteAccess_Disable)) 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup IWDG_prescaler 67 | * @{ 68 | */ 69 | 70 | #define IWDG_Prescaler_4 ((uint8_t)0x00) 71 | #define IWDG_Prescaler_8 ((uint8_t)0x01) 72 | #define IWDG_Prescaler_16 ((uint8_t)0x02) 73 | #define IWDG_Prescaler_32 ((uint8_t)0x03) 74 | #define IWDG_Prescaler_64 ((uint8_t)0x04) 75 | #define IWDG_Prescaler_128 ((uint8_t)0x05) 76 | #define IWDG_Prescaler_256 ((uint8_t)0x06) 77 | #define IS_IWDG_PRESCALER(PRESCALER) (((PRESCALER) == IWDG_Prescaler_4) || \ 78 | ((PRESCALER) == IWDG_Prescaler_8) || \ 79 | ((PRESCALER) == IWDG_Prescaler_16) || \ 80 | ((PRESCALER) == IWDG_Prescaler_32) || \ 81 | ((PRESCALER) == IWDG_Prescaler_64) || \ 82 | ((PRESCALER) == IWDG_Prescaler_128)|| \ 83 | ((PRESCALER) == IWDG_Prescaler_256)) 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup IWDG_Flag 89 | * @{ 90 | */ 91 | 92 | #define IWDG_FLAG_PVU ((uint16_t)0x0001) 93 | #define IWDG_FLAG_RVU ((uint16_t)0x0002) 94 | #define IS_IWDG_FLAG(FLAG) (((FLAG) == IWDG_FLAG_PVU) || ((FLAG) == IWDG_FLAG_RVU)) 95 | #define IS_IWDG_RELOAD(RELOAD) ((RELOAD) <= 0xFFF) 96 | /** 97 | * @} 98 | */ 99 | 100 | /** 101 | * @} 102 | */ 103 | 104 | /** @defgroup IWDG_Exported_Macros 105 | * @{ 106 | */ 107 | 108 | /** 109 | * @} 110 | */ 111 | 112 | /** @defgroup IWDG_Exported_Functions 113 | * @{ 114 | */ 115 | 116 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess); 117 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler); 118 | void IWDG_SetReload(uint16_t Reload); 119 | void IWDG_ReloadCounter(void); 120 | void IWDG_Enable(void); 121 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG); 122 | 123 | #ifdef __cplusplus 124 | } 125 | #endif 126 | 127 | #endif /* __STM32F10x_IWDG_H */ 128 | /** 129 | * @} 130 | */ 131 | 132 | /** 133 | * @} 134 | */ 135 | 136 | /** 137 | * @} 138 | */ 139 | 140 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 141 | -------------------------------------------------------------------------------- /ledController.cpp: -------------------------------------------------------------------------------- 1 | #include "ledController.hpp" 2 | 3 | 4 | 5 | #include "drv/led/led.hpp" 6 | #include "arduino.h" 7 | #include "lpf.hpp" 8 | #include 9 | 10 | #define STOPPED_SPEED 300 // speed at which board is considered stopped 11 | #define TURN_INDICATION_MIN_RATE_DEG_SEC 45 12 | 13 | enum DriveState { 14 | Stopped, // Display battery level? 15 | Fwd, 16 | Reverse 17 | }; 18 | 19 | enum TurnState { 20 | None, 21 | Left, 22 | Rigth 23 | }; 24 | 25 | struct LedState { 26 | DriveState drive_state; 27 | TurnState turn_state; 28 | bool braking = false; 29 | float prev_speed; 30 | float battery_level; 31 | }; 32 | 33 | LedState led_state; 34 | 35 | float rotation_rc = 0.1; 36 | LPF rotation_lpf(&rotation_rc); 37 | 38 | 39 | void led_controller_set_state(float speed, float rotation_rate, float battery_level) { 40 | led_state.braking = (fabsf(speed) < fabsf(led_state.prev_speed) * 0.95); // if speed reduced by 5% or more 41 | led_state.battery_level = battery_level; 42 | 43 | if (abs(speed) < STOPPED_SPEED) { 44 | led_state.drive_state = DriveState::Stopped; 45 | } 46 | else { 47 | led_state.drive_state = speed < 0 ? DriveState::Fwd : DriveState::Reverse; 48 | } 49 | 50 | led_state.prev_speed = speed; 51 | 52 | rotation_rate = rotation_lpf.compute(rotation_rate); 53 | if (rotation_rate > TURN_INDICATION_MIN_RATE_DEG_SEC) { 54 | led_state.turn_state = TurnState::Left; 55 | } 56 | else if (rotation_rate < -TURN_INDICATION_MIN_RATE_DEG_SEC) { 57 | led_state.turn_state = TurnState::Rigth; 58 | } else { 59 | led_state.turn_state = TurnState::None; 60 | } 61 | } 62 | 63 | #define LED_COUNT_SINGLE_SIDE (LED_COUNT/2) 64 | 65 | #define COLOR_YELLOW 0x909F00 66 | 67 | void led_controller_update() { 68 | static uint16_t prev_time = 0; 69 | 70 | uint16_t time = half_millis(); 71 | if (time - prev_time < 80u) { 72 | return; 73 | } 74 | 75 | prev_time = time; 76 | 77 | static int led_idx = 0; 78 | if (led_state.turn_state != TurnState::None ) { 79 | if (led_idx >= LED_COUNT_SINGLE_SIDE) { 80 | for (int i = 0; i < LED_COUNT; i++) { 81 | led_set_color(i, 0); 82 | } 83 | led_idx = 0; 84 | } 85 | else { 86 | if ((led_state.turn_state == TurnState::Left && led_state.drive_state == DriveState::Reverse) 87 | || 88 | (led_state.turn_state == TurnState::Rigth && led_state.drive_state == DriveState::Fwd)) { 89 | led_set_color(led_idx, COLOR_YELLOW); 90 | led_set_color(LED_COUNT_SINGLE_SIDE + led_idx, COLOR_YELLOW); 91 | } 92 | else { 93 | led_set_color(LED_COUNT_SINGLE_SIDE - led_idx - 1, COLOR_YELLOW); 94 | led_set_color(LED_COUNT_SINGLE_SIDE + LED_COUNT_SINGLE_SIDE - led_idx - 1, COLOR_YELLOW); 95 | } 96 | 97 | led_idx++; 98 | } 99 | 100 | return; 101 | } 102 | 103 | if (led_state.drive_state == DriveState::Stopped) { 104 | 105 | int green_led_cnt = led_state.battery_level * LED_COUNT_SINGLE_SIDE; 106 | 107 | for (int i = 0; i < LED_COUNT_SINGLE_SIDE; i++) { 108 | uint32_t color = i < green_led_cnt ? 0xDF0000 : 0x00DF00; 109 | led_set_color(i, color); 110 | led_set_color(LED_COUNT - i - 1, color); 111 | } 112 | return; 113 | } 114 | 115 | led_idx = LED_COUNT_SINGLE_SIDE; 116 | int32_t front_color = led_state.drive_state == DriveState::Fwd ? 0xFFFFFF : 0x00FF00; 117 | int32_t back_color = led_state.drive_state == DriveState::Reverse ? 0xFFFFFF : 0x00FF00; 118 | for (int i = 0; i < LED_COUNT_SINGLE_SIDE; i++) { 119 | led_set_color(i, front_color); 120 | } 121 | for (int i = 0; i < LED_COUNT_SINGLE_SIDE; i++) { 122 | led_set_color(LED_COUNT_SINGLE_SIDE + i, back_color); 123 | } 124 | } 125 | 126 | void led_controller_startup_animation() { 127 | static uint16_t prev_time = 0; 128 | uint16_t time = half_millis(); 129 | if (time - prev_time < 150u) { 130 | return; 131 | } 132 | prev_time = time; 133 | 134 | for (int i = 0; i < LED_COUNT; i++) { 135 | led_set_color(i, 0); 136 | } 137 | 138 | for (int i = 0; i < 4; i++) { 139 | int led = rand() % LED_COUNT; 140 | int clr = rand() % (1 << 4); 141 | led_set_color(led, 142 | ((clr & (1)) ? 0x30 : 0) 143 | | ((clr & (1 << 1)) ? 0x3000 : 0) 144 | | ((clr & (1 << 2)) ? 0x300000 : 0) ); 145 | } 146 | } 147 | 148 | void led_controller_init() { 149 | led_init(); 150 | rotation_rc = 0.12; 151 | 152 | rotation_lpf = LPF(&rotation_rc); 153 | rotation_lpf.reset(); 154 | } 155 | -------------------------------------------------------------------------------- /imu/imu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "imu.hpp" 3 | #include "stm32f10x_gpio.h" 4 | 5 | 6 | #define sq(v) ((v)*(v)) 7 | #define PI 3.141593 8 | 9 | #define fcos(angle) (1 - sq(angle)/2) 10 | #define fsin(angle) (angle) 11 | 12 | #define MW_GYRO_SCALE (4 / 65.5) //MPU6050 and MPU3050 65.5 LSB/(deg/s) and we ignore the last 2 bits 13 | #define COMP_GYRO_SCALE_RAD (MW_GYRO_SCALE * PI / 180.0) 14 | 15 | #define compFilterAccWeight 0.0004 16 | #define compFilterAccWeightCalib 0.005 17 | #define GRAVITY_TOLERANCE 0.15 18 | 19 | #ifdef MADGWICK 20 | 21 | 22 | // Madgwick takes about 220us to compute 23 | void IMU::compute(const MpuUpdate& update, bool init) { 24 | // GPIOA->BSRR = GPIO_Pin_11; 25 | 26 | 27 | if (init) { 28 | // While gyro is getting initialized its data is invalid - ignore gyro. 29 | // Take all data from ACC with much higher weight - assuming board is stationary (otherwise gyro would not calibrate) 30 | mw_.updateIMU(0, 0, 0, update.acc[0] / (float)ACC_1G, update.acc[1] / (float)ACC_1G, update.acc[2] / (float)ACC_1G, 0.8); 31 | } 32 | else { 33 | rates[0] = update.gyro[0] * MW_GYRO_SCALE; 34 | rates[1] = update.gyro[1] * MW_GYRO_SCALE; 35 | rates[2] = update.gyro[2] * MW_GYRO_SCALE; 36 | mw_.updateIMU(rates[0], rates[1], rates[2], update.acc[0] / (float)ACC_1G, update.acc[1] / (float)ACC_1G, update.acc[2] / (float)ACC_1G, config_->balance_settings.imu_beta); 37 | } 38 | 39 | angles[0] = mw_.getRoll() + config_->callibration.x_offset; // TODO: replace with proper vector rotation in MpuUpdate (so pid controler sees rotated gyro input too) 40 | angles[1] = - mw_.getPitch() + config_->callibration.y_offset; 41 | 42 | // GPIOA->BRR = GPIO_Pin_11; 43 | } 44 | 45 | #else 46 | 47 | static void RotateVector(float vector[], float roll, float pitch, float yaw) 48 | { 49 | float cosR = fcos(roll); 50 | float sinR = fsin(roll); 51 | 52 | float cosP = fcos(pitch); 53 | float sinP = fsin(pitch); 54 | 55 | float cosY = fcos(yaw); 56 | float sinY = fsin(yaw); 57 | 58 | float tmpCopy[3]; 59 | 60 | tmpCopy[0] = vector[0] * cosP * cosY + vector[1] * (cosR * sinY + sinR * sinP * cosY) + vector[2] * (sinR * sinY - cosR * sinP * cosY); 61 | tmpCopy[1] = -vector[0] * cosP * sinY + vector[1] * (cosR * cosY - sinR * sinP * sinY) + vector[2] * (sinR * cosY + cosR * sinP * sinY); 62 | tmpCopy[2] = vector[0] * sinP - vector[1] * (sinR * cosP) + vector[2] * (cosR * cosP); 63 | 64 | for (int i = 0; i < 3; i++) { 65 | vector[i] = tmpCopy[i]; 66 | } 67 | } 68 | 69 | float invSqrt(float x) { 70 | float halfx = 0.5f * x; 71 | float y = x; 72 | long i = *(long*)& y; 73 | i = 0x5f3759df - (i >> 1); 74 | y = *(float*)& i; 75 | y = y * (1.5f - (halfx * y * y)); 76 | return y; 77 | } 78 | 79 | void IMU::compute(const MpuUpdate& update, bool init) { 80 | if (init) { 81 | // While gyro is getting initialized its data is invalid - ignore gyro. 82 | // Take all data from ACC with much higher weight - assuming board is stationary (otherwise gyro would not calibrate) 83 | for (int i = 0; i < 3; i++) { 84 | accCompensatedVector_[i] = (1 - compFilterAccWeightCalib) * accCompensatedVector_[i] + compFilterAccWeightCalib * update.acc[i]; 85 | } 86 | return; 87 | } 88 | 89 | const float scale = COMP_GYRO_SCALE_RAD / 1000.0; // TODO: use actual time 90 | RotateVector(accCompensatedVector_, update.gyro[0] * scale, update.gyro[1] * scale, update.gyro[2] * scale); 91 | 92 | int32_t sumAccSq = 0; 93 | for (int i = 0; i < 3; i++) { 94 | sumAccSq += sq((int32_t)update.acc[i]); 95 | } 96 | 97 | float currRatio = atan2(accCompensatedVector_[1], accCompensatedVector_[2]); 98 | float accRatio = atan2(update.acc[1], update.acc[2]); 99 | 100 | if (sumAccSq > sq(ACC_1G * (1 - GRAVITY_TOLERANCE)) && sumAccSq < sq(ACC_1G * (1 + GRAVITY_TOLERANCE)) && fabsf(currRatio - accRatio) < 0.5) { 101 | for (int i = 0; i < 3; i++) { 102 | accCompensatedVector_[i] = (1 - compFilterAccWeight) * accCompensatedVector_[i] + compFilterAccWeight * update.acc[i]; 103 | } 104 | } 105 | 106 | float inv_sqrt = invSqrt(sq(accCompensatedVector_[0]) + sq(accCompensatedVector_[1]) + sq(accCompensatedVector_[2])) * ACC_1G; 107 | accCompensatedVector_[0] *= inv_sqrt; 108 | accCompensatedVector_[1] *= inv_sqrt; 109 | accCompensatedVector_[2] *= inv_sqrt; 110 | 111 | angles[0] = atan2(accCompensatedVector_[1], accCompensatedVector_[2]) * 180 / PI + config_->callibration.x_offset; 112 | angles[1] = atan2(accCompensatedVector_[0], sqrt(sq(accCompensatedVector_[2]) + sq(accCompensatedVector_[1]))) * 180 / PI + config_->callibration.y_offset; 113 | } 114 | 115 | #endif 116 | -------------------------------------------------------------------------------- /drv/comms/config.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto2"; 2 | 3 | message Config { 4 | message Callibration { 5 | required int32 acc_x = 1 [ default = 0 ]; 6 | required int32 acc_y = 2 [ default = 0 ]; 7 | required int32 acc_z = 3 [ default = 0 ]; 8 | 9 | optional float x_offset = 4 [ default = 0 ]; 10 | optional float y_offset = 5 [ default = 0 ]; 11 | optional float z_offset = 6 [ default = 0 ]; 12 | } 13 | 14 | optional Callibration callibration = 1; 15 | 16 | message PidConfig { 17 | // leaning angle from 0 to balance_angle_scaling maps 0 to 100% throttle. 18 | // 0-1 = 0-100% 19 | required float p = 1 [ default = 1 ]; 20 | // leaning rate in deg/sec is multiplied by d 21 | required float d = 2 [ default = 0.005 ]; 22 | required float i = 3 [ default = 0.001 ]; 23 | required float max_i = 4 [ default = 0.006 ]; 24 | optional float i_expo = 13 [ default = 0 ]; 25 | } 26 | 27 | required PidConfig rate_pid = 9; 28 | 29 | required PidConfig angle_pid = 2; 30 | 31 | message FootPadSettings { 32 | required float filter_rc = 1 [ default = 0.05 ]; 33 | required int32 min_level_to_start = 2 [ default = 3300 ]; 34 | required int32 min_level_to_continue = 3 [ default = 2000 ]; 35 | required int32 shutoff_delay_ms = 4 [ default = 100 ]; 36 | } 37 | 38 | required FootPadSettings foot_pad = 3; 39 | 40 | message BalancingConfig { 41 | // min value is 1 = linear 42 | required float balance_expo = 1 [ default = 0.15 ]; 43 | 44 | // scale input angel in deg by deviding by 'balance_angle_scaling' 45 | required float balance_angle_scaling = 2 [ default = 15 ]; 46 | 47 | required int32 max_start_angle_steer = 3 [ default = 15 ]; 48 | required int32 shutoff_angle_steer = 4 [ default = 40 ]; 49 | required int32 shutoff_angle_drive = 5 [ default = 14 ]; 50 | 51 | required int32 max_update_limiter = 6 [ default = 300 ]; 52 | required float output_lpf_hz = 7 [ default = 100 ]; 53 | 54 | // Max rotation rate for D param is limited to 100 deg/sec by default 55 | required int32 balance_d_param_limiter = 8 [ default = 100 ]; 56 | 57 | required float balance_d_param_lpf_hz = 9 [ default = 50 ]; 58 | 59 | // MPU6050_LPF_256HZ 0, MPU6050_LPF_188HZ 1, MPU6050_LPF_98HZ 2, 60 | // MPU6050_LPF_42HZ 3, MPU6050_LPF_20HZ 4, MPU6050_LPF_10HZ 5, 61 | // MPU6050_LPF_5HZ 6 Reboot required to change 62 | required uint32 global_gyro_lpf = 10 [ default = 2 ]; 63 | 64 | optional float imu_beta = 11 [ default = 0.02 ]; 65 | 66 | // 0 - exponential, 1 natural, 2 - poly 67 | optional int32 expo_type = 12 [ default = 0 ]; 68 | 69 | // If non-zero, balance controler sends current control requests via usart 70 | // (keep PPM wire disconnected.) 71 | optional float usart_control_scaling = 13 [ default = 0 ]; 72 | } 73 | 74 | required BalancingConfig balance_settings = 4; 75 | 76 | message PusbackSettings { 77 | required int32 min_speed_erpm = 1 [ default = 100000 ]; 78 | reserved 2; 79 | 80 | required float push_raise_speed_deg_sec = 3 [ default = 0.5 ]; 81 | required float push_release_speed_deg_sec = 4 [ default = 0.2 ]; 82 | optional float push_angle = 5 [ default = 5 ]; 83 | } 84 | 85 | message Misc { 86 | // 0-1, 1 = 100% of output power = no warning at all 87 | required float throttle_threshold = 7 [ default = 0.75 ]; 88 | required float throttle_rc = 6 [ default = 0.05 ]; 89 | 90 | // Get pushback/beep if duty_cycle exceeds this threshold 91 | optional float duty_threshold = 8 [ default = 0.75 ]; 92 | optional float duty_rc = 9 [ default = 0.25 ]; 93 | 94 | // Get pushback/beep if erpm exceeds this threshold 95 | optional int32 erpm_threshold = 10 [ default = 6000 ]; 96 | optional float erpm_rc = 11 [ default = 0.25 ]; 97 | 98 | // Get pushback/beep if voltage drops below this threshold 99 | optional float low_volt_threshold = 12 [ default = 45 ]; 100 | optional float volt_rc = 13 [ default = 0.25 ]; 101 | 102 | reserved 14; 103 | 104 | // length of 1 electrical rotation 105 | optional float erpm_to_dist_const = 15 [ default = 1 ]; 106 | 107 | optional int32 batt_cells = 16 [ default = 1 ]; 108 | } 109 | 110 | required Misc misc = 5; 111 | optional PusbackSettings pushback = 6; 112 | 113 | message LoadLift { 114 | required int32 start_current = 1 [ default = 10 ]; 115 | required float max_angle = 2 [ default = 5]; 116 | required float filter_rc = 3 [ default = 0.05 ]; 117 | required float multiplier = 4 [ default = 0.1 ]; // degrees = current*multiplier 118 | required float ramp_deg_sec = 5 [ default = 0.5 ]; // deg per sec. 119 | } 120 | 121 | optional LoadLift load_lift = 7; 122 | } 123 | 124 | message Stats { 125 | required float batt_voltage = 1; 126 | required float cell_voltage = 14; 127 | required float batt_current = 2; 128 | required float motor_current = 3; 129 | required float speed = 5; 130 | required float distance_traveled = 6; 131 | required float drive_angle = 7; 132 | required float steer_angle = 8; 133 | 134 | required uint32 pad_pressure1 = 9; 135 | required uint32 pad_pressure2 = 10; 136 | 137 | required float motor_duty = 11; 138 | required float esc_temp = 12; 139 | required float motor_temp = 13; 140 | } -------------------------------------------------------------------------------- /imu/MadgwickAHRS.cpp: -------------------------------------------------------------------------------- 1 | //============================================================================================= 2 | // MadgwickAHRS.c 3 | //============================================================================================= 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/open-source-imu-and-ahrs-algorithms/ 7 | // 8 | // From the x-io website "Open-source resources available on this website are 9 | // provided under the GNU General Public Licence unless an alternative licence 10 | // is provided in source." 11 | // 12 | // Date Author Notes 13 | // 29/09/2011 SOH Madgwick Initial release 14 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 15 | // 19/02/2012 SOH Madgwick Magnetometer measurement is normalised 16 | // 17 | //============================================================================================= 18 | 19 | //------------------------------------------------------------------------------------------- 20 | // Header files 21 | 22 | #include "MadgwickAHRS.hpp" 23 | #include 24 | 25 | //------------------------------------------------------------------------------------------- 26 | // Definitions 27 | 28 | #define sampleFreqDef 1000.0f // sample frequency in Hz 29 | #define betaDef 0.02f // 2 * proportional gain 30 | 31 | 32 | //============================================================================================ 33 | // Functions 34 | 35 | //------------------------------------------------------------------------------------------- 36 | // AHRS algorithm update 37 | 38 | Madgwick::Madgwick(){ 39 | q0 = 1.0f; 40 | q1 = 0.0f; 41 | q2 = 0.0f; 42 | q3 = 0.0f; 43 | invSampleFreq = 1.0f / sampleFreqDef; 44 | anglesComputed = 0; 45 | } 46 | 47 | 48 | //------------------------------------------------------------------------------------------- 49 | // IMU algorithm update 50 | 51 | void Madgwick::updateIMU(float gx, float gy, float gz, float ax, float ay, float az, float beta) { 52 | float recipNorm; 53 | float s0, s1, s2, s3; 54 | float qDot1, qDot2, qDot3, qDot4; 55 | float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3; 56 | 57 | // Convert gyroscope degrees/sec to radians/sec 58 | gx *= 0.0174533f; 59 | gy *= 0.0174533f; 60 | gz *= 0.0174533f; 61 | 62 | // Rate of change of quaternion from gyroscope 63 | qDot1 = 0.5f * (-q1 * gx - q2 * gy - q3 * gz); 64 | qDot2 = 0.5f * (q0 * gx + q2 * gz - q3 * gy); 65 | qDot3 = 0.5f * (q0 * gy - q1 * gz + q3 * gx); 66 | qDot4 = 0.5f * (q0 * gz + q1 * gy - q2 * gx); 67 | 68 | // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation) 69 | if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { 70 | 71 | // Normalise accelerometer measurement 72 | recipNorm = invSqrt(ax * ax + ay * ay + az * az); 73 | ax *= recipNorm; 74 | ay *= recipNorm; 75 | az *= recipNorm; 76 | 77 | // Auxiliary variables to avoid repeated arithmetic 78 | _2q0 = 2.0f * q0; 79 | _2q1 = 2.0f * q1; 80 | _2q2 = 2.0f * q2; 81 | _2q3 = 2.0f * q3; 82 | _4q0 = 4.0f * q0; 83 | _4q1 = 4.0f * q1; 84 | _4q2 = 4.0f * q2; 85 | _8q1 = 8.0f * q1; 86 | _8q2 = 8.0f * q2; 87 | q0q0 = q0 * q0; 88 | q1q1 = q1 * q1; 89 | q2q2 = q2 * q2; 90 | q3q3 = q3 * q3; 91 | 92 | // Gradient decent algorithm corrective step 93 | s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; 94 | s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q1 - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; 95 | s2 = 4.0f * q0q0 * q2 + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; 96 | s3 = 4.0f * q1q1 * q3 - _2q1 * ax + 4.0f * q2q2 * q3 - _2q2 * ay; 97 | recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude 98 | s0 *= recipNorm; 99 | s1 *= recipNorm; 100 | s2 *= recipNorm; 101 | s3 *= recipNorm; 102 | 103 | // Apply feedback step 104 | qDot1 -= beta * s0; 105 | qDot2 -= beta * s1; 106 | qDot3 -= beta * s2; 107 | qDot4 -= beta * s3; 108 | } 109 | 110 | // Integrate rate of change of quaternion to yield quaternion 111 | q0 += qDot1 * invSampleFreq; 112 | q1 += qDot2 * invSampleFreq; 113 | q2 += qDot3 * invSampleFreq; 114 | q3 += qDot4 * invSampleFreq; 115 | 116 | // Normalise quaternion 117 | recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); 118 | q0 *= recipNorm; 119 | q1 *= recipNorm; 120 | q2 *= recipNorm; 121 | q3 *= recipNorm; 122 | anglesComputed = 0; 123 | } 124 | 125 | //------------------------------------------------------------------------------------------- 126 | // Fast inverse square-root 127 | // See: http://en.wikipedia.org/wiki/Fast_inverse_square_root 128 | 129 | float Madgwick::invSqrt(float x) { 130 | float halfx = 0.5f * x; 131 | float y = x; 132 | long i = *(long*)& y; 133 | i = 0x5f3759df - (i >> 1); 134 | y = *(float*)& i; 135 | y = y * (1.5f - (halfx * y * y)); 136 | y = y * (1.5f - (halfx * y * y)); 137 | return y; 138 | } 139 | 140 | //------------------------------------------------------------------------------------------- 141 | 142 | void Madgwick::computeAngles() 143 | { 144 | roll = atan2f(q0 * q1 + q2 * q3, 0.5f - q1 * q1 - q2 * q2); 145 | pitch = asinf(-2.0f * (q1 * q3 - q0 * q2)); 146 | yaw = atan2f(q1 * q2 + q0 * q3, 0.5f - q2 * q2 - q3 * q3); 147 | anglesComputed = 1; 148 | } 149 | -------------------------------------------------------------------------------- /io/i2c.cpp: -------------------------------------------------------------------------------- 1 | #include "stm32f10x.h" 2 | #include "stm32f10x_gpio.h" 3 | #include "stm32f10x_dma.h" 4 | #include "stm32f10x_rcc.h" 5 | #include "stm32f10x_i2c.h" 6 | #include "misc.h" 7 | #include "arduino.h" 8 | 9 | 10 | #define PinSCL GPIO_Pin_10 11 | #define PinSDA GPIO_Pin_11 12 | 13 | // TODO: try replacing all while waits with interrupts 14 | 15 | void i2c_init() { 16 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_I2C2, ENABLE); 17 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | RCC_APB2Periph_AFIO, ENABLE); 18 | 19 | /* GPIO configuration, drive pins manually at first to get i2c unstuck */ 20 | GPIO_WriteBit(GPIOB, PinSCL, BitAction::Bit_SET); 21 | GPIO_InitTypeDef GPIO_InitStructure; 22 | GPIO_InitStructure.GPIO_Pin = PinSCL; 23 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 24 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD; 25 | GPIO_Init(GPIOB, &GPIO_InitStructure); 26 | 27 | GPIO_InitStructure.GPIO_Pin = PinSDA; 28 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 29 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 30 | GPIO_Init(GPIOB, &GPIO_InitStructure); 31 | 32 | while (!GPIO_ReadInputDataBit(GPIOB, PinSDA)) { 33 | GPIO_WriteBit(GPIOB, PinSCL, BitAction::Bit_RESET); 34 | delay(2); 35 | GPIO_WriteBit(GPIOB, PinSCL, BitAction::Bit_SET); 36 | delay(2); 37 | } 38 | 39 | 40 | /* GPIO configuration for I2C */ 41 | GPIO_InitStructure.GPIO_Pin = PinSCL | PinSDA; 42 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 43 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD; 44 | GPIO_Init(GPIOB, &GPIO_InitStructure); 45 | 46 | delay(2); 47 | 48 | I2C_DeInit(I2C2); 49 | delay(2); 50 | I2C_Cmd(I2C2, ENABLE); 51 | delay(2); 52 | 53 | /* I2C configuration */ 54 | I2C_InitTypeDef I2C_InitStructure; 55 | I2C_StructInit(&I2C_InitStructure); 56 | 57 | I2C_InitStructure.I2C_Mode = I2C_Mode_I2C; 58 | I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2; 59 | I2C_InitStructure.I2C_OwnAddress1 = 0x4A; 60 | I2C_InitStructure.I2C_Ack = I2C_Ack_Enable; 61 | I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit; 62 | I2C_InitStructure.I2C_ClockSpeed = 400000; 63 | 64 | I2C_Init(I2C2, &I2C_InitStructure); 65 | I2C_Cmd(I2C2, ENABLE); 66 | } 67 | 68 | void start() { 69 | I2C_GenerateSTART(I2C2, ENABLE); 70 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_MODE_SELECT)); 71 | } 72 | 73 | void sendAddr(uint8_t hwAddr, uint8_t tx) { 74 | if (tx) { 75 | I2C_Send7bitAddress(I2C2, hwAddr << 1, I2C_Direction_Transmitter); 76 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_TRANSMITTER_MODE_SELECTED)); 77 | } 78 | else { 79 | I2C_Send7bitAddress(I2C2, hwAddr << 1, I2C_Direction_Receiver); 80 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_RECEIVER_MODE_SELECTED)); 81 | } 82 | } 83 | 84 | void sendData(uint8_t data) { 85 | I2C_SendData(I2C2, data); 86 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); 87 | } 88 | 89 | void stop() { 90 | I2C_GenerateSTOP(I2C2, ENABLE); 91 | while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY)); 92 | } 93 | 94 | 95 | 96 | uint8_t i2c_readReg(uint8_t hwAddr, uint8_t rAddr) { 97 | start(); 98 | sendAddr(hwAddr, 1); 99 | sendData(rAddr); 100 | 101 | I2C_AcknowledgeConfig(I2C2, ENABLE); 102 | start(); 103 | sendAddr(hwAddr, 0); 104 | 105 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)); 106 | uint8_t data = I2C_ReceiveData(I2C2); 107 | 108 | I2C_AcknowledgeConfig(I2C2, DISABLE); 109 | 110 | stop(); 111 | return data; 112 | } 113 | 114 | void i2c_read_reg_to_buf(uint8_t hwAddr, uint8_t rAddr, uint8_t* buf, uint8_t size) { 115 | start(); 116 | 117 | sendAddr(hwAddr, 1); 118 | 119 | I2C_SendData(I2C2, rAddr); 120 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_TRANSMITTED)); 121 | 122 | start(); 123 | 124 | sendAddr(hwAddr, 0); 125 | 126 | I2C_AcknowledgeConfig(I2C2, ENABLE); 127 | 128 | for (int i = 0; i < size; i++) { 129 | while(!I2C_CheckEvent(I2C2, I2C_EVENT_MASTER_BYTE_RECEIVED)); 130 | buf[i] = I2C_ReceiveData(I2C2); 131 | 132 | if (i >= size - 2) 133 | I2C_AcknowledgeConfig(I2C2, DISABLE); 134 | } 135 | 136 | stop(); 137 | } 138 | 139 | void i2c_writeReg(uint8_t hwAddr, uint8_t wAddr, uint8_t value) { 140 | start(); 141 | sendAddr(hwAddr, 1); 142 | 143 | sendData(wAddr); 144 | sendData(value); 145 | 146 | stop(); 147 | } 148 | 149 | void i2c_DmaRead(uint8_t hwAddr, uint8_t rAddr, DMA_Channel_TypeDef* DMAy_Channelx, uint16_t dataNumber) // 72 us 150 | { 151 | // GPIOA->BSRR = GPIO_Pin_11; 152 | 153 | DMA_Cmd(DMAy_Channelx, DISABLE); 154 | /* Set current data number again to 14 for MPu6050, only possible after disabling the DMA channel */ 155 | DMA_SetCurrDataCounter(DMAy_Channelx, dataNumber); 156 | DMA_Cmd(DMAy_Channelx, ENABLE); 157 | 158 | // /* While the bus is busy */ 159 | // while(I2C_GetFlagStatus(I2C2, I2C_FLAG_BUSY)); 160 | 161 | /* Enable DMA NACK automatic generation */ 162 | I2C_DMALastTransferCmd(I2C2, ENABLE); //Note this one, very important 163 | 164 | start(); 165 | sendAddr(hwAddr, 1); 166 | sendData(rAddr); 167 | 168 | start(); 169 | sendAddr(hwAddr, 0); 170 | 171 | /* Start DMA to receive data from I2C */ 172 | I2C_DMACmd(I2C2, ENABLE); 173 | 174 | //GPIOA->BRR = GPIO_Pin_11; 175 | 176 | // When the data transmission is complete, it will automatically jump to DMA interrupt routine to finish the rest. 177 | } 178 | -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_iwdg.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_iwdg.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the IWDG firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_iwdg.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup IWDG 30 | * @brief IWDG driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup IWDG_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup IWDG_Private_Defines 43 | * @{ 44 | */ 45 | 46 | /* ---------------------- IWDG registers bit mask ----------------------------*/ 47 | 48 | /* KR register bit mask */ 49 | #define KR_KEY_Reload ((uint16_t)0xAAAA) 50 | #define KR_KEY_Enable ((uint16_t)0xCCCC) 51 | 52 | /** 53 | * @} 54 | */ 55 | 56 | /** @defgroup IWDG_Private_Macros 57 | * @{ 58 | */ 59 | 60 | /** 61 | * @} 62 | */ 63 | 64 | /** @defgroup IWDG_Private_Variables 65 | * @{ 66 | */ 67 | 68 | /** 69 | * @} 70 | */ 71 | 72 | /** @defgroup IWDG_Private_FunctionPrototypes 73 | * @{ 74 | */ 75 | 76 | /** 77 | * @} 78 | */ 79 | 80 | /** @defgroup IWDG_Private_Functions 81 | * @{ 82 | */ 83 | 84 | /** 85 | * @brief Enables or disables write access to IWDG_PR and IWDG_RLR registers. 86 | * @param IWDG_WriteAccess: new state of write access to IWDG_PR and IWDG_RLR registers. 87 | * This parameter can be one of the following values: 88 | * @arg IWDG_WriteAccess_Enable: Enable write access to IWDG_PR and IWDG_RLR registers 89 | * @arg IWDG_WriteAccess_Disable: Disable write access to IWDG_PR and IWDG_RLR registers 90 | * @retval None 91 | */ 92 | void IWDG_WriteAccessCmd(uint16_t IWDG_WriteAccess) 93 | { 94 | /* Check the parameters */ 95 | assert_param(IS_IWDG_WRITE_ACCESS(IWDG_WriteAccess)); 96 | IWDG->KR = IWDG_WriteAccess; 97 | } 98 | 99 | /** 100 | * @brief Sets IWDG Prescaler value. 101 | * @param IWDG_Prescaler: specifies the IWDG Prescaler value. 102 | * This parameter can be one of the following values: 103 | * @arg IWDG_Prescaler_4: IWDG prescaler set to 4 104 | * @arg IWDG_Prescaler_8: IWDG prescaler set to 8 105 | * @arg IWDG_Prescaler_16: IWDG prescaler set to 16 106 | * @arg IWDG_Prescaler_32: IWDG prescaler set to 32 107 | * @arg IWDG_Prescaler_64: IWDG prescaler set to 64 108 | * @arg IWDG_Prescaler_128: IWDG prescaler set to 128 109 | * @arg IWDG_Prescaler_256: IWDG prescaler set to 256 110 | * @retval None 111 | */ 112 | void IWDG_SetPrescaler(uint8_t IWDG_Prescaler) 113 | { 114 | /* Check the parameters */ 115 | assert_param(IS_IWDG_PRESCALER(IWDG_Prescaler)); 116 | IWDG->PR = IWDG_Prescaler; 117 | } 118 | 119 | /** 120 | * @brief Sets IWDG Reload value. 121 | * @param Reload: specifies the IWDG Reload value. 122 | * This parameter must be a number between 0 and 0x0FFF. 123 | * @retval None 124 | */ 125 | void IWDG_SetReload(uint16_t Reload) 126 | { 127 | /* Check the parameters */ 128 | assert_param(IS_IWDG_RELOAD(Reload)); 129 | IWDG->RLR = Reload; 130 | } 131 | 132 | /** 133 | * @brief Reloads IWDG counter with value defined in the reload register 134 | * (write access to IWDG_PR and IWDG_RLR registers disabled). 135 | * @param None 136 | * @retval None 137 | */ 138 | void IWDG_ReloadCounter(void) 139 | { 140 | IWDG->KR = KR_KEY_Reload; 141 | } 142 | 143 | /** 144 | * @brief Enables IWDG (write access to IWDG_PR and IWDG_RLR registers disabled). 145 | * @param None 146 | * @retval None 147 | */ 148 | void IWDG_Enable(void) 149 | { 150 | IWDG->KR = KR_KEY_Enable; 151 | } 152 | 153 | /** 154 | * @brief Checks whether the specified IWDG flag is set or not. 155 | * @param IWDG_FLAG: specifies the flag to check. 156 | * This parameter can be one of the following values: 157 | * @arg IWDG_FLAG_PVU: Prescaler Value Update on going 158 | * @arg IWDG_FLAG_RVU: Reload Value Update on going 159 | * @retval The new state of IWDG_FLAG (SET or RESET). 160 | */ 161 | FlagStatus IWDG_GetFlagStatus(uint16_t IWDG_FLAG) 162 | { 163 | FlagStatus bitstatus = RESET; 164 | /* Check the parameters */ 165 | assert_param(IS_IWDG_FLAG(IWDG_FLAG)); 166 | if ((IWDG->SR & IWDG_FLAG) != (uint32_t)RESET) 167 | { 168 | bitstatus = SET; 169 | } 170 | else 171 | { 172 | bitstatus = RESET; 173 | } 174 | /* Return the flag status */ 175 | return bitstatus; 176 | } 177 | 178 | /** 179 | * @} 180 | */ 181 | 182 | /** 183 | * @} 184 | */ 185 | 186 | /** 187 | * @} 188 | */ 189 | 190 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 191 | -------------------------------------------------------------------------------- /io/usart.cpp: -------------------------------------------------------------------------------- 1 | #include "usart.hpp" 2 | #include "stm32f10x_rcc.h" 3 | #include "stm32f10x_gpio.h" 4 | #include "stm32f10x_usart.h" 5 | #include "misc.h" 6 | 7 | void init(uint16_t rx_pin, uint16_t tx_pin, uint32_t baud, USART_TypeDef* usart, uint8_t IRQ_Channel) { 8 | GPIO_InitTypeDef GPIO_InitStructure; 9 | USART_InitTypeDef USART_InitStructure; 10 | 11 | /* Enable GPIO clock */ 12 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA | RCC_APB2Periph_AFIO, ENABLE); 13 | 14 | /* Configure USART Tx alternate function */ 15 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz; 16 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 17 | 18 | GPIO_InitStructure.GPIO_Pin = tx_pin; 19 | GPIO_Init(GPIOA, &GPIO_InitStructure); 20 | 21 | // RX 22 | GPIO_InitStructure.GPIO_Pin = rx_pin; 23 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 24 | GPIO_Init(GPIOA, &GPIO_InitStructure); 25 | 26 | USART_StructInit(&USART_InitStructure); 27 | USART_InitStructure.USART_BaudRate = baud; 28 | USART_InitStructure.USART_WordLength = USART_WordLength_8b; 29 | USART_InitStructure.USART_StopBits = USART_StopBits_1; 30 | USART_InitStructure.USART_Parity = USART_Parity_No; 31 | USART_InitStructure.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 32 | USART_InitStructure.USART_Mode = USART_Mode_Rx | USART_Mode_Tx; 33 | 34 | /* USART configuration */ 35 | USART_Init(usart, &USART_InitStructure); 36 | 37 | /* Enable USART */ 38 | USART_Cmd(usart, ENABLE); 39 | 40 | 41 | NVIC_InitTypeDef NVIC_InitStructure; 42 | NVIC_InitStructure.NVIC_IRQChannel = IRQ_Channel; 43 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0; 44 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; 45 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 46 | NVIC_Init(&NVIC_InitStructure); 47 | 48 | USART_ITConfig(usart, USART_IT_RXNE, ENABLE); 49 | USART_ITConfig(usart, USART_IT_ORE, ENABLE); 50 | } 51 | 52 | bool Usart::Init(USART_TypeDef * device, uint32_t baud) { 53 | device_ = device; 54 | rxEmpty = true; 55 | rxBufferReadIdx = 0; 56 | rxBufferWriteIdx = 0; 57 | 58 | txBufferReadIdx = 0; 59 | txBufferWriteIdx = 0; 60 | txStarted = false; 61 | 62 | if (device_ == USART1) { 63 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE); 64 | init(GPIO_Pin_10, GPIO_Pin_9, baud, USART1, USART1_IRQn); 65 | return true; 66 | } 67 | 68 | if (device_ == USART2) { 69 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART2, ENABLE); 70 | init(GPIO_Pin_3, GPIO_Pin_2, baud, USART2, USART2_IRQn); 71 | return true; 72 | } 73 | 74 | return false; 75 | } 76 | 77 | int32_t Usart::Read(uint8_t* data, int32_t max_size) { 78 | if (rxEmpty) 79 | return 0; 80 | 81 | int bytes_available = (RX_BUFFER_SIZE + rxBufferWriteIdx - rxBufferReadIdx) % RX_BUFFER_SIZE; 82 | int bytes_to_copy = min(max_size, bytes_available); 83 | for (int i = 0; i < bytes_to_copy; i++) { 84 | data[i] = rxBuffer[rxBufferReadIdx++]; 85 | if (rxBufferReadIdx >= RX_BUFFER_SIZE) 86 | rxBufferReadIdx = 0; 87 | } 88 | if (rxBufferReadIdx == rxBufferWriteIdx) 89 | rxEmpty = true; 90 | 91 | return bytes_to_copy; 92 | } 93 | 94 | bool Usart::HasData() { 95 | return !rxEmpty; 96 | } 97 | 98 | void Usart::SendCurrentByteFromBuffer() { 99 | USART_SendData(device_, txBuffer[txBufferReadIdx++]); 100 | if (txBufferReadIdx >= TX_BUFFER_SIZE) { 101 | txBufferReadIdx = 0; 102 | } 103 | } 104 | 105 | int32_t Usart::TxBufferFreeCapacity() { 106 | int32_t currently_buffered = txBufferWriteIdx - txBufferReadIdx; 107 | if (currently_buffered < 0) { 108 | currently_buffered += TX_BUFFER_SIZE; 109 | } 110 | 111 | return TX_BUFFER_SIZE - 1 - currently_buffered; 112 | } 113 | 114 | // TODO: this function is not thread safe, but works OK in practice :) 115 | 116 | // TODO: Implement block TX, such that caller blocks is if there is no room in the buffer left and waits until all data is buffered. 117 | int32_t Usart::Send(const uint8_t* data, int32_t size) { 118 | int32_t tx_available = TxBufferFreeCapacity(); 119 | 120 | if (size > tx_available) { 121 | size = tx_available; 122 | } 123 | 124 | for (int i = 0; i < size; i++) { 125 | txBuffer[txBufferWriteIdx] = data[i]; 126 | 127 | if (txBufferWriteIdx < TX_BUFFER_SIZE - 1) 128 | txBufferWriteIdx++; 129 | else 130 | txBufferWriteIdx = 0; 131 | } 132 | 133 | if (!txStarted) { 134 | txStarted = 1; 135 | 136 | SendCurrentByteFromBuffer(); 137 | USART_ITConfig(device_, USART_IT_TXE, ENABLE); 138 | } 139 | 140 | return size; 141 | } 142 | 143 | void Usart::SendWithWait(const uint8_t* data, int32_t size) { 144 | while (size > 0) { 145 | int32_t sent = Send(data, size); 146 | if (sent < 64) { 147 | delay(5); // wait for 10ms to free up the buffer. 148 | } 149 | data += sent; 150 | size -= sent; 151 | } 152 | } 153 | 154 | 155 | void Usart::handleIRQ(){ 156 | if(USART_GetITStatus(device_, USART_IT_TXE) != RESET) { 157 | if (txBufferReadIdx != txBufferWriteIdx) { 158 | SendCurrentByteFromBuffer(); 159 | } 160 | else { 161 | USART_ITConfig(device_, USART_IT_TXE, DISABLE); 162 | txStarted = 0; 163 | } 164 | } 165 | if(USART_GetITStatus(device_, USART_IT_RXNE) != RESET) { 166 | rxBuffer[rxBufferWriteIdx++] = USART_ReceiveData(device_); 167 | rxEmpty = false; 168 | if (rxBufferWriteIdx >= RX_BUFFER_SIZE) { 169 | rxBufferWriteIdx = 0; 170 | } 171 | } 172 | if(USART_GetITStatus(device_, USART_IT_ORE) != RESET) { 173 | device_->SR; 174 | USART_ReceiveData(device_); 175 | } 176 | } 177 | 178 | extern "C" void USART1_IRQHandler(void) { 179 | Serial1.handleIRQ(); 180 | } 181 | 182 | extern "C" void USART2_IRQHandler(void) { 183 | Serial2.handleIRQ(); 184 | } 185 | 186 | Usart Serial1(USART1); 187 | Usart Serial2(USART2); 188 | 189 | -------------------------------------------------------------------------------- /drv/vesc/vesc.cpp: -------------------------------------------------------------------------------- 1 | #include "vesc.hpp" 2 | #include "crc.h" 3 | #include 4 | 5 | 6 | void VescComm::sendRequest(const uint8_t* payload, int payload_len) { 7 | uint16_t crc_payload = crc16(payload, payload_len); 8 | 9 | if (payload_len <= 256) { 10 | uint8_t header[] = {0x2, (uint8_t)payload_len}; 11 | serial_->Send(header, sizeof(header)); 12 | } 13 | else { 14 | uint8_t header[] = {0x3, (uint8_t)(payload_len >> 8), (uint8_t)payload_len}; 15 | serial_->Send(header, sizeof(header)); 16 | } 17 | 18 | serial_->Send(payload, payload_len); 19 | uint8_t footer[] = {(uint8_t)(crc_payload >> 8), (uint8_t)crc_payload, 0x3}; 20 | serial_->Send(footer, sizeof(footer)); 21 | } 22 | 23 | void buffer_append_int32(uint8_t* buffer, int32_t number, int32_t *index) { 24 | buffer[(*index)++] = number >> 24; 25 | buffer[(*index)++] = number >> 16; 26 | buffer[(*index)++] = number >> 8; 27 | buffer[(*index)++] = number; 28 | } 29 | 30 | void buffer_append_float32(uint8_t* buffer, float number, float scale, int32_t *index) { 31 | buffer_append_int32(buffer, (int32_t)(number * scale), index); 32 | } 33 | 34 | void VescComm::requestStats() { 35 | uint8_t request[] = { (uint8_t)COMM_PACKET_ID::COMM_GET_VALUES}; 36 | sendRequest(request , sizeof(request)); 37 | } 38 | 39 | void VescComm::setCurrent(float current) { 40 | uint8_t request[] = { (uint8_t)COMM_PACKET_ID::COMM_SET_CURRENT, 0, 0, 0, 0}; 41 | int32_t send_index = 1; 42 | buffer_append_float32(request, current, 1000.0, &send_index); 43 | sendRequest(request , sizeof(request)); 44 | } 45 | 46 | void VescComm::setCurrentBrake(float current) { 47 | uint8_t request[] = { (uint8_t)COMM_PACKET_ID::COMM_SET_CURRENT_BRAKE, 0, 0, 0, 0}; 48 | int32_t send_index = 1; 49 | buffer_append_float32(request, current, 1000.0, &send_index); 50 | sendRequest(request , sizeof(request)); 51 | } 52 | 53 | #define kMsgTimeoutMsDiv2 100u 54 | #define kHeaderSize 3 55 | #define kFooterSize 3 56 | 57 | 58 | static int16_t buffer_get_int16(const uint8_t *buffer, int32_t *index) { 59 | int16_t res = ((uint16_t) buffer[*index]) << 8 | 60 | ((uint16_t) buffer[*index + 1]); 61 | *index += 2; 62 | return res; 63 | } 64 | 65 | static uint16_t buffer_get_uint16(const uint8_t *buffer, int32_t *index) { 66 | uint16_t res = ((uint16_t) buffer[*index]) << 8 | 67 | ((uint16_t) buffer[*index + 1]); 68 | *index += 2; 69 | return res; 70 | } 71 | 72 | static int32_t buffer_get_int32(const uint8_t *buffer, int32_t *index) { 73 | int32_t res = ((uint32_t) buffer[*index]) << 24 | 74 | ((uint32_t) buffer[*index + 1]) << 16 | 75 | ((uint32_t) buffer[*index + 2]) << 8 | 76 | ((uint32_t) buffer[*index + 3]); 77 | *index += 4; 78 | return res; 79 | } 80 | 81 | static uint32_t buffer_get_uint32(const uint8_t *buffer, int32_t *index) { 82 | uint32_t res = ((uint32_t) buffer[*index]) << 24 | 83 | ((uint32_t) buffer[*index + 1]) << 16 | 84 | ((uint32_t) buffer[*index + 2]) << 8 | 85 | ((uint32_t) buffer[*index + 3]); 86 | *index += 4; 87 | return res; 88 | } 89 | 90 | static float buffer_get_float16(const uint8_t *buffer, float scale, int32_t *index) { 91 | return (float)buffer_get_int16(buffer, index) / scale; 92 | } 93 | 94 | static float buffer_get_float32(const uint8_t *buffer, float scale, int32_t *index) { 95 | return (float)buffer_get_int32(buffer, index) / scale; 96 | } 97 | 98 | 99 | int VescComm::update() { 100 | if (!serial_->HasData()) 101 | return 0; 102 | 103 | uint16_t time = half_millis(); 104 | if ((uint16_t)(time - last_uart_data_time_) > kMsgTimeoutMsDiv2) { 105 | buffer_pos_ = 0; 106 | } 107 | 108 | last_uart_data_time_ = time; 109 | 110 | int32_t received_bytes = serial_->Read(rx_data_ + buffer_pos_, sizeof(rx_data_) - buffer_pos_); 111 | buffer_pos_+= received_bytes; 112 | if (buffer_pos_ > kHeaderSize) { 113 | if (buffer_pos_ >= sizeof(rx_data_)) { 114 | buffer_pos_ = 0; // too long/invalid 115 | } 116 | 117 | if (buffer_pos_ >= actual_header_size() + expected_msg_len() + kFooterSize) { 118 | uint16_t crc_payload = crc16(rx_data_ + actual_header_size(), expected_msg_len()); 119 | 120 | int footer_start_idx = actual_header_size() + expected_msg_len(); 121 | 122 | if (crc_payload >> 8 != rx_data_[footer_start_idx] || (crc_payload & 0xFF) != rx_data_[footer_start_idx + 1]) { 123 | buffer_pos_ = 0; 124 | return -1; // Bad CRC 125 | } 126 | 127 | int msg_id = rx_data_[actual_header_size()]; 128 | int32_t idx = actual_header_size() + 1; 129 | 130 | if (msg_id == (uint8_t)COMM_PACKET_ID::COMM_GET_VALUES) { 131 | mc_values_.temp_mos_filtered = buffer_get_float16(rx_data_, 1e1, &idx); 132 | mc_values_.temp_motor_filtered = buffer_get_float16(rx_data_, 1e1, &idx); 133 | mc_values_.avg_motor_current = buffer_get_float32(rx_data_, 1e2, &idx); 134 | mc_values_.avg_input_current = buffer_get_float32(rx_data_, 1e2, &idx); 135 | idx+=8; // i_current, q_curent 136 | mc_values_.duty_now = buffer_get_float16(rx_data_, 1e3, &idx); 137 | mc_values_.rpm = buffer_get_float32(rx_data_, 1e0, &idx); 138 | mc_values_.v_in = buffer_get_float16(rx_data_, 1e1, &idx); 139 | mc_values_.amp_hours = buffer_get_float32(rx_data_, 1e4, &idx); 140 | mc_values_.amp_hours_charged = buffer_get_float32(rx_data_, 1e4, &idx); 141 | idx+=8; // watt hours used, charged 142 | mc_values_.tachometer = buffer_get_int32(rx_data_, &idx); 143 | mc_values_.tachometer_abs = buffer_get_int32(rx_data_, &idx); 144 | } 145 | 146 | int bytes_to_move = buffer_pos_ - (footer_start_idx + kFooterSize); 147 | if (bytes_to_move > 0) { 148 | memmove(rx_data_, rx_data_ + footer_start_idx + kFooterSize, bytes_to_move); 149 | } 150 | buffer_pos_ = bytes_to_move; 151 | 152 | return msg_id; 153 | } 154 | } 155 | 156 | return 0; 157 | } 158 | -------------------------------------------------------------------------------- /stm_lib/inc/stm32f10x_exti.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_exti.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file contains all the functions prototypes for the EXTI firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __STM32F10x_EXTI_H 25 | #define __STM32F10x_EXTI_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup EXTI 39 | * @{ 40 | */ 41 | 42 | /** @defgroup EXTI_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @brief EXTI mode enumeration 48 | */ 49 | 50 | typedef enum 51 | { 52 | EXTI_Mode_Interrupt = 0x00, 53 | EXTI_Mode_Event = 0x04 54 | }EXTIMode_TypeDef; 55 | 56 | #define IS_EXTI_MODE(MODE) (((MODE) == EXTI_Mode_Interrupt) || ((MODE) == EXTI_Mode_Event)) 57 | 58 | /** 59 | * @brief EXTI Trigger enumeration 60 | */ 61 | 62 | typedef enum 63 | { 64 | EXTI_Trigger_Rising = 0x08, 65 | EXTI_Trigger_Falling = 0x0C, 66 | EXTI_Trigger_Rising_Falling = 0x10 67 | }EXTITrigger_TypeDef; 68 | 69 | #define IS_EXTI_TRIGGER(TRIGGER) (((TRIGGER) == EXTI_Trigger_Rising) || \ 70 | ((TRIGGER) == EXTI_Trigger_Falling) || \ 71 | ((TRIGGER) == EXTI_Trigger_Rising_Falling)) 72 | /** 73 | * @brief EXTI Init Structure definition 74 | */ 75 | 76 | typedef struct 77 | { 78 | uint32_t EXTI_Line; /*!< Specifies the EXTI lines to be enabled or disabled. 79 | This parameter can be any combination of @ref EXTI_Lines */ 80 | 81 | EXTIMode_TypeDef EXTI_Mode; /*!< Specifies the mode for the EXTI lines. 82 | This parameter can be a value of @ref EXTIMode_TypeDef */ 83 | 84 | EXTITrigger_TypeDef EXTI_Trigger; /*!< Specifies the trigger signal active edge for the EXTI lines. 85 | This parameter can be a value of @ref EXTIMode_TypeDef */ 86 | 87 | FunctionalState EXTI_LineCmd; /*!< Specifies the new state of the selected EXTI lines. 88 | This parameter can be set either to ENABLE or DISABLE */ 89 | }EXTI_InitTypeDef; 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** @defgroup EXTI_Exported_Constants 96 | * @{ 97 | */ 98 | 99 | /** @defgroup EXTI_Lines 100 | * @{ 101 | */ 102 | 103 | #define EXTI_Line0 ((uint32_t)0x00001) /*!< External interrupt line 0 */ 104 | #define EXTI_Line1 ((uint32_t)0x00002) /*!< External interrupt line 1 */ 105 | #define EXTI_Line2 ((uint32_t)0x00004) /*!< External interrupt line 2 */ 106 | #define EXTI_Line3 ((uint32_t)0x00008) /*!< External interrupt line 3 */ 107 | #define EXTI_Line4 ((uint32_t)0x00010) /*!< External interrupt line 4 */ 108 | #define EXTI_Line5 ((uint32_t)0x00020) /*!< External interrupt line 5 */ 109 | #define EXTI_Line6 ((uint32_t)0x00040) /*!< External interrupt line 6 */ 110 | #define EXTI_Line7 ((uint32_t)0x00080) /*!< External interrupt line 7 */ 111 | #define EXTI_Line8 ((uint32_t)0x00100) /*!< External interrupt line 8 */ 112 | #define EXTI_Line9 ((uint32_t)0x00200) /*!< External interrupt line 9 */ 113 | #define EXTI_Line10 ((uint32_t)0x00400) /*!< External interrupt line 10 */ 114 | #define EXTI_Line11 ((uint32_t)0x00800) /*!< External interrupt line 11 */ 115 | #define EXTI_Line12 ((uint32_t)0x01000) /*!< External interrupt line 12 */ 116 | #define EXTI_Line13 ((uint32_t)0x02000) /*!< External interrupt line 13 */ 117 | #define EXTI_Line14 ((uint32_t)0x04000) /*!< External interrupt line 14 */ 118 | #define EXTI_Line15 ((uint32_t)0x08000) /*!< External interrupt line 15 */ 119 | #define EXTI_Line16 ((uint32_t)0x10000) /*!< External interrupt line 16 Connected to the PVD Output */ 120 | #define EXTI_Line17 ((uint32_t)0x20000) /*!< External interrupt line 17 Connected to the RTC Alarm event */ 121 | #define EXTI_Line18 ((uint32_t)0x40000) /*!< External interrupt line 18 Connected to the USB Device/USB OTG FS 122 | Wakeup from suspend event */ 123 | #define EXTI_Line19 ((uint32_t)0x80000) /*!< External interrupt line 19 Connected to the Ethernet Wakeup event */ 124 | 125 | #define IS_EXTI_LINE(LINE) ((((LINE) & (uint32_t)0xFFF00000) == 0x00) && ((LINE) != (uint16_t)0x00)) 126 | #define IS_GET_EXTI_LINE(LINE) (((LINE) == EXTI_Line0) || ((LINE) == EXTI_Line1) || \ 127 | ((LINE) == EXTI_Line2) || ((LINE) == EXTI_Line3) || \ 128 | ((LINE) == EXTI_Line4) || ((LINE) == EXTI_Line5) || \ 129 | ((LINE) == EXTI_Line6) || ((LINE) == EXTI_Line7) || \ 130 | ((LINE) == EXTI_Line8) || ((LINE) == EXTI_Line9) || \ 131 | ((LINE) == EXTI_Line10) || ((LINE) == EXTI_Line11) || \ 132 | ((LINE) == EXTI_Line12) || ((LINE) == EXTI_Line13) || \ 133 | ((LINE) == EXTI_Line14) || ((LINE) == EXTI_Line15) || \ 134 | ((LINE) == EXTI_Line16) || ((LINE) == EXTI_Line17) || \ 135 | ((LINE) == EXTI_Line18) || ((LINE) == EXTI_Line19)) 136 | 137 | 138 | /** 139 | * @} 140 | */ 141 | 142 | /** 143 | * @} 144 | */ 145 | 146 | /** @defgroup EXTI_Exported_Macros 147 | * @{ 148 | */ 149 | 150 | /** 151 | * @} 152 | */ 153 | 154 | /** @defgroup EXTI_Exported_Functions 155 | * @{ 156 | */ 157 | 158 | void EXTI_DeInit(void); 159 | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct); 160 | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct); 161 | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line); 162 | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line); 163 | void EXTI_ClearFlag(uint32_t EXTI_Line); 164 | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line); 165 | void EXTI_ClearITPendingBit(uint32_t EXTI_Line); 166 | 167 | #ifdef __cplusplus 168 | } 169 | #endif 170 | 171 | #endif /* __STM32F10x_EXTI_H */ 172 | /** 173 | * @} 174 | */ 175 | 176 | /** 177 | * @} 178 | */ 179 | 180 | /** 181 | * @} 182 | */ 183 | 184 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 185 | -------------------------------------------------------------------------------- /boardController.hpp: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #include 3 | 4 | #include "global.h" 5 | #include "pid.hpp" 6 | #include "imu/imu.hpp" 7 | #include "stateTracker.hpp" 8 | #include "lpf.hpp" 9 | #include "drv/vesc/vesc.hpp" 10 | #include "io/genericOut.hpp" 11 | #include "io/pwm_out.hpp" 12 | 13 | #define BRAKE_VIA_USART 14 | 15 | class BoardController : public UpdateListener { 16 | public: 17 | BoardController(Config* settings, IMU& imu, PwmOut& motor_out, GenericOut& status_led, 18 | GenericOut& beeper, Guard** guards, int guards_count, GenericOut& green_led, VescComm* vesc) 19 | : settings_(settings), 20 | imu_(imu), 21 | state_(guards, guards_count), 22 | balancer_(settings_), 23 | ppm_motor_out_(motor_out), 24 | status_led_(status_led), 25 | beeper_(beeper), 26 | avg_running_motor_out_(&settings->misc.throttle_rc), 27 | green_led_(green_led), 28 | motor_out_lpf_(&settings->balance_settings.output_lpf_hz), 29 | vesc_(vesc), 30 | load_lift_current_lpf_(&settings->load_lift.filter_rc), 31 | load_lift_ramp_(&settings->load_lift.ramp_deg_sec, &settings->load_lift.ramp_deg_sec, &settings->load_lift.max_angle), 32 | pushback_ramp_(&settings->pushback.push_raise_speed_deg_sec, &settings->pushback.push_release_speed_deg_sec, &settings->pushback.push_angle) { 33 | } 34 | 35 | float filterMotorCommand(float cmd) { 36 | cmd = constrain(cmd, -1.0f, 1.0f); 37 | float max_update = settings_->balance_settings.max_update_limiter / 512.0f; 38 | float new_out = constrain(cmd, prev_out_ - max_update, prev_out_ + max_update); 39 | new_out = motor_out_lpf_.compute(new_out); 40 | 41 | prev_out_ = new_out; 42 | return new_out; 43 | } 44 | 45 | 46 | // Send BRAKE_MOTOR_CMD after a short delay after controller goes into Stopped state. 47 | void enableBrakes() { 48 | if (first_stopped_to_brake_iteration_) { 49 | // Remember the stop time on the first iteration. Don't overwrite it until run again. 50 | first_stopped_to_brake_iteration_ = false; 51 | stopped_since_ts32_ = millis32(); 52 | return; 53 | } 54 | 55 | uint32_t stopped_millis = millis32() - stopped_since_ts32_; 56 | if (stopped_millis > 1000ul*60ul*1ul) { 57 | // Timeout after 1 minute, release brakes to allow controller got to sleep. 58 | return; 59 | } 60 | 61 | // Wait 400ms, then apply brakes to avoid very hard stop 62 | if (stopped_millis > 400ul) { 63 | setMotorOutput(BRAKE_MOTOR_CMD); 64 | return; 65 | } 66 | 67 | // Freewheel the motor. 68 | setMotorOutput(0); 69 | } 70 | 71 | // Main control loop. Runs at 1000hz Must finish in less than 1ms otherwise controller will freeze. 72 | void processUpdate(const MpuUpdate& update) { 73 | imu_.compute(update); 74 | current_state_ = state_.update(); 75 | 76 | switch (current_state_) { 77 | case State::Stopped: 78 | enableBrakes(); 79 | status_led_.setState(0); 80 | beeper_.setState(0); 81 | break; 82 | 83 | case State::FirstIteration: 84 | first_stopped_to_brake_iteration_ = true; 85 | last_current_ = 0; 86 | balancer_.reset(); 87 | motor_out_lpf_.reset(0); 88 | prev_out_ = 0; 89 | avg_running_motor_out_.reset(); 90 | status_led_.setState(1); 91 | load_lift_ramp_.Reset(); 92 | pushback_ramp_.Reset(); 93 | // intentional fall through 94 | case State::Starting: 95 | setMotorOutput(filterMotorCommand(balancer_.computeStarting((float*)imu_.rates, (float*)imu_.angles, state_.start_progress()))); 96 | break; 97 | 98 | case State::Running: 99 | float out = balancer_.compute((float*)imu_.rates, (float*)imu_.angles, balance_angle_); 100 | 101 | setMotorOutput(filterMotorCommand(out)); 102 | 103 | bool warning_requested = shouldWarn(out); 104 | beeper_.setState(warning_requested); 105 | 106 | 107 | balance_angle_ = computeLoadLiftOffset(last_current_, &settings_->load_lift); 108 | 109 | balance_angle_+= computePushbackOffset(warning_requested && fabsf(vesc_->mc_values_.erpm_smoothed) > settings_->pushback.min_speed_erpm, vesc_->mc_values_.erpm_smoothed > 0); 110 | 111 | break; 112 | } 113 | 114 | if (vesc_update_cycle_ctr_++ >= 50) { 115 | // request a stats update every 50 cycles => 20hz 116 | vesc_update_cycle_ctr_ = 0; 117 | vesc_->requestStats(); 118 | } 119 | } 120 | 121 | bool shouldWarn(float current_throttle) { 122 | float smoothed_throttle = avg_running_motor_out_.compute(current_throttle); 123 | 124 | bool warning_requested = false; 125 | warning_requested |= fabsf(smoothed_throttle) >= settings_->misc.throttle_threshold; 126 | warning_requested |= fabsf(vesc_->mc_values_.duty_smoothed) > settings_->misc.duty_threshold; 127 | warning_requested |= abs(vesc_->mc_values_.erpm_smoothed) > settings_->misc.erpm_threshold; 128 | warning_requested |= vesc_->mc_values_.v_in_smoothed < settings_->misc.low_volt_threshold; 129 | return warning_requested; 130 | } 131 | 132 | float computeLoadLiftOffset(float unfiltered_motor_current, const Config_LoadLift* lift_settings) { 133 | const float motor_current = load_lift_current_lpf_.compute(unfiltered_motor_current); 134 | if (fabs(motor_current) > lift_settings->start_current) { 135 | float current_with_start_offset = motor_current > 0 136 | ? motor_current - lift_settings->start_current 137 | : motor_current + lift_settings->start_current; 138 | return load_lift_ramp_.Compute(current_with_start_offset * lift_settings->multiplier); 139 | } 140 | else { 141 | return load_lift_ramp_.Compute(0); 142 | } 143 | } 144 | 145 | 146 | float computePushbackOffset(bool shouldPushback, bool forward) { 147 | if (!shouldPushback) { 148 | return pushback_ramp_.Compute(0); 149 | } 150 | 151 | float push_angle = forward ? settings_->pushback.push_angle : -settings_->pushback.push_angle; 152 | return pushback_ramp_.Compute(push_angle); 153 | } 154 | 155 | void setMotorOutput(float cmd) { 156 | float usart_scaling = settings_->balance_settings.usart_control_scaling; 157 | if (fabs(usart_scaling) > 0) { 158 | // Using usart for control 159 | ppm_motor_out_.set(0); 160 | 161 | if (cmd == BRAKE_MOTOR_CMD) { 162 | vesc_->setCurrentBrake(20); 163 | } 164 | else { 165 | float current = cmd * usart_scaling; 166 | last_current_ = current; 167 | vesc_->setCurrent(current); 168 | } 169 | } 170 | else { 171 | if (cmd == BRAKE_MOTOR_CMD) { 172 | ppm_motor_out_.set(0); 173 | 174 | #ifdef BRAKE_VIA_USART 175 | vesc_->setCurrentBrake(20); 176 | #endif 177 | } 178 | else { 179 | ppm_motor_out_.set(fmap(cmd, -1.0f, 1.0f, MIN_MOTOR_CMD, MAX_MOTOR_CMD)); 180 | } 181 | } 182 | } 183 | 184 | float getLastOut() { 185 | return prev_out_; 186 | } 187 | 188 | State current_state() { 189 | return current_state_; 190 | } 191 | 192 | 193 | private: 194 | Config* settings_; 195 | IMU& imu_; 196 | StateTracker state_; 197 | BalanceController balancer_; 198 | PwmOut& ppm_motor_out_; 199 | GenericOut& status_led_; 200 | GenericOut& beeper_; 201 | // avg value for zero-center motor out. range [-1:1] 202 | LPF avg_running_motor_out_; 203 | float prev_out_; 204 | GenericOut& green_led_; 205 | BiQuadLpf motor_out_lpf_; 206 | uint32_t stopped_since_ts32_; 207 | bool first_stopped_to_brake_iteration_ = true; 208 | 209 | VescComm* vesc_; 210 | int vesc_update_cycle_ctr_ = 0; 211 | 212 | float balance_angle_ = 0; 213 | State current_state_; 214 | 215 | LPF load_lift_current_lpf_ = 0; 216 | Ramp load_lift_ramp_; 217 | Ramp pushback_ramp_; 218 | float last_current_ = 0; 219 | }; 220 | -------------------------------------------------------------------------------- /stm_lib/src/misc.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file misc.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the miscellaneous firmware functions (add-on 8 | * to CMSIS functions). 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Includes ------------------------------------------------------------------*/ 24 | #include "misc.h" 25 | 26 | /** @addtogroup STM32F10x_StdPeriph_Driver 27 | * @{ 28 | */ 29 | 30 | /** @defgroup MISC 31 | * @brief MISC driver modules 32 | * @{ 33 | */ 34 | 35 | /** @defgroup MISC_Private_TypesDefinitions 36 | * @{ 37 | */ 38 | 39 | /** 40 | * @} 41 | */ 42 | 43 | /** @defgroup MISC_Private_Defines 44 | * @{ 45 | */ 46 | 47 | #define AIRCR_VECTKEY_MASK ((uint32_t)0x05FA0000) 48 | /** 49 | * @} 50 | */ 51 | 52 | /** @defgroup MISC_Private_Macros 53 | * @{ 54 | */ 55 | 56 | /** 57 | * @} 58 | */ 59 | 60 | /** @defgroup MISC_Private_Variables 61 | * @{ 62 | */ 63 | 64 | /** 65 | * @} 66 | */ 67 | 68 | /** @defgroup MISC_Private_FunctionPrototypes 69 | * @{ 70 | */ 71 | 72 | /** 73 | * @} 74 | */ 75 | 76 | /** @defgroup MISC_Private_Functions 77 | * @{ 78 | */ 79 | 80 | /** 81 | * @brief Configures the priority grouping: pre-emption priority and subpriority. 82 | * @param NVIC_PriorityGroup: specifies the priority grouping bits length. 83 | * This parameter can be one of the following values: 84 | * @arg NVIC_PriorityGroup_0: 0 bits for pre-emption priority 85 | * 4 bits for subpriority 86 | * @arg NVIC_PriorityGroup_1: 1 bits for pre-emption priority 87 | * 3 bits for subpriority 88 | * @arg NVIC_PriorityGroup_2: 2 bits for pre-emption priority 89 | * 2 bits for subpriority 90 | * @arg NVIC_PriorityGroup_3: 3 bits for pre-emption priority 91 | * 1 bits for subpriority 92 | * @arg NVIC_PriorityGroup_4: 4 bits for pre-emption priority 93 | * 0 bits for subpriority 94 | * @retval None 95 | */ 96 | void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup) 97 | { 98 | /* Check the parameters */ 99 | assert_param(IS_NVIC_PRIORITY_GROUP(NVIC_PriorityGroup)); 100 | 101 | /* Set the PRIGROUP[10:8] bits according to NVIC_PriorityGroup value */ 102 | SCB->AIRCR = AIRCR_VECTKEY_MASK | NVIC_PriorityGroup; 103 | } 104 | 105 | /** 106 | * @brief Initializes the NVIC peripheral according to the specified 107 | * parameters in the NVIC_InitStruct. 108 | * @param NVIC_InitStruct: pointer to a NVIC_InitTypeDef structure that contains 109 | * the configuration information for the specified NVIC peripheral. 110 | * @retval None 111 | */ 112 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct) 113 | { 114 | uint32_t tmppriority = 0x00, tmppre = 0x00, tmpsub = 0x0F; 115 | 116 | /* Check the parameters */ 117 | assert_param(IS_FUNCTIONAL_STATE(NVIC_InitStruct->NVIC_IRQChannelCmd)); 118 | assert_param(IS_NVIC_PREEMPTION_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority)); 119 | assert_param(IS_NVIC_SUB_PRIORITY(NVIC_InitStruct->NVIC_IRQChannelSubPriority)); 120 | 121 | if (NVIC_InitStruct->NVIC_IRQChannelCmd != DISABLE) 122 | { 123 | /* Compute the Corresponding IRQ Priority --------------------------------*/ 124 | tmppriority = (0x700 - ((SCB->AIRCR) & (uint32_t)0x700))>> 0x08; 125 | tmppre = (0x4 - tmppriority); 126 | tmpsub = tmpsub >> tmppriority; 127 | 128 | tmppriority = (uint32_t)NVIC_InitStruct->NVIC_IRQChannelPreemptionPriority << tmppre; 129 | tmppriority |= NVIC_InitStruct->NVIC_IRQChannelSubPriority & tmpsub; 130 | tmppriority = tmppriority << 0x04; 131 | 132 | NVIC->IP[NVIC_InitStruct->NVIC_IRQChannel] = tmppriority; 133 | 134 | /* Enable the Selected IRQ Channels --------------------------------------*/ 135 | NVIC->ISER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] = 136 | (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); 137 | } 138 | else 139 | { 140 | /* Disable the Selected IRQ Channels -------------------------------------*/ 141 | NVIC->ICER[NVIC_InitStruct->NVIC_IRQChannel >> 0x05] = 142 | (uint32_t)0x01 << (NVIC_InitStruct->NVIC_IRQChannel & (uint8_t)0x1F); 143 | } 144 | } 145 | 146 | /** 147 | * @brief Sets the vector table location and Offset. 148 | * @param NVIC_VectTab: specifies if the vector table is in RAM or FLASH memory. 149 | * This parameter can be one of the following values: 150 | * @arg NVIC_VectTab_RAM 151 | * @arg NVIC_VectTab_FLASH 152 | * @param Offset: Vector Table base offset field. This value must be a multiple 153 | * of 0x200. 154 | * @retval None 155 | */ 156 | void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset) 157 | { 158 | /* Check the parameters */ 159 | assert_param(IS_NVIC_VECTTAB(NVIC_VectTab)); 160 | assert_param(IS_NVIC_OFFSET(Offset)); 161 | 162 | SCB->VTOR = NVIC_VectTab | (Offset & (uint32_t)0x1FFFFF80); 163 | } 164 | 165 | /** 166 | * @brief Selects the condition for the system to enter low power mode. 167 | * @param LowPowerMode: Specifies the new mode for the system to enter low power mode. 168 | * This parameter can be one of the following values: 169 | * @arg NVIC_LP_SEVONPEND 170 | * @arg NVIC_LP_SLEEPDEEP 171 | * @arg NVIC_LP_SLEEPONEXIT 172 | * @param NewState: new state of LP condition. This parameter can be: ENABLE or DISABLE. 173 | * @retval None 174 | */ 175 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState) 176 | { 177 | /* Check the parameters */ 178 | assert_param(IS_NVIC_LP(LowPowerMode)); 179 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 180 | 181 | if (NewState != DISABLE) 182 | { 183 | SCB->SCR |= LowPowerMode; 184 | } 185 | else 186 | { 187 | SCB->SCR &= (uint32_t)(~(uint32_t)LowPowerMode); 188 | } 189 | } 190 | 191 | /** 192 | * @brief Configures the SysTick clock source. 193 | * @param SysTick_CLKSource: specifies the SysTick clock source. 194 | * This parameter can be one of the following values: 195 | * @arg SysTick_CLKSource_HCLK_Div8: AHB clock divided by 8 selected as SysTick clock source. 196 | * @arg SysTick_CLKSource_HCLK: AHB clock selected as SysTick clock source. 197 | * @retval None 198 | */ 199 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource) 200 | { 201 | /* Check the parameters */ 202 | assert_param(IS_SYSTICK_CLK_SOURCE(SysTick_CLKSource)); 203 | if (SysTick_CLKSource == SysTick_CLKSource_HCLK) 204 | { 205 | SysTick->CTRL |= SysTick_CLKSource_HCLK; 206 | } 207 | else 208 | { 209 | SysTick->CTRL &= SysTick_CLKSource_HCLK_Div8; 210 | } 211 | } 212 | 213 | /** 214 | * @} 215 | */ 216 | 217 | /** 218 | * @} 219 | */ 220 | 221 | /** 222 | * @} 223 | */ 224 | 225 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 226 | -------------------------------------------------------------------------------- /stm_lib/src/stm32f10x_exti.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_exti.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the EXTI firmware functions. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "stm32f10x_exti.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup EXTI 30 | * @brief EXTI driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup EXTI_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup EXTI_Private_Defines 43 | * @{ 44 | */ 45 | 46 | #define EXTI_LINENONE ((uint32_t)0x00000) /* No interrupt selected */ 47 | 48 | /** 49 | * @} 50 | */ 51 | 52 | /** @defgroup EXTI_Private_Macros 53 | * @{ 54 | */ 55 | 56 | /** 57 | * @} 58 | */ 59 | 60 | /** @defgroup EXTI_Private_Variables 61 | * @{ 62 | */ 63 | 64 | /** 65 | * @} 66 | */ 67 | 68 | /** @defgroup EXTI_Private_FunctionPrototypes 69 | * @{ 70 | */ 71 | 72 | /** 73 | * @} 74 | */ 75 | 76 | /** @defgroup EXTI_Private_Functions 77 | * @{ 78 | */ 79 | 80 | /** 81 | * @brief Deinitializes the EXTI peripheral registers to their default reset values. 82 | * @param None 83 | * @retval None 84 | */ 85 | void EXTI_DeInit(void) 86 | { 87 | EXTI->IMR = 0x00000000; 88 | EXTI->EMR = 0x00000000; 89 | EXTI->RTSR = 0x00000000; 90 | EXTI->FTSR = 0x00000000; 91 | EXTI->PR = 0x000FFFFF; 92 | } 93 | 94 | /** 95 | * @brief Initializes the EXTI peripheral according to the specified 96 | * parameters in the EXTI_InitStruct. 97 | * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure 98 | * that contains the configuration information for the EXTI peripheral. 99 | * @retval None 100 | */ 101 | void EXTI_Init(EXTI_InitTypeDef* EXTI_InitStruct) 102 | { 103 | uint32_t tmp = 0; 104 | 105 | /* Check the parameters */ 106 | assert_param(IS_EXTI_MODE(EXTI_InitStruct->EXTI_Mode)); 107 | assert_param(IS_EXTI_TRIGGER(EXTI_InitStruct->EXTI_Trigger)); 108 | assert_param(IS_EXTI_LINE(EXTI_InitStruct->EXTI_Line)); 109 | assert_param(IS_FUNCTIONAL_STATE(EXTI_InitStruct->EXTI_LineCmd)); 110 | 111 | tmp = (uint32_t)EXTI_BASE; 112 | 113 | if (EXTI_InitStruct->EXTI_LineCmd != DISABLE) 114 | { 115 | /* Clear EXTI line configuration */ 116 | EXTI->IMR &= ~EXTI_InitStruct->EXTI_Line; 117 | EXTI->EMR &= ~EXTI_InitStruct->EXTI_Line; 118 | 119 | tmp += EXTI_InitStruct->EXTI_Mode; 120 | 121 | *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line; 122 | 123 | /* Clear Rising Falling edge configuration */ 124 | EXTI->RTSR &= ~EXTI_InitStruct->EXTI_Line; 125 | EXTI->FTSR &= ~EXTI_InitStruct->EXTI_Line; 126 | 127 | /* Select the trigger for the selected external interrupts */ 128 | if (EXTI_InitStruct->EXTI_Trigger == EXTI_Trigger_Rising_Falling) 129 | { 130 | /* Rising Falling edge */ 131 | EXTI->RTSR |= EXTI_InitStruct->EXTI_Line; 132 | EXTI->FTSR |= EXTI_InitStruct->EXTI_Line; 133 | } 134 | else 135 | { 136 | tmp = (uint32_t)EXTI_BASE; 137 | tmp += EXTI_InitStruct->EXTI_Trigger; 138 | 139 | *(__IO uint32_t *) tmp |= EXTI_InitStruct->EXTI_Line; 140 | } 141 | } 142 | else 143 | { 144 | tmp += EXTI_InitStruct->EXTI_Mode; 145 | 146 | /* Disable the selected external lines */ 147 | *(__IO uint32_t *) tmp &= ~EXTI_InitStruct->EXTI_Line; 148 | } 149 | } 150 | 151 | /** 152 | * @brief Fills each EXTI_InitStruct member with its reset value. 153 | * @param EXTI_InitStruct: pointer to a EXTI_InitTypeDef structure which will 154 | * be initialized. 155 | * @retval None 156 | */ 157 | void EXTI_StructInit(EXTI_InitTypeDef* EXTI_InitStruct) 158 | { 159 | EXTI_InitStruct->EXTI_Line = EXTI_LINENONE; 160 | EXTI_InitStruct->EXTI_Mode = EXTI_Mode_Interrupt; 161 | EXTI_InitStruct->EXTI_Trigger = EXTI_Trigger_Falling; 162 | EXTI_InitStruct->EXTI_LineCmd = DISABLE; 163 | } 164 | 165 | /** 166 | * @brief Generates a Software interrupt. 167 | * @param EXTI_Line: specifies the EXTI lines to be enabled or disabled. 168 | * This parameter can be any combination of EXTI_Linex where x can be (0..19). 169 | * @retval None 170 | */ 171 | void EXTI_GenerateSWInterrupt(uint32_t EXTI_Line) 172 | { 173 | /* Check the parameters */ 174 | assert_param(IS_EXTI_LINE(EXTI_Line)); 175 | 176 | EXTI->SWIER |= EXTI_Line; 177 | } 178 | 179 | /** 180 | * @brief Checks whether the specified EXTI line flag is set or not. 181 | * @param EXTI_Line: specifies the EXTI line flag to check. 182 | * This parameter can be: 183 | * @arg EXTI_Linex: External interrupt line x where x(0..19) 184 | * @retval The new state of EXTI_Line (SET or RESET). 185 | */ 186 | FlagStatus EXTI_GetFlagStatus(uint32_t EXTI_Line) 187 | { 188 | FlagStatus bitstatus = RESET; 189 | /* Check the parameters */ 190 | assert_param(IS_GET_EXTI_LINE(EXTI_Line)); 191 | 192 | if ((EXTI->PR & EXTI_Line) != (uint32_t)RESET) 193 | { 194 | bitstatus = SET; 195 | } 196 | else 197 | { 198 | bitstatus = RESET; 199 | } 200 | return bitstatus; 201 | } 202 | 203 | /** 204 | * @brief Clears the EXTI's line pending flags. 205 | * @param EXTI_Line: specifies the EXTI lines flags to clear. 206 | * This parameter can be any combination of EXTI_Linex where x can be (0..19). 207 | * @retval None 208 | */ 209 | void EXTI_ClearFlag(uint32_t EXTI_Line) 210 | { 211 | /* Check the parameters */ 212 | assert_param(IS_EXTI_LINE(EXTI_Line)); 213 | 214 | EXTI->PR = EXTI_Line; 215 | } 216 | 217 | /** 218 | * @brief Checks whether the specified EXTI line is asserted or not. 219 | * @param EXTI_Line: specifies the EXTI line to check. 220 | * This parameter can be: 221 | * @arg EXTI_Linex: External interrupt line x where x(0..19) 222 | * @retval The new state of EXTI_Line (SET or RESET). 223 | */ 224 | ITStatus EXTI_GetITStatus(uint32_t EXTI_Line) 225 | { 226 | ITStatus bitstatus = RESET; 227 | uint32_t enablestatus = 0; 228 | /* Check the parameters */ 229 | assert_param(IS_GET_EXTI_LINE(EXTI_Line)); 230 | 231 | enablestatus = EXTI->IMR & EXTI_Line; 232 | if (((EXTI->PR & EXTI_Line) != (uint32_t)RESET) && (enablestatus != (uint32_t)RESET)) 233 | { 234 | bitstatus = SET; 235 | } 236 | else 237 | { 238 | bitstatus = RESET; 239 | } 240 | return bitstatus; 241 | } 242 | 243 | /** 244 | * @brief Clears the EXTI's line pending bits. 245 | * @param EXTI_Line: specifies the EXTI lines to clear. 246 | * This parameter can be any combination of EXTI_Linex where x can be (0..19). 247 | * @retval None 248 | */ 249 | void EXTI_ClearITPendingBit(uint32_t EXTI_Line) 250 | { 251 | /* Check the parameters */ 252 | assert_param(IS_EXTI_LINE(EXTI_Line)); 253 | 254 | EXTI->PR = EXTI_Line; 255 | } 256 | 257 | /** 258 | * @} 259 | */ 260 | 261 | /** 262 | * @} 263 | */ 264 | 265 | /** 266 | * @} 267 | */ 268 | 269 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 270 | -------------------------------------------------------------------------------- /drv/mpu6050/mpu.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "../../cmsis_boot/stm32f10x.h" 5 | #include "../../stm_lib/inc/stm32f10x_gpio.h" 6 | #include "../../stm_lib/inc/stm32f10x_rcc.h" 7 | #include "../../stm_lib/inc/stm32f10x_i2c.h" 8 | #include "../../stm_lib/inc/stm32f10x_dma.h" 9 | #include "../../stm_lib/inc/stm32f10x_exti.h" 10 | #include "../../stm_lib/inc/misc.h" 11 | #include "mpu.hpp" 12 | #include "mpu6050_registers.hpp" 13 | #include "../../io/i2c.hpp" 14 | #include "../../global.h" 15 | #include "../../utils.h" 16 | 17 | 18 | #define MPU6050_ADDRESS 0x68u 19 | 20 | #define MPU6050_DMA_Channel DMA1_Channel5 21 | #define MPU6050_DMA_ChannelIRQn DMA1_Channel5_IRQn 22 | #define MPU6050_DMA_TC_FLAG DMA1_FLAG_TC5 23 | #define MPU6050_I2C I2C2 24 | 25 | #ifdef REV5 26 | #define MPU6050_INT_ExtiPin GPIO_Pin_13; 27 | #define MPU6050_INT_ExtiPort GPIOC 28 | #define MPU6050_INT_ExtiPeriph RCC_APB2Periph_GPIOC 29 | #define MPU6050_INT_ExtiPinSource GPIO_PortSourceGPIOC 30 | #else 31 | #define MPU6050_INT_ExtiPin GPIO_Pin_13; 32 | #define MPU6050_INT_ExtiPort GPIOB 33 | #define MPU6050_INT_ExtiPeriph RCC_APB2Periph_GPIOB 34 | #define MPU6050_INT_ExtiPinSource GPIO_PortSourceGPIOB 35 | #endif 36 | 37 | unsigned char MPU6050_Rx_Buffer[6+2+6]; 38 | 39 | #ifndef BOARD_ROTATION_MACRO 40 | #define BOARD_ROTATION_MACRO(XYZ) {} 41 | #endif 42 | 43 | volatile bool data_processed = true; 44 | 45 | void mpuHandleDataReady() { 46 | if (!data_processed) 47 | return; 48 | 49 | data_processed = false; 50 | i2c_DmaRead(MPU6050_ADDRESS, MPU6050_ACCEL_XOUT_H, MPU6050_DMA_Channel, 14); 51 | } 52 | 53 | void MPU_DataReadyIntInit() { 54 | RCC_APB2PeriphClockCmd(MPU6050_INT_ExtiPeriph, ENABLE); 55 | 56 | /* GPIO configuration */ 57 | GPIO_InitTypeDef GPIO_InitStructure; 58 | GPIO_InitStructure.GPIO_Pin = MPU6050_INT_ExtiPin; 59 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 60 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING; 61 | GPIO_Init(MPU6050_INT_ExtiPort, &GPIO_InitStructure); 62 | 63 | GPIO_EXTILineConfig(GPIO_PortSourceGPIOB, GPIO_PinSource13); 64 | 65 | EXTI_InitTypeDef EXTI_InitStructure; 66 | EXTI_InitStructure.EXTI_Line = EXTI_Line13; 67 | EXTI_InitStructure.EXTI_Mode = EXTI_Mode_Interrupt; 68 | EXTI_InitStructure.EXTI_Trigger = EXTI_Trigger_Rising; 69 | EXTI_InitStructure.EXTI_LineCmd = ENABLE; 70 | EXTI_Init(&EXTI_InitStructure); 71 | 72 | NVIC_InitTypeDef NVIC_InitStructure; 73 | NVIC_InitStructure.NVIC_IRQChannel = EXTI15_10_IRQn; //I2C2 connect to channel 5 of DMA1 74 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; 75 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1; 76 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 77 | NVIC_Init(&NVIC_InitStructure); 78 | } 79 | 80 | void MPU_DmaInit() { 81 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); 82 | 83 | NVIC_InitTypeDef NVIC_InitStructure; 84 | DMA_InitTypeDef DMA_InitStructure; 85 | 86 | DMA_DeInit(MPU6050_DMA_Channel); //reset DMA1 channe1 to default values; 87 | 88 | DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&MPU6050_I2C->DR; //address of data reading register of I2C1 89 | DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)MPU6050_Rx_Buffer; //variable to store data 90 | DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; //channel will be used for peripheral to memory transfer 91 | DMA_InitStructure.DMA_Mode = DMA_Mode_Normal; // (non circular) 92 | DMA_InitStructure.DMA_Priority = DMA_Priority_High; 93 | DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; //Location assigned to peripheral register will be source 94 | DMA_InitStructure.DMA_BufferSize = 14; //number of data to be transfered 95 | DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; //automatic memory increment disable for peripheral 96 | DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; //automatic memory increment enable for memory 97 | DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte; //source peripheral data size = 8bit 98 | DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Byte; //destination memory data size = 8bit 99 | DMA_Init(MPU6050_DMA_Channel, &DMA_InitStructure); 100 | DMA_ITConfig(MPU6050_DMA_Channel, DMA_IT_TC, ENABLE); 101 | 102 | NVIC_InitStructure.NVIC_IRQChannel = MPU6050_DMA_ChannelIRQn; //I2C2 connect to channel 5 of DMA1 103 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; 104 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; 105 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 106 | NVIC_Init(&NVIC_InitStructure); 107 | } 108 | 109 | Mpu accGyro; 110 | 111 | void Mpu::callibrateAcc() { 112 | accCalibrationIterationsLeft_ = GYRO_CALIBRATION_ITERATIONS_REQUIRED; 113 | } 114 | 115 | void Mpu::handleRawData(uint8_t* rawData) { 116 | MpuUpdate update; 117 | handleGyroData(update.gyro, rawData+8); // first 6 are ACC data, then 2 temperature and next 6 are gyro data 118 | handleAccData(update.acc, update.gyro, rawData); 119 | 120 | if (listener_ != nullptr) { 121 | listener_->processUpdate(update); 122 | } 123 | 124 | data_processed = true; 125 | } 126 | 127 | inline void applyZeroOffset(int16_t* data, int16_t* offsets) { 128 | for (int i = 0; i < 3; i++) { 129 | data[i] -= offsets[i]; 130 | } 131 | } 132 | 133 | inline int16_t getAccVal(uint8_t* rawData, uint8_t axis) { 134 | return ((int16_t)((rawData[axis*2]<<8) | rawData[axis*2 + 1]))>>2; 135 | } 136 | 137 | void Mpu::handleAccData(int16_t* acc, int16_t* gyro, uint8_t* rawData) { 138 | acc[0] = -getAccVal(rawData, 1); 139 | acc[1] = -getAccVal(rawData, 0); 140 | acc[2] = getAccVal(rawData, 2); 141 | BOARD_ROTATION_MACRO(acc); 142 | if (accCalibrationIterationsLeft_ > 0) { 143 | if (--accCalibrationIterationsLeft_ == 0) { 144 | accZeroOffsets_[0] = acc[0]; 145 | accZeroOffsets_[1] = acc[1]; 146 | accZeroOffsets_[2] = acc[2] - ACC_1G; 147 | } 148 | if (gyro[0] > GYRO_CALIBRATION_MAX_DIFF || gyro[1] > GYRO_CALIBRATION_MAX_DIFF || gyro[2] > GYRO_CALIBRATION_MAX_DIFF) 149 | accCalibrationIterationsLeft_ = GYRO_CALIBRATION_ITERATIONS_REQUIRED; 150 | } 151 | applyZeroOffset(acc, accZeroOffsets_); 152 | } 153 | 154 | inline int16_t getGyroVal(uint8_t* rawData, uint8_t axis) { 155 | return ((int16_t)((rawData[axis * 2] << 8) | rawData[axis * 2 + 1])) >> 2; // range: +/- 8192; +/- 500 deg/sec 156 | } 157 | 158 | void Mpu::handleGyroData(int16_t* gyro, uint8_t* rawData) { 159 | gyro[0] = getGyroVal(rawData, 1); 160 | gyro[1] = getGyroVal(rawData, 0); 161 | gyro[2] = -getGyroVal(rawData, 2); 162 | BOARD_ROTATION_MACRO(gyro); 163 | if (calibrationComplete()) { 164 | applyZeroOffset(gyro, gyroZeroOffsets_); 165 | } 166 | else { // run one calibration step 167 | if (gyroCalibrationIterationsLeft_ == GYRO_CALIBRATION_ITERATIONS_REQUIRED) { // init at first iteration 168 | for (int axis = 0; axis < 3; axis++) { 169 | gyroCalibrationAccumulator_[axis] = 0; 170 | } 171 | } 172 | 173 | bool allAxisHaveSmallDiff = true; 174 | const int calibrationIterationNumber = GYRO_CALIBRATION_ITERATIONS_REQUIRED - gyroCalibrationIterationsLeft_; 175 | for (int axis = 0; axis < 3; axis++) { 176 | gyroCalibrationAccumulator_[axis] += gyro[axis]; 177 | const int axisGyroAvg = gyroCalibrationAccumulator_[axis] / (calibrationIterationNumber + 1); 178 | allAxisHaveSmallDiff = allAxisHaveSmallDiff && abs(axisGyroAvg - gyro[axis]) < GYRO_CALIBRATION_MAX_DIFF; 179 | } 180 | 181 | if (allAxisHaveSmallDiff) { // successful iteration 182 | gyroCalibrationIterationsLeft_--; 183 | } 184 | else { // device has moved during calibration, restart 185 | gyroCalibrationIterationsLeft_ = GYRO_CALIBRATION_ITERATIONS_REQUIRED; 186 | } 187 | 188 | if (gyroCalibrationIterationsLeft_ == 0) { // last iteration, save calibration values 189 | for (int axis = 0; axis < 3; axis++) { 190 | gyroZeroOffsets_[axis] = gyroCalibrationAccumulator_[axis] / GYRO_CALIBRATION_ITERATIONS_REQUIRED; 191 | } 192 | } 193 | } 194 | } 195 | 196 | extern "C" void DMA1_Channel5_IRQHandler(void) 197 | { 198 | //GPIOA->BSRR = GPIO_Pin_11; 199 | if (DMA_GetFlagStatus(MPU6050_DMA_TC_FLAG)) // 172us 200 | { 201 | DMA_ClearFlag(MPU6050_DMA_TC_FLAG); 202 | 203 | I2C_DMACmd(MPU6050_I2C, DISABLE); 204 | /* Send I2C1 STOP Condition */ 205 | I2C_GenerateSTOP(MPU6050_I2C, ENABLE); 206 | /* Disable DMA channel*/ 207 | DMA_Cmd(MPU6050_DMA_Channel, DISABLE); 208 | 209 | accGyro.handleRawData(MPU6050_Rx_Buffer); 210 | } 211 | //GPIOA->BRR = GPIO_Pin_11; 212 | } 213 | 214 | void Mpu::init(uint8_t lpfSettings) { 215 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_PWR_MGMT_1, MPU6050_CLOCK_PLL_ZGYRO); 216 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_SMPLRT_DIV, 0); 217 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_CONFIG, lpfSettings); 218 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_GYRO_CONFIG, MPU6050_GYRO_FS_500); 219 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_ACCEL_CONFIG, MPU6050_ACCEL_FS_4); 220 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_INT_PIN_CFG, 1<<4); 221 | i2c_writeReg(MPU6050_ADDRESS, MPU6050_INT_ENABLE, 1); 222 | 223 | MPU_DataReadyIntInit(); 224 | MPU_DmaInit(); 225 | 226 | gyroCalibrationIterationsLeft_ = GYRO_CALIBRATION_ITERATIONS_REQUIRED; 227 | } 228 | -------------------------------------------------------------------------------- /stm_lib/inc/misc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file misc.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 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 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 13 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 14 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 15 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 16 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 17 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 18 | * 19 | *

© COPYRIGHT 2011 STMicroelectronics

20 | ****************************************************************************** 21 | */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __MISC_H 25 | #define __MISC_H 26 | 27 | #ifdef __cplusplus 28 | extern "C" { 29 | #endif 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "stm32f10x.h" 33 | 34 | /** @addtogroup STM32F10x_StdPeriph_Driver 35 | * @{ 36 | */ 37 | 38 | /** @addtogroup MISC 39 | * @{ 40 | */ 41 | 42 | /** @defgroup MISC_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @brief NVIC Init Structure definition 48 | */ 49 | 50 | typedef struct 51 | { 52 | uint8_t NVIC_IRQChannel; /*!< Specifies the IRQ channel to be enabled or disabled. 53 | This parameter can be a value of @ref IRQn_Type 54 | (For the complete STM32 Devices IRQ Channels list, please 55 | refer to stm32f10x.h file) */ 56 | 57 | uint8_t NVIC_IRQChannelPreemptionPriority; /*!< Specifies the pre-emption priority for the IRQ channel 58 | specified in NVIC_IRQChannel. This parameter can be a value 59 | between 0 and 15 as described in the table @ref NVIC_Priority_Table */ 60 | 61 | uint8_t NVIC_IRQChannelSubPriority; /*!< Specifies the subpriority level for the IRQ channel specified 62 | in NVIC_IRQChannel. This parameter can be a value 63 | between 0 and 15 as described in the table @ref NVIC_Priority_Table */ 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 | */ 73 | 74 | /** @defgroup NVIC_Priority_Table 75 | * @{ 76 | */ 77 | 78 | /** 79 | @code 80 | The table below gives the allowed values of the pre-emption priority and subpriority according 81 | to the Priority Grouping configuration performed by NVIC_PriorityGroupConfig function 82 | ============================================================================================================================ 83 | NVIC_PriorityGroup | NVIC_IRQChannelPreemptionPriority | NVIC_IRQChannelSubPriority | Description 84 | ============================================================================================================================ 85 | NVIC_PriorityGroup_0 | 0 | 0-15 | 0 bits for pre-emption priority 86 | | | | 4 bits for subpriority 87 | ---------------------------------------------------------------------------------------------------------------------------- 88 | NVIC_PriorityGroup_1 | 0-1 | 0-7 | 1 bits for pre-emption priority 89 | | | | 3 bits for subpriority 90 | ---------------------------------------------------------------------------------------------------------------------------- 91 | NVIC_PriorityGroup_2 | 0-3 | 0-3 | 2 bits for pre-emption priority 92 | | | | 2 bits for subpriority 93 | ---------------------------------------------------------------------------------------------------------------------------- 94 | NVIC_PriorityGroup_3 | 0-7 | 0-1 | 3 bits for pre-emption priority 95 | | | | 1 bits for subpriority 96 | ---------------------------------------------------------------------------------------------------------------------------- 97 | NVIC_PriorityGroup_4 | 0-15 | 0 | 4 bits for pre-emption priority 98 | | | | 0 bits for subpriority 99 | ============================================================================================================================ 100 | @endcode 101 | */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** @defgroup MISC_Exported_Constants 108 | * @{ 109 | */ 110 | 111 | /** @defgroup Vector_Table_Base 112 | * @{ 113 | */ 114 | 115 | #define NVIC_VectTab_RAM ((uint32_t)0x20000000) 116 | #define NVIC_VectTab_FLASH ((uint32_t)0x08000000) 117 | #define IS_NVIC_VECTTAB(VECTTAB) (((VECTTAB) == NVIC_VectTab_RAM) || \ 118 | ((VECTTAB) == NVIC_VectTab_FLASH)) 119 | /** 120 | * @} 121 | */ 122 | 123 | /** @defgroup System_Low_Power 124 | * @{ 125 | */ 126 | 127 | #define NVIC_LP_SEVONPEND ((uint8_t)0x10) 128 | #define NVIC_LP_SLEEPDEEP ((uint8_t)0x04) 129 | #define NVIC_LP_SLEEPONEXIT ((uint8_t)0x02) 130 | #define IS_NVIC_LP(LP) (((LP) == NVIC_LP_SEVONPEND) || \ 131 | ((LP) == NVIC_LP_SLEEPDEEP) || \ 132 | ((LP) == NVIC_LP_SLEEPONEXIT)) 133 | /** 134 | * @} 135 | */ 136 | 137 | /** @defgroup Preemption_Priority_Group 138 | * @{ 139 | */ 140 | 141 | #define NVIC_PriorityGroup_0 ((uint32_t)0x700) /*!< 0 bits for pre-emption priority 142 | 4 bits for subpriority */ 143 | #define NVIC_PriorityGroup_1 ((uint32_t)0x600) /*!< 1 bits for pre-emption priority 144 | 3 bits for subpriority */ 145 | #define NVIC_PriorityGroup_2 ((uint32_t)0x500) /*!< 2 bits for pre-emption priority 146 | 2 bits for subpriority */ 147 | #define NVIC_PriorityGroup_3 ((uint32_t)0x400) /*!< 3 bits for pre-emption priority 148 | 1 bits for subpriority */ 149 | #define NVIC_PriorityGroup_4 ((uint32_t)0x300) /*!< 4 bits for pre-emption priority 150 | 0 bits for subpriority */ 151 | 152 | #define IS_NVIC_PRIORITY_GROUP(GROUP) (((GROUP) == NVIC_PriorityGroup_0) || \ 153 | ((GROUP) == NVIC_PriorityGroup_1) || \ 154 | ((GROUP) == NVIC_PriorityGroup_2) || \ 155 | ((GROUP) == NVIC_PriorityGroup_3) || \ 156 | ((GROUP) == NVIC_PriorityGroup_4)) 157 | 158 | #define IS_NVIC_PREEMPTION_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) 159 | 160 | #define IS_NVIC_SUB_PRIORITY(PRIORITY) ((PRIORITY) < 0x10) 161 | 162 | #define IS_NVIC_OFFSET(OFFSET) ((OFFSET) < 0x000FFFFF) 163 | 164 | /** 165 | * @} 166 | */ 167 | 168 | /** @defgroup SysTick_clock_source 169 | * @{ 170 | */ 171 | 172 | #define SysTick_CLKSource_HCLK_Div8 ((uint32_t)0xFFFFFFFB) 173 | #define SysTick_CLKSource_HCLK ((uint32_t)0x00000004) 174 | #define IS_SYSTICK_CLK_SOURCE(SOURCE) (((SOURCE) == SysTick_CLKSource_HCLK) || \ 175 | ((SOURCE) == SysTick_CLKSource_HCLK_Div8)) 176 | /** 177 | * @} 178 | */ 179 | 180 | /** 181 | * @} 182 | */ 183 | 184 | /** @defgroup MISC_Exported_Macros 185 | * @{ 186 | */ 187 | 188 | /** 189 | * @} 190 | */ 191 | 192 | /** @defgroup MISC_Exported_Functions 193 | * @{ 194 | */ 195 | 196 | void NVIC_PriorityGroupConfig(uint32_t NVIC_PriorityGroup); 197 | void NVIC_Init(NVIC_InitTypeDef* NVIC_InitStruct); 198 | void NVIC_SetVectorTable(uint32_t NVIC_VectTab, uint32_t Offset); 199 | void NVIC_SystemLPConfig(uint8_t LowPowerMode, FunctionalState NewState); 200 | void SysTick_CLKSourceConfig(uint32_t SysTick_CLKSource); 201 | 202 | #ifdef __cplusplus 203 | } 204 | #endif 205 | 206 | #endif /* __MISC_H */ 207 | 208 | /** 209 | * @} 210 | */ 211 | 212 | /** 213 | * @} 214 | */ 215 | 216 | /** 217 | * @} 218 | */ 219 | 220 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 221 | -------------------------------------------------------------------------------- /BalancingController.coproj: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 22 | 23 | 52 | 53 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | -------------------------------------------------------------------------------- /main.cpp: -------------------------------------------------------------------------------- 1 | #include "stm_lib/inc/misc.h" 2 | #include "cmsis_boot/stm32f10x.h" 3 | #include "stm_lib/inc/stm32f10x_dma.h" 4 | #include "stm_lib/inc/stm32f10x_exti.h" 5 | #include "stm_lib/inc/stm32f10x_flash.h" 6 | #include "stm_lib/inc/stm32f10x_gpio.h" 7 | #include "stm_lib/inc/stm32f10x_i2c.h" 8 | #include "stm_lib/inc/stm32f10x_iwdg.h" 9 | #include "stm_lib/inc/stm32f10x_rcc.h" 10 | 11 | #include 12 | #include 13 | #include 14 | 15 | #include "arduino.h" 16 | #include "balanceController.hpp" 17 | #include "boardController.hpp" 18 | #include "drv/comms/communicator.hpp" 19 | #include "drv/mpu6050/mpu.hpp" 20 | #include "drv/settings/settings.hpp" 21 | #include "drv/vesc/vesc.hpp" 22 | #include "global.h" 23 | #include "guards/angleGuard.hpp" 24 | #include "guards/footpadGuard.hpp" 25 | #include "imu/imu.hpp" 26 | #include "io/genericOut.hpp" 27 | #include "io/i2c.hpp" 28 | #include "io/pwm_out.hpp" 29 | #include "io/usart.hpp" 30 | #include "ledController.hpp" 31 | #include "lpf.hpp" 32 | #include "pid.hpp" 33 | #include "stateTracker.hpp" 34 | #include "stm32f10x_adc.h" 35 | 36 | extern "C" void EXTI15_10_IRQHandler(void) { 37 | // GPIOA->BSRR = GPIO_Pin_11; 38 | if (EXTI_GetITStatus(MPU6050_INT_Exti)) // MPU6050_INT 39 | { 40 | EXTI_ClearITPendingBit(MPU6050_INT_Exti); 41 | mpuHandleDataReady(); 42 | } 43 | // GPIOA->BRR = GPIO_Pin_11; 44 | } 45 | 46 | class InitWaiter : public UpdateListener { 47 | public: 48 | InitWaiter(IMU *imu, Guard* angle_guard) 49 | : imu_(imu), angle_guard_(angle_guard) {} 50 | 51 | void processUpdate(const MpuUpdate &update) { 52 | if (!accGyro.calibrationComplete()) { 53 | imu_->compute(update, true); 54 | } else { 55 | imu_->compute(update); 56 | } 57 | angle_guard_->Update(); 58 | } 59 | 60 | private: 61 | IMU *imu_; 62 | Guard* angle_guard_; 63 | }; 64 | 65 | static uint8_t scratch[512]; 66 | 67 | uint8_t write_pos = 0; 68 | uint8_t read_pos = 0; 69 | static uint8_t debug[200]; 70 | 71 | void applyCalibrationConfig(const Config &cfg, Mpu *accGyro) { 72 | int16_t acc_offsets[3] = {(int16_t)cfg.callibration.acc_x, 73 | (int16_t)cfg.callibration.acc_y, 74 | (int16_t)cfg.callibration.acc_z}; 75 | accGyro->applyAccZeroOffsets(acc_offsets); 76 | } 77 | 78 | 79 | void usart2txConfigure(bool manual) { 80 | GPIO_InitTypeDef GPIO_InitStructure; 81 | 82 | /* Configure USART Tx alternate function */ 83 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 84 | GPIO_InitStructure.GPIO_Mode = manual ? GPIO_Mode_Out_PP : GPIO_Mode_AF_PP; 85 | 86 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2; 87 | GPIO_Init(GPIOA, &GPIO_InitStructure); 88 | } 89 | 90 | extern "C" void EXTI9_5_IRQHandler(void) { 91 | if (EXTI_GetITStatus(EXTI_Line7)) 92 | { 93 | EXTI_ClearITPendingBit(EXTI_Line7); 94 | GPIO_WriteBit(GPIOA, GPIO_Pin_2, (BitAction)GPIO_ReadInputDataBit(GPIOB, GPIO_Pin_7)); 95 | } 96 | } 97 | 98 | Communicator comms; 99 | 100 | extern uint8_t cf_data[] asm("_binary_build_descriptor_pb_bin_deflate_start"); 101 | extern uint8_t cf_data_e[] asm("_binary_build_descriptor_pb_bin_deflate_end"); 102 | 103 | int main(void) { 104 | SystemInit(); 105 | 106 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE); 107 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE); 108 | GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable, ENABLE); 109 | RCC_AHBPeriphClockCmd(RCC_AHBPeriph_CRC, ENABLE); 110 | NVIC_PriorityGroupConfig(NVIC_PriorityGroup_1); 111 | FLASH_SetLatency(FLASH_Latency_1); 112 | 113 | RCC_LSICmd(ENABLE); 114 | /* Wait till LSI is ready */ 115 | while (RCC_GetFlagStatus(RCC_FLAG_LSIRDY) == RESET) { 116 | } 117 | 118 | /* Enable Watchdog*/ 119 | IWDG_WriteAccessCmd(IWDG_WriteAccess_Enable); 120 | IWDG_SetPrescaler(IWDG_Prescaler_8); // 4, 8, 16 ... 256 121 | IWDG_SetReload(0x0FFF); // This parameter must be a number between 0 and 0x0FFF. 122 | IWDG_ReloadCounter(); 123 | IWDG_Enable(); 124 | 125 | PwmOut motor_out; 126 | motor_out.init(NEUTRAL_MOTOR_CMD); 127 | 128 | initArduino(); 129 | 130 | i2c_init(); 131 | Serial1.Init(USART1, 115200); 132 | Serial2.Init(USART2, 115200); 133 | 134 | comms.Init(&Serial1); 135 | 136 | Config cfg = Config_init_default; 137 | if (readSettingsFromFlash(&cfg)) { 138 | const char msg[] = "Config OK\n"; 139 | Serial1.Send((uint8_t *)msg, sizeof(msg)); 140 | applyCalibrationConfig(cfg, &accGyro); 141 | } else { 142 | cfg = Config_init_default; 143 | const char msg[] = "Config DEFAULT\n"; 144 | Serial1.Send((uint8_t *)msg, sizeof(msg)); 145 | } 146 | 147 | // TODO: Consider running imu and balacning loop outside of interrupt. Set a 148 | // have_new_data flag in the interrupt, copy data to output if flag was 149 | // cleared. Ignore data if flag was not cleared. Reset flag in main loop when 150 | // data is copied. 151 | 152 | // TODO 2: while(1) must run at specific loop time for any LPFs and pids 153 | // (footpad, etc to work correctly). Fix the loop time 154 | 155 | GenericOut status_led(RCC_APB2Periph_GPIOB, GPIOB, GPIO_Pin_4, 1); // red 156 | status_led.init(); 157 | status_led.setState(0); 158 | 159 | IMU imu(&cfg); 160 | AngleGuard angle_guard(imu, &cfg.balance_settings); 161 | InitWaiter waiter(&imu, &angle_guard); // wait for angle. Wait for pads too? 162 | accGyro.setListener(&waiter); 163 | accGyro.init(cfg.balance_settings.global_gyro_lpf); 164 | 165 | GenericOut beeper(RCC_APB2Periph_GPIOA, GPIOA, GPIO_Pin_12, true); 166 | beeper.init(true); 167 | 168 | led_controller_init(); 169 | 170 | 171 | beeper.toggle(); 172 | { 173 | uint16_t last_check_time = 0; 174 | while (!accGyro.calibrationComplete() || angle_guard.CanStart()) { 175 | IWDG_ReloadCounter(); 176 | if ((uint16_t)(half_millis() - last_check_time) > 200u) { 177 | last_check_time = half_millis(); 178 | status_led.toggle(); 179 | beeper.setState(false); 180 | } 181 | led_controller_startup_animation(); 182 | } 183 | } 184 | 185 | 186 | // Beep to indicate calibration done. 187 | { 188 | beeper.setState(true); 189 | delay(150); 190 | beeper.setState(false); 191 | delay(250); 192 | beeper.setState(true); 193 | delay(150); 194 | beeper.setState(false); 195 | } 196 | 197 | GenericOut green_led(RCC_APB2Periph_GPIOB, GPIOB, GPIO_Pin_3, true); 198 | green_led.init(); 199 | 200 | // GenericOut debug_out(RCC_APB2Periph_GPIOA, GPIOA, GPIO_Pin_11, false); 201 | // debug_out.init(); 202 | 203 | FootpadGuard foot_pad_guard(&cfg.foot_pad); 204 | Guard *guards[] = {&angle_guard, &foot_pad_guard}; 205 | int guards_count = sizeof(guards) / sizeof(Guard *); 206 | 207 | VescComm vesc(&Serial2); 208 | LPF erpm_lpf(&cfg.misc.erpm_rc); 209 | LPF v_in_lpf(&cfg.misc.volt_rc); 210 | LPF duty_lpf(&cfg.misc.duty_rc); 211 | 212 | BoardController main_ctrl(&cfg, imu, motor_out, status_led, beeper, guards, 213 | guards_count, green_led, &vesc); 214 | 215 | accGyro.setListener(&main_ctrl); 216 | 217 | uint16_t last_check_time = 0; 218 | 219 | write_pos = 0; 220 | read_pos = 0; 221 | uint8_t debug_stream_type = 0; 222 | 223 | while (1) { // background work 224 | IWDG_ReloadCounter(); 225 | led_controller_update(); 226 | 227 | if ((uint16_t)(half_millis() - last_check_time) > 100u) { 228 | last_check_time = half_millis(); 229 | 230 | float battery_level = vesc.mc_values_.v_in / max(cfg.misc.batt_cells, 1); 231 | 232 | static constexpr float kMinCharge = 3.0; 233 | static constexpr float kMaxCharge = 4.15; 234 | battery_level = constrain(battery_level, kMinCharge, kMaxCharge); 235 | battery_level = fmap(battery_level, kMinCharge, kMaxCharge, 0, 1); 236 | 237 | led_controller_set_state(vesc.mc_values_.rpm, imu.rates[2], battery_level); 238 | switch (debug_stream_type) { 239 | case 1: 240 | debug[write_pos++] = (int8_t)imu.angles[ANGLE_DRIVE]; 241 | break; 242 | case 2: 243 | debug[write_pos++] = (int8_t)(main_ctrl.getLastOut() * 127); 244 | break; 245 | case 3: 246 | debug[write_pos++] = (int8_t)(imu.angles[ANGLE_DRIVE] * 10); 247 | break; 248 | case 4: 249 | debug[write_pos++] = (int8_t)(vesc.mc_values_.avg_motor_current); 250 | break; 251 | case 5: 252 | debug[write_pos++] = (int8_t)(vesc.mc_values_.v_in); 253 | break; 254 | case 6: 255 | debug[write_pos++] = (int8_t)(vesc.mc_values_.avg_input_current); 256 | break; 257 | case 7: 258 | debug[write_pos++] = (int8_t)(vesc.mc_values_.rpm); 259 | break; 260 | } 261 | 262 | if (write_pos >= sizeof(debug)) write_pos = 0; 263 | } 264 | 265 | uint8_t comms_msg = comms.update(); 266 | switch (comms_msg) { 267 | case RequestId_READ_CONFIG: { 268 | int16_t data_len = saveProtoToBuffer(scratch, sizeof(scratch), 269 | Config_fields, &cfg, &Serial1); 270 | if (data_len > 0) { 271 | comms.SendMsg(ReplyId_CONFIG, scratch, data_len); 272 | } else { 273 | comms.SendMsg(ReplyId_GENERIC_FAIL); 274 | } 275 | break; 276 | } 277 | 278 | case RequestId_SET_DEBUG_STREAM_ID: 279 | if (comms.data_len() == 1) { 280 | debug_stream_type = comms.data()[0]; 281 | } else { 282 | comms.SendMsg(ReplyId_GENERIC_FAIL); 283 | } 284 | break; 285 | 286 | case RequestId_GET_DEBUG_BUFFER: { 287 | // TODO: make sure it all fits in TX buffer or an overrun will occur 288 | if (write_pos < read_pos) { 289 | int data_len_tail = sizeof(debug) - read_pos; 290 | if (data_len_tail > 0) { 291 | comms.SendMsg(ReplyId_DEBUG_BUFFER, debug + read_pos, 292 | data_len_tail); 293 | read_pos = 0; 294 | } 295 | } 296 | 297 | int rem_len = write_pos - read_pos; 298 | if (rem_len > 0) { 299 | comms.SendMsg(ReplyId_DEBUG_BUFFER, debug + read_pos, rem_len); 300 | read_pos = write_pos; 301 | } 302 | break; 303 | } 304 | 305 | case RequestId_WRITE_CONFIG: { 306 | Config_Callibration c = cfg.callibration; 307 | bool good = 308 | readSettingsFromBuffer(&cfg, comms.data(), comms.data_len()); 309 | if (good) 310 | comms.SendMsg(ReplyId_GENERIC_OK); 311 | else 312 | comms.SendMsg(ReplyId_GENERIC_FAIL); 313 | if (!cfg.has_callibration) // use own calibration if not received one. 314 | { 315 | cfg.callibration = c; 316 | cfg.has_callibration = true; 317 | } else { 318 | applyCalibrationConfig(cfg, &accGyro); 319 | } 320 | break; 321 | } 322 | 323 | case RequestId_GET_STATS: { 324 | Stats stats = Stats_init_default; 325 | stats.drive_angle = imu.angles[ANGLE_DRIVE]; 326 | stats.steer_angle = imu.angles[ANGLE_STEER]; 327 | stats.pad_pressure1 = foot_pad_guard.getLevel(0); 328 | stats.pad_pressure2 = foot_pad_guard.getLevel(1); 329 | stats.batt_current = vesc.mc_values_.avg_input_current; 330 | stats.batt_voltage = vesc.mc_values_.v_in; 331 | stats.cell_voltage = stats.batt_voltage / max(cfg.misc.batt_cells, 1); 332 | stats.motor_current = vesc.mc_values_.avg_motor_current; 333 | stats.distance_traveled = vesc.mc_values_.tachometer_abs; 334 | stats.speed = vesc.mc_values_.rpm; 335 | stats.motor_duty = vesc.mc_values_.duty_now; 336 | stats.esc_temp = vesc.mc_values_.temp_mos_filtered; 337 | stats.motor_temp = vesc.mc_values_.temp_motor_filtered; 338 | 339 | int16_t data_len = 340 | saveProtoToBuffer(scratch, sizeof(scratch), Stats_fields, &stats); 341 | if (data_len != -1) { 342 | comms.SendMsg(ReplyId_STATS, scratch, data_len); 343 | } else { 344 | comms.SendMsg(ReplyId_GENERIC_FAIL); 345 | } 346 | break; 347 | } 348 | 349 | case RequestId_CALLIBRATE_ACC: 350 | comms.SendMsg(ReplyId_GENERIC_OK); 351 | accGyro.callibrateAcc(); 352 | while (!accGyro.accCalibrationComplete()) IWDG_ReloadCounter(); 353 | cfg.has_callibration = true; 354 | cfg.callibration.acc_x = accGyro.getAccOffsets()[0]; 355 | cfg.callibration.acc_y = accGyro.getAccOffsets()[1]; 356 | cfg.callibration.acc_z = accGyro.getAccOffsets()[2]; 357 | comms.SendMsg(ReplyId_GENERIC_OK); 358 | break; 359 | 360 | case RequestId_SAVE_CONFIG: 361 | saveSettingsToFlash(cfg); 362 | comms.SendMsg(ReplyId_GENERIC_OK); 363 | break; 364 | 365 | case RequestId_GET_CONFIG_DESCRIPTOR: 366 | comms.SendMsg(ReplyId_CONFIG_DESCRIPTOR, (uint8_t*)cf_data, (int)cf_data_e - (int)cf_data); 367 | break; 368 | } 369 | 370 | if (vesc.update() == (uint8_t)VescComm::COMM_PACKET_ID::COMM_GET_VALUES) { 371 | // got a stats update; recalculate smoothed values 372 | // Runs at 20hz (values requested from balance controller to sync with 373 | // current control over USART request. 374 | vesc.mc_values_.erpm_smoothed = erpm_lpf.compute(vesc.mc_values_.rpm); 375 | vesc.mc_values_.v_in_smoothed = v_in_lpf.compute(vesc.mc_values_.v_in); 376 | vesc.mc_values_.duty_smoothed = 377 | duty_lpf.compute(vesc.mc_values_.duty_now); 378 | } 379 | } 380 | } 381 | -------------------------------------------------------------------------------- /cmsis_boot/startup/startup_stm32f10x_md.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file startup_stm32f10x_md.c 4 | * @author Coocox 5 | * @version V1.0 6 | * @date 12/23/2009 7 | * @brief STM32F10x Medium Density Devices Startup code. 8 | * This module performs: 9 | * - Set the initial SP 10 | * - Set the vector table entries with the exceptions ISR address 11 | * - Initialize data and bss 12 | * - Setup the microcontroller system. 13 | * - Call the application's entry point. 14 | * After Reset the Cortex-M3 processor is in Thread mode, 15 | * priority is Privileged, and the Stack is set to Main. 16 | ******************************************************************************* 17 | */ 18 | 19 | 20 | /*----------Stack Configuration-----------------------------------------------*/ 21 | #define STACK_SIZE 0x00000500 /*!< The Stack size suggest using even number */ 22 | __attribute__ ((section(".co_stack"))) 23 | unsigned long pulStack[STACK_SIZE]; 24 | 25 | 26 | /*----------Macro definition--------------------------------------------------*/ 27 | #define WEAK __attribute__ ((weak)) 28 | 29 | 30 | /*----------Declaration of the default fault handlers-------------------------*/ 31 | /* System exception vector handler */ 32 | __attribute__ ((used)) 33 | void WEAK Reset_Handler(void); 34 | void WEAK NMI_Handler(void); 35 | void WEAK HardFault_Handler(void); 36 | void WEAK MemManage_Handler(void); 37 | void WEAK BusFault_Handler(void); 38 | void WEAK UsageFault_Handler(void); 39 | void WEAK SVC_Handler(void); 40 | void WEAK DebugMon_Handler(void); 41 | void WEAK PendSV_Handler(void); 42 | void WEAK SysTick_Handler(void); 43 | void WEAK WWDG_IRQHandler(void); 44 | void WEAK PVD_IRQHandler(void); 45 | void WEAK TAMPER_IRQHandler(void); 46 | void WEAK RTC_IRQHandler(void); 47 | void WEAK FLASH_IRQHandler(void); 48 | void WEAK RCC_IRQHandler(void); 49 | void WEAK EXTI0_IRQHandler(void); 50 | void WEAK EXTI1_IRQHandler(void); 51 | void WEAK EXTI2_IRQHandler(void); 52 | void WEAK EXTI3_IRQHandler(void); 53 | void WEAK EXTI4_IRQHandler(void); 54 | void WEAK DMA1_Channel1_IRQHandler(void); 55 | void WEAK DMA1_Channel2_IRQHandler(void); 56 | void WEAK DMA1_Channel3_IRQHandler(void); 57 | void WEAK DMA1_Channel4_IRQHandler(void); 58 | void WEAK DMA1_Channel5_IRQHandler(void); 59 | void WEAK DMA1_Channel6_IRQHandler(void); 60 | void WEAK DMA1_Channel7_IRQHandler(void); 61 | void WEAK ADC1_2_IRQHandler(void); 62 | void WEAK USB_HP_CAN1_TX_IRQHandler(void); 63 | void WEAK USB_LP_CAN1_RX0_IRQHandler(void); 64 | void WEAK CAN1_RX1_IRQHandler(void); 65 | void WEAK CAN1_SCE_IRQHandler(void); 66 | void WEAK EXTI9_5_IRQHandler(void); 67 | void WEAK TIM1_BRK_IRQHandler(void); 68 | void WEAK TIM1_UP_IRQHandler(void); 69 | void WEAK TIM1_TRG_COM_IRQHandler(void); 70 | void WEAK TIM1_CC_IRQHandler(void); 71 | void WEAK TIM2_IRQHandler(void); 72 | void WEAK TIM3_IRQHandler(void); 73 | void WEAK TIM4_IRQHandler(void); 74 | void WEAK I2C1_EV_IRQHandler(void); 75 | void WEAK I2C1_ER_IRQHandler(void); 76 | void WEAK I2C2_EV_IRQHandler(void); 77 | void WEAK I2C2_ER_IRQHandler(void); 78 | void WEAK SPI1_IRQHandler(void); 79 | void WEAK SPI2_IRQHandler(void); 80 | void WEAK USART1_IRQHandler(void); 81 | void WEAK USART2_IRQHandler(void); 82 | void WEAK USART3_IRQHandler(void); 83 | void WEAK EXTI15_10_IRQHandler(void); 84 | void WEAK RTCAlarm_IRQHandler(void); 85 | void WEAK USBWakeUp_IRQHandler(void); 86 | 87 | 88 | /*----------Symbols defined in linker script----------------------------------*/ 89 | extern unsigned long _sidata; /*!< Start address for the initialization 90 | values of the .data section. */ 91 | extern unsigned long _sdata; /*!< Start address for the .data section */ 92 | extern unsigned long _edata; /*!< End address for the .data section */ 93 | extern unsigned long _sbss; /*!< Start address for the .bss section */ 94 | extern unsigned long _ebss; /*!< End address for the .bss section */ 95 | extern void _eram; /*!< End address for ram */ 96 | 97 | 98 | /*----------Function prototypes-----------------------------------------------*/ 99 | extern int main(void); /*!< The entry point for the application. */ 100 | extern void SystemInit(void); /*!< Setup the microcontroller system(CMSIS) */ 101 | void Default_Reset_Handler(void); /*!< Default reset handler */ 102 | static void Default_Handler(void); /*!< Default exception handler */ 103 | 104 | 105 | /** 106 | *@brief The minimal vector table for a Cortex M3. Note that the proper constructs 107 | * must be placed on this to ensure that it ends up at physical address 108 | * 0x00000000. 109 | */ 110 | __attribute__ ((used,section(".isr_vector"))) 111 | void (* const g_pfnVectors[])(void) = 112 | { 113 | /*----------Core Exceptions-------------------------------------------------*/ 114 | (void *)&pulStack[STACK_SIZE], /*!< The initial stack pointer */ 115 | Reset_Handler, /*!< Reset Handler */ 116 | NMI_Handler, /*!< NMI Handler */ 117 | HardFault_Handler, /*!< Hard Fault Handler */ 118 | MemManage_Handler, /*!< MPU Fault Handler */ 119 | BusFault_Handler, /*!< Bus Fault Handler */ 120 | UsageFault_Handler, /*!< Usage Fault Handler */ 121 | 0,0,0,0, /*!< Reserved */ 122 | SVC_Handler, /*!< SVCall Handler */ 123 | DebugMon_Handler, /*!< Debug Monitor Handler */ 124 | 0, /*!< Reserved */ 125 | PendSV_Handler, /*!< PendSV Handler */ 126 | SysTick_Handler, /*!< SysTick Handler */ 127 | 128 | /*----------External Exceptions---------------------------------------------*/ 129 | WWDG_IRQHandler, /*!< 0: Window Watchdog */ 130 | PVD_IRQHandler, /*!< 1: PVD through EXTI Line detect */ 131 | TAMPER_IRQHandler, /*!< 2: Tamper */ 132 | RTC_IRQHandler, /*!< 3: RTC */ 133 | FLASH_IRQHandler, /*!< 4: Flash */ 134 | RCC_IRQHandler, /*!< 5: RCC */ 135 | EXTI0_IRQHandler, /*!< 6: EXTI Line 0 */ 136 | EXTI1_IRQHandler, /*!< 7: EXTI Line 1 */ 137 | EXTI2_IRQHandler, /*!< 8: EXTI Line 2 */ 138 | EXTI3_IRQHandler, /*!< 9: EXTI Line 3 */ 139 | EXTI4_IRQHandler, /*!< 10: EXTI Line 4 */ 140 | DMA1_Channel1_IRQHandler, /*!< 11: DMA1 Channel 1 */ 141 | DMA1_Channel2_IRQHandler, /*!< 12: DMA1 Channel 2 */ 142 | DMA1_Channel3_IRQHandler, /*!< 13: DMA1 Channel 3 */ 143 | DMA1_Channel4_IRQHandler, /*!< 14: DMA1 Channel 4 */ 144 | DMA1_Channel5_IRQHandler, /*!< 15: DMA1 Channel 5 */ 145 | DMA1_Channel6_IRQHandler, /*!< 16: DMA1 Channel 6 */ 146 | DMA1_Channel7_IRQHandler, /*!< 17: DMA1 Channel 7 */ 147 | ADC1_2_IRQHandler, /*!< 18: ADC1 & ADC2 */ 148 | USB_HP_CAN1_TX_IRQHandler, /*!< 19: USB High Priority or CAN1 TX */ 149 | USB_LP_CAN1_RX0_IRQHandler, /*!< 20: USB Low Priority or CAN1 RX0 */ 150 | CAN1_RX1_IRQHandler, /*!< 21: CAN1 RX1 */ 151 | CAN1_SCE_IRQHandler, /*!< 22: CAN1 SCE */ 152 | EXTI9_5_IRQHandler, /*!< 23: EXTI Line 9..5 */ 153 | TIM1_BRK_IRQHandler, /*!< 24: TIM1 Break */ 154 | TIM1_UP_IRQHandler, /*!< 25: TIM1 Update */ 155 | TIM1_TRG_COM_IRQHandler, /*!< 26: TIM1 Trigger and Commutation */ 156 | TIM1_CC_IRQHandler, /*!< 27: TIM1 Capture Compare */ 157 | TIM2_IRQHandler, /*!< 28: TIM2 */ 158 | TIM3_IRQHandler, /*!< 29: TIM3 */ 159 | TIM4_IRQHandler, /*!< 30: TIM4 */ 160 | I2C1_EV_IRQHandler, /*!< 31: I2C1 Event */ 161 | I2C1_ER_IRQHandler, /*!< 32: I2C1 Error */ 162 | I2C2_EV_IRQHandler, /*!< 33: I2C2 Event */ 163 | I2C2_ER_IRQHandler, /*!< 34: I2C2 Error */ 164 | SPI1_IRQHandler, /*!< 35: SPI1 */ 165 | SPI2_IRQHandler, /*!< 36: SPI2 */ 166 | USART1_IRQHandler, /*!< 37: USART1 */ 167 | USART2_IRQHandler, /*!< 38: USART2 */ 168 | USART3_IRQHandler, /*!< 39: USART3 */ 169 | EXTI15_10_IRQHandler, /*!< 40: EXTI Line 15..10 */ 170 | RTCAlarm_IRQHandler, /*!< 41: RTC Alarm through EXTI Line */ 171 | USBWakeUp_IRQHandler, /*!< 42: USB Wakeup from suspend */ 172 | 0,0,0,0,0,0,0, /*!< Reserved */ 173 | (void *)0xF108F85F /*!< Boot in RAM mode */ 174 | }; 175 | 176 | 177 | /** 178 | * @brief This is the code that gets called when the processor first 179 | * starts execution following a reset event. Only the absolutely 180 | * necessary set is performed, after which the application 181 | * supplied main() routine is called. 182 | * @param None 183 | * @retval None 184 | */ 185 | void Default_Reset_Handler(void) 186 | { 187 | /* Initialize data and bss */ 188 | unsigned long *pulSrc, *pulDest; 189 | 190 | /* Copy the data segment initializers from flash to SRAM */ 191 | pulSrc = &_sidata; 192 | 193 | for(pulDest = &_sdata; pulDest < &_edata; ) 194 | { 195 | *(pulDest++) = *(pulSrc++); 196 | } 197 | 198 | /* Zero fill the bss segment. This is done with inline assembly since this 199 | will clear the value of pulDest if it is not kept in a register. */ 200 | __asm(" ldr r0, =_sbss\n" 201 | " ldr r1, =_ebss\n" 202 | " mov r2, #0\n" 203 | " .thumb_func\n" 204 | "zero_loop:\n" 205 | " cmp r0, r1\n" 206 | " it lt\n" 207 | " strlt r2, [r0], #4\n" 208 | " blt zero_loop"); 209 | 210 | /* Setup the microcontroller system. */ 211 | SystemInit(); 212 | 213 | /* Call the application's entry point.*/ 214 | main(); 215 | } 216 | 217 | /** 218 | *@brief Provide weak aliases for each Exception handler to the Default_Handler. 219 | * As they are weak aliases, any function with the same name will override 220 | * this definition. 221 | */ 222 | #pragma weak Reset_Handler = Default_Reset_Handler 223 | #pragma weak NMI_Handler = Default_Handler 224 | #pragma weak HardFault_Handler = Default_Handler 225 | #pragma weak MemManage_Handler = Default_Handler 226 | #pragma weak BusFault_Handler = Default_Handler 227 | #pragma weak UsageFault_Handler = Default_Handler 228 | #pragma weak SVC_Handler = Default_Handler 229 | #pragma weak DebugMon_Handler = Default_Handler 230 | #pragma weak PendSV_Handler = Default_Handler 231 | #pragma weak SysTick_Handler = Default_Handler 232 | #pragma weak WWDG_IRQHandler = Default_Handler 233 | #pragma weak PVD_IRQHandler = Default_Handler 234 | #pragma weak TAMPER_IRQHandler = Default_Handler 235 | #pragma weak RTC_IRQHandler = Default_Handler 236 | #pragma weak FLASH_IRQHandler = Default_Handler 237 | #pragma weak RCC_IRQHandler = Default_Handler 238 | #pragma weak EXTI0_IRQHandler = Default_Handler 239 | #pragma weak EXTI1_IRQHandler = Default_Handler 240 | #pragma weak EXTI2_IRQHandler = Default_Handler 241 | #pragma weak EXTI3_IRQHandler = Default_Handler 242 | #pragma weak EXTI4_IRQHandler = Default_Handler 243 | #pragma weak DMA1_Channel1_IRQHandler = Default_Handler 244 | #pragma weak DMA1_Channel2_IRQHandler = Default_Handler 245 | #pragma weak DMA1_Channel3_IRQHandler = Default_Handler 246 | #pragma weak DMA1_Channel4_IRQHandler = Default_Handler 247 | #pragma weak DMA1_Channel5_IRQHandler = Default_Handler 248 | #pragma weak DMA1_Channel6_IRQHandler = Default_Handler 249 | #pragma weak DMA1_Channel7_IRQHandler = Default_Handler 250 | #pragma weak ADC1_2_IRQHandler = Default_Handler 251 | #pragma weak USB_HP_CAN1_TX_IRQHandler = Default_Handler 252 | #pragma weak USB_LP_CAN1_RX0_IRQHandler = Default_Handler 253 | #pragma weak CAN1_RX1_IRQHandler = Default_Handler 254 | #pragma weak CAN1_SCE_IRQHandler = Default_Handler 255 | #pragma weak EXTI9_5_IRQHandler = Default_Handler 256 | #pragma weak TIM1_BRK_IRQHandler = Default_Handler 257 | #pragma weak TIM1_UP_IRQHandler = Default_Handler 258 | #pragma weak TIM1_TRG_COM_IRQHandler = Default_Handler 259 | #pragma weak TIM1_CC_IRQHandler = Default_Handler 260 | #pragma weak TIM2_IRQHandler = Default_Handler 261 | #pragma weak TIM3_IRQHandler = Default_Handler 262 | #pragma weak TIM4_IRQHandler = Default_Handler 263 | #pragma weak I2C1_EV_IRQHandler = Default_Handler 264 | #pragma weak I2C1_ER_IRQHandler = Default_Handler 265 | #pragma weak I2C2_EV_IRQHandler = Default_Handler 266 | #pragma weak I2C2_ER_IRQHandler = Default_Handler 267 | #pragma weak SPI1_IRQHandler = Default_Handler 268 | #pragma weak SPI2_IRQHandler = Default_Handler 269 | #pragma weak USART1_IRQHandler = Default_Handler 270 | #pragma weak USART2_IRQHandler = Default_Handler 271 | #pragma weak USART3_IRQHandler = Default_Handler 272 | #pragma weak EXTI15_10_IRQHandler = Default_Handler 273 | #pragma weak RTCAlarm_IRQHandler = Default_Handler 274 | #pragma weak USBWakeUp_IRQHandler = Default_Handler 275 | 276 | #include "stm32f10x_gpio.h" 277 | /** 278 | * @brief This is the code that gets called when the processor receives an 279 | * unexpected interrupt. This simply enters an infinite loop, 280 | * preserving the system state for examination by a debugger. 281 | * @param None 282 | * @retval None 283 | */ 284 | static void Default_Handler(void) 285 | { 286 | /* Go into an infinite loop. */ 287 | while (1) 288 | { 289 | GPIOA->BSRR = GPIO_Pin_11; 290 | GPIOA->BRR = GPIO_Pin_11; 291 | } 292 | } 293 | 294 | /*********************** (C) COPYRIGHT 2009 Coocox ************END OF FILE*****/ 295 | --------------------------------------------------------------------------------