├── .gitignore ├── kicad_schema └── navigator_schema │ ├── mad_symbols.bck │ ├── mad_symbols.dcm │ ├── navigatorschema-rescue.dcm │ ├── sym-lib-table │ ├── fp-lib-table │ ├── mad_footprints.pretty │ ├── Nokia5110_blue.kicad_mod │ └── NEO6-M.kicad_mod │ ├── mad_symbols.lib │ ├── navigatorschema-rescue.lib │ └── navigatorschema.pro ├── StdPeriph_Driver ├── src │ ├── stm32f10x_i2c.c │ ├── stm32f10x_flash.c │ ├── stm32f10x_usart.c │ ├── stm32f10x_crc.c │ ├── stm32f10x_iwdg.c │ ├── stm32f10x_dbgmcu.c │ ├── stm32f10x_wwdg.c │ ├── misc.c │ └── stm32f10x_exti.c └── inc │ ├── stm32f10x_crc.h │ ├── stm32f10x_conf.h │ ├── stm32f10x_wwdg.h │ ├── stm32f10x_dbgmcu.h │ ├── stm32f10x_iwdg.h │ ├── stm32f10x_rtc.h │ ├── stm32f10x_pwr.h │ ├── stm32f10x_cec.h │ ├── stm32f10x_exti.h │ └── stm32f10x_bkp.h ├── inc ├── power │ └── power.h ├── commons │ ├── bearing.h │ ├── vector3d.h │ ├── quaternion.h │ ├── magnetic.h │ ├── commons.h │ └── madgwick.h ├── display │ ├── nokiaLcd │ │ ├── nokia5110_font.h │ │ ├── nokia5110_buffered.h │ │ └── nokia5110.h │ └── display.h ├── flash │ ├── flash.h │ └── settings.h ├── stm32vldiscovery_utils.h ├── hardware.h ├── gps │ ├── neo6m.h │ └── nmea.h ├── accelerometer │ └── mpu6050 │ │ └── mpu6050_i2c.h ├── i2c1 │ └── i2c1.h ├── time │ └── kov_time.h ├── stm32f10x_it.h └── magnetometer │ ├── lis3mdl │ └── lis3mdl_i2c.h │ ├── hmc5883 │ └── hmc5883.h │ └── qmc5883l │ └── qmc5883l.h ├── src ├── power │ └── power.c ├── stm32vldiscovery_utils.c ├── commons │ ├── vector3d.c │ ├── commons.c │ ├── bearing.c │ └── quaternion.c ├── flash │ ├── settings.c │ └── flash.c ├── accelerometer │ └── mpu6050 │ │ └── mpu6050_i2c.c ├── magnetometer │ ├── qmc5883l │ │ └── qmc5883l.c │ └── lis3mdl │ │ └── lis3mdl_i2c.c ├── display │ └── nokiaLcd │ │ ├── nokia5110_buffered.c │ │ ├── nokia5110_font.c │ │ └── nokia5110.c ├── syscalls.c ├── gps │ └── neo6m.c └── time │ └── kov_time.c ├── Readme.md ├── CMSIS └── device │ └── system_stm32f10x.h ├── Makefile ├── navigator.pro └── LinkerScript.ld /.gitignore: -------------------------------------------------------------------------------- 1 | *.pro.user 2 | *.o 3 | *.d 4 | bin/* 5 | build/* 6 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/mad_symbols.bck: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/mad_symbols.dcm: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/navigatorschema-rescue.dcm: -------------------------------------------------------------------------------- 1 | EESchema-DOCLIB Version 2.0 2 | # 3 | #End Doc Library 4 | -------------------------------------------------------------------------------- /StdPeriph_Driver/src/stm32f10x_i2c.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/mad-navigator/HEAD/StdPeriph_Driver/src/stm32f10x_i2c.c -------------------------------------------------------------------------------- /StdPeriph_Driver/src/stm32f10x_flash.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/mad-navigator/HEAD/StdPeriph_Driver/src/stm32f10x_flash.c -------------------------------------------------------------------------------- /StdPeriph_Driver/src/stm32f10x_usart.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/maddevsio/mad-navigator/HEAD/StdPeriph_Driver/src/stm32f10x_usart.c -------------------------------------------------------------------------------- /inc/power/power.h: -------------------------------------------------------------------------------- 1 | #ifndef __POWER_H 2 | #define __POWER_H 3 | 4 | void goToSleepMode(void); 5 | void initWakeup(void); 6 | 7 | #endif //__POWER_H -------------------------------------------------------------------------------- /inc/commons/bearing.h: -------------------------------------------------------------------------------- 1 | #ifndef BEARING_H 2 | #define BEARING_H 3 | 4 | float initialBearing(float lat1, float lon1, float lat2, float lon2); 5 | 6 | #endif // BEARING_H 7 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/sym-lib-table: -------------------------------------------------------------------------------- 1 | (sym_lib_table 2 | (lib (name kov_schema-rescue)(type Legacy)(uri ${KIPRJMOD}/kov_schema-rescue.lib)(options "")(descr "")) 3 | ) 4 | -------------------------------------------------------------------------------- /inc/display/nokiaLcd/nokia5110_font.h: -------------------------------------------------------------------------------- 1 | #ifndef STM32F10X_PCD8544_FONT_H 2 | #define STM32F10X_PCD8544_FONT_H 3 | 4 | #include 5 | 6 | extern uint8_t font6_8[92][6]; 7 | 8 | #endif // STM32F10X_PCD8544_FONT_H 9 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/fp-lib-table: -------------------------------------------------------------------------------- 1 | (fp_lib_table 2 | (lib (name kov_schema)(type KiCad)(uri /home/raistlin/work/kov/kicad_schema/kov_schema.pretty)(options "")(descr "")) 3 | (lib (name mad_footprints)(type KiCad)(uri ${KIPRJMOD}/mad_footprints.pretty)(options "")(descr "")) 4 | ) 5 | -------------------------------------------------------------------------------- /inc/flash/flash.h: -------------------------------------------------------------------------------- 1 | #ifndef __FLASH_H 2 | #define __FLASH_H 3 | 4 | #include "stdbool.h" 5 | #include "stdint.h" 6 | 7 | #define BANK1_PAGES (uint32_t)128 8 | #define BANK1_PAGE_SIZE (uint32_t)0x400 9 | 10 | bool flashInit(); 11 | bool flashLock(); 12 | bool flashUnlock(); 13 | bool flashRead(uint32_t startAddress, void* buffer, uint16_t size); 14 | bool flashWrite(uint32_t startAddress, void* buffer, uint16_t size); 15 | 16 | #endif //__FLASH_H 17 | -------------------------------------------------------------------------------- /inc/commons/vector3d.h: -------------------------------------------------------------------------------- 1 | #ifndef VECTOR3D_H 2 | #define VECTOR3D_H 3 | 4 | #include 5 | 6 | typedef struct vector3d { 7 | float x, y, z, len; 8 | } vector3d_t; 9 | 10 | vector3d_t vector_new(float x, float y, float z); 11 | void vector_normalize(vector3d_t *v3d); 12 | 13 | float vector_rotationSign(const vector3d_t *a, const vector3d_t *b); 14 | float vector_flatCos(const vector3d_t *a, const vector3d_t *b); 15 | 16 | #endif // VECTOR3D_H 17 | -------------------------------------------------------------------------------- /inc/stm32vldiscovery_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef __STM32F10xVLDISCOVERY_RCC_H 2 | #define __STM32F10xVLDISCOVERY_RCC_H 3 | 4 | #include 5 | #ifdef __cplusplus 6 | extern "C" { 7 | #endif 8 | 9 | extern volatile uint32_t *DWT_CONTROL; 10 | extern volatile uint32_t *DWT_CYCCNT; 11 | extern volatile uint32_t *DEMCR; 12 | extern volatile uint32_t *LAR; 13 | 14 | void initDWT(void); 15 | 16 | 17 | #ifdef __cplusplus 18 | } 19 | 20 | #endif //__cplusplus 21 | #endif //__STM32F10xVLDISCOVERY_RCC_H 22 | -------------------------------------------------------------------------------- /inc/flash/settings.h: -------------------------------------------------------------------------------- 1 | #ifndef __SETTINGS_H 2 | #define __SETTINGS_H 3 | 4 | #include "stdint.h" 5 | #include "stdbool.h" 6 | 7 | #define SETTINGS_MAGIC_V1 0xFBFAF0F9 8 | 9 | typedef struct { 10 | union { 11 | struct { 12 | uint32_t magic; 13 | uint16_t timeShift; 14 | } values; 15 | char buffer[124]; 16 | } data; 17 | } SettingsV1; 18 | 19 | SettingsV1 settingsReadV1(); 20 | bool settingsWriteV1(SettingsV1* settings); 21 | SettingsV1* getCurrentSettings(); 22 | 23 | #endif -------------------------------------------------------------------------------- /src/power/power.c: -------------------------------------------------------------------------------- 1 | #include "stm32f10x_pwr.h" 2 | #include "display/display.h" 3 | #include "gps/neo6m.h" 4 | #include "accelerometer/mpu6050/mpu6050_i2c.h" 5 | #include "magnetometer/qmc5883l/qmc5883l.h" 6 | 7 | void goToSleepMode(void) { 8 | display->clear(); 9 | display->syncAll(); 10 | display->setBacklight(0); 11 | 12 | mpu6050_power(false); 13 | neo6m_power(false); 14 | qmc5883l_power(false); 15 | 16 | PWR_EnterSTANDBYMode(); 17 | } 18 | 19 | void initWakeup(void) { 20 | PWR_WakeUpPinCmd(ENABLE); 21 | } 22 | -------------------------------------------------------------------------------- /inc/hardware.h: -------------------------------------------------------------------------------- 1 | /* 2 | * hardware.h 3 | * 4 | * Created on: Apr 24, 2019 5 | * Author: raistlin 6 | */ 7 | 8 | #ifndef HARDWARE_H_ 9 | #define HARDWARE_H_ 10 | 11 | // uncomment this define for dummy board 12 | #define BOARD_DUMMY 13 | #define BUFFERED_DISPLAY 14 | // uncomment this define for production board 15 | //#define BOARD_PRODUCTION 16 | 17 | //dummy board hardware defines 18 | #ifdef BOARD_DUMMY 19 | #define DISPLAY_NOKIA5110 20 | #endif 21 | 22 | //production board hardware defines 23 | #ifdef BOARD_PRODUCTION 24 | #define DISPLAY_0152G 25 | #endif 26 | 27 | #endif /* HARDWARE_H_ */ 28 | -------------------------------------------------------------------------------- /inc/commons/quaternion.h: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNION_H 2 | #define QUATERNION_H 3 | 4 | #include 5 | #include "commons/vector3d.h" 6 | 7 | typedef struct quaternion { 8 | float w, x, y, z; 9 | } quaternion_t; 10 | 11 | quaternion_t quaternion_new_wxyz(float w, float x, float y, float z); 12 | quaternion_t quaternion_new_vec(const vector3d_t *rv, float angleRads); 13 | vector3d_t quaternion_transform_vec(const quaternion_t *q, const vector3d_t *v); 14 | 15 | float quaternion_pitch(const quaternion_t *q); 16 | float quaternion_roll(const quaternion_t *q); 17 | float quaternion_yaw(const quaternion_t *q); 18 | #endif // QUATERNION_H 19 | -------------------------------------------------------------------------------- /inc/commons/magnetic.h: -------------------------------------------------------------------------------- 1 | #ifndef MAGNETIC_H 2 | #define MAGNETIC_H 3 | 4 | /// 5 | /// \brief calculate 6 | /// \param alt - altitude in kilometers 7 | /// \param lat - decimal degrees 8 | /// \param lon - decimal degrees 9 | /// \param year - decimal (2015.0 - 2020.0) 10 | /// \param declination 11 | /// \param inclination 12 | /// \param totalIntensity 13 | /// \param gridVariation 14 | /// 15 | 16 | void magnetic_calculate(float alt, float lat, float lon, 17 | float year, float *declination, float *inclination, 18 | float *totalIntensity, float *gridVariation); 19 | 20 | #endif // MAGNETIC_H 21 | -------------------------------------------------------------------------------- /inc/display/nokiaLcd/nokia5110_buffered.h: -------------------------------------------------------------------------------- 1 | #ifndef __STM32F10X_PCD8544_BUFFERED_H_ 2 | #define __STM32F10X_PCD8544_BUFFERED_H_ 3 | 4 | #include 5 | #include 6 | #include "nokia5110_font.h" 7 | 8 | void nokia5110_bufferInit(void); 9 | void nokia5110_syncBufferPart(int16_t x0, int16_t y0, int16_t x1, int16_t y1); 10 | void nokia5110_syncBuffer(void); 11 | void nokia5110_clearBuffer(void); 12 | 13 | void nokia5110_bufferWriteString(int16_t x, int16_t y, const char *s); 14 | void nokia5110_bufferSetPixel(int16_t x, int16_t y); 15 | uint8_t nokia5110_bufferGetPixel(int16_t x, int16_t y); 16 | void nokia5110_bufferClearPixel(int16_t x, int16_t y); 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /inc/gps/neo6m.h: -------------------------------------------------------------------------------- 1 | #ifndef __NEO6M 2 | #define __NEO6M 3 | 4 | #include 5 | #include 6 | 7 | #include "stm32f10x.h" 8 | #include "nmea.h" 9 | #include "time/kov_time.h" 10 | 11 | typedef struct neo6m_data { 12 | float latitude; 13 | float longitude; 14 | float altitude; 15 | kov_time_t time; 16 | kov_date_t date; 17 | float speed_knots; 18 | float course; 19 | float magnetic_variation; //! 20 | } neo6m_data_t; 21 | 22 | void neo6m_init(void); 23 | void neo6m_power(bool on); 24 | void neo6m_interrupts(bool enable); 25 | 26 | extern volatile bool neo6m_needParseCurrentBuff; 27 | neo6m_data_t neo6m_parseCurrentBuff(bool *valid); 28 | 29 | #endif //__NEO6M 30 | -------------------------------------------------------------------------------- /inc/accelerometer/mpu6050/mpu6050_i2c.h: -------------------------------------------------------------------------------- 1 | #ifndef MPU6050_I2C_H 2 | #define MPU6050_I2C_H 3 | 4 | #include 5 | #include 6 | #include "MPU6050.h" 7 | #include "i2c1/i2c1.h" 8 | 9 | typedef union { 10 | uint8_t raw8[14]; 11 | int16_t raw16[7]; 12 | struct { 13 | int16_t ax, ay, az; 14 | int16_t t; 15 | int16_t gx, gy, gz; 16 | } data; 17 | } mpu6050_raw_data_t; 18 | 19 | void mpu6050_init(void); 20 | void mpu6050_pin_config(void); 21 | void mpu6050_power(bool on); 22 | 23 | void mpu6050ReadSync(mpu6050_raw_data_t *data); 24 | void mpu6050ReadAsync(mpu6050_raw_data_t *data, volatile int8_t *finishCode, i2c1_pf_callback cb); 25 | void mpu6050FixData(mpu6050_raw_data_t *imuRaw); 26 | 27 | #endif //MPU6050_I2C_H 28 | -------------------------------------------------------------------------------- /src/stm32vldiscovery_utils.c: -------------------------------------------------------------------------------- 1 | /* 2 | * stm32vldiscovery_utils.c 3 | * 4 | * Created on: Apr 22, 2019 5 | * Author: lezh1k 6 | */ 7 | #include "stm32vldiscovery_utils.h" 8 | 9 | volatile uint32_t *DWT_CONTROL = (uint32_t *) 0xE0001000; 10 | volatile uint32_t *DWT_CYCCNT = (uint32_t *) 0xE0001004; 11 | volatile uint32_t *DEMCR = (uint32_t *) 0xE000EDFC; 12 | volatile uint32_t *LAR = (uint32_t *) 0xE0001FB0; // <-- added lock access register 13 | 14 | void initDWT(void) { 15 | *DEMCR = *DEMCR | 0x01000000; // enable trace 16 | *LAR = 0xC5ACCE55; // <-- added unlock access to DWT (ITM, etc.)registers 17 | *DWT_CYCCNT = 0; // clear DWT cycle counter 18 | *DWT_CONTROL = *DWT_CONTROL | 1; // enable DWT cycle counter 19 | } 20 | -------------------------------------------------------------------------------- /inc/i2c1/i2c1.h: -------------------------------------------------------------------------------- 1 | #ifndef I2C1_H 2 | #define I2C1_H 3 | 4 | #include 5 | #include 6 | 7 | typedef enum { 8 | FC_FINISHED = 0, 9 | FC_IN_PROGRESS = 1, 10 | FC_I2C_ERR = 2, 11 | FC_DFA_ERR = 3 12 | } I2C1FinishCode; 13 | 14 | void i2c1_init(void); 15 | void i2c1_interrupts(FunctionalState on); 16 | 17 | typedef void (*i2c1_pf_callback)(I2C1FinishCode); 18 | 19 | void i2c1_read_buff_async(uint8_t slaveAddr, uint8_t startReg, uint8_t *buff, uint8_t len, volatile int8_t *finishCode, i2c1_pf_callback cb); 20 | I2C1FinishCode i2c1_read_buff_sync(uint8_t slaveAddr, uint8_t startReg, uint8_t *buff, uint8_t len); 21 | 22 | void i2c1_write_buff_async(uint8_t slaveAddr, uint8_t startReg, uint8_t *buff, uint8_t len, volatile int8_t *finishCode, i2c1_pf_callback cb); 23 | I2C1FinishCode i2c1_write_buff_sync(uint8_t slaveAddr, uint8_t startReg, uint8_t *buff, uint8_t len); 24 | 25 | void i2c1_scan(void (*cb)(uint8_t, uint8_t)); 26 | 27 | #endif // I2C1_H 28 | -------------------------------------------------------------------------------- /inc/display/display.h: -------------------------------------------------------------------------------- 1 | #ifndef __DISPLAY_H 2 | #define __DISPLAY_H 3 | 4 | #include 5 | #include 6 | 7 | typedef struct { 8 | int16_t x; 9 | int16_t y; 10 | } Point2d; 11 | 12 | typedef struct { 13 | void (*clear)(void); 14 | void (*init)(void); 15 | void (*syncAll)(void); 16 | void (*setBacklight)(bool isOn); 17 | 18 | void (*setPixel)(int16_t x, int16_t y); 19 | uint8_t (*getPixel)(int16_t x, int16_t y); 20 | void (*clearPixel)(int16_t x, int16_t y); 21 | 22 | void (*syncRect)(int16_t x0, int16_t y0, int16_t x1, int16_t y1); 23 | void (*printText)(int16_t x, int16_t y, const char *value); 24 | } Display_t; 25 | 26 | void calcCircleBuffer(uint16_t radius); 27 | void drawCircleFromBuffer(int16_t cx, int16_t cy, bool putPixel); 28 | void drawLine(int16_t x0, int16_t y0, int16_t x1, int16_t y1, bool putPixel); 29 | void drawArrow(int16_t arrowCenterX, int16_t arrowCenterY, float azimuth, bool paint); 30 | 31 | extern Display_t *display; 32 | Display_t *initDisplay(void); 33 | 34 | #endif 35 | -------------------------------------------------------------------------------- /src/commons/vector3d.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "commons/vector3d.h" 3 | 4 | vector3d_t vector_new(float x, float y, float z) { 5 | vector3d_t r = {.x = x, .y = y, .z = z}; 6 | r.len = sqrtf(x*x + y*y + z*z); 7 | return r; 8 | } 9 | /////////////////////////////////////////////////////// 10 | 11 | void vector_normalize(vector3d_t *v3d) { 12 | v3d->x /= v3d->len; 13 | v3d->y /= v3d->len; 14 | v3d->z /= v3d->len; 15 | v3d->len = 1.0f; //normalized 16 | } 17 | /////////////////////////////////////////////////////// 18 | 19 | float vector_rotationSign(const vector3d_t *a, 20 | const vector3d_t *b) { 21 | float sign = a->x * b->y - a->y * b->x; 22 | return sign >= 0.0f ? 1.0f : -1.0f; 23 | } 24 | /////////////////////////////////////////////////////// 25 | 26 | float vector_flatCos(const vector3d_t *a, 27 | const vector3d_t *b) { 28 | float num = a->x * b->x + a->y * b->y; 29 | float denom = sqrtf(a->x*a->x + a->y*a->y) * sqrtf(b->x*b->x + b->y*b->y); 30 | return num / denom; 31 | } 32 | 33 | -------------------------------------------------------------------------------- /inc/commons/commons.h: -------------------------------------------------------------------------------- 1 | #ifndef COMMONS_H 2 | #define COMMONS_H 3 | 4 | #include 5 | #include 6 | #define UNUSED(x) ((void)x) 7 | 8 | #define deg2rad_coeff ((float)M_PI / 180.0f) 9 | #define rad2deg_coeff (180.0f / (float)M_PI) 10 | 11 | /////////////////////////////////////////////////////// 12 | typedef struct { 13 | float ax, ay, az; 14 | } AccelerometerData; 15 | 16 | typedef struct { 17 | float gx, gy, gz; 18 | } GyroscopeData; 19 | 20 | typedef struct { 21 | float mx, my, mz; 22 | } MagnetometerData; 23 | 24 | typedef struct { 25 | AccelerometerData acc; 26 | GyroscopeData gyro; 27 | } IMUData; 28 | 29 | typedef struct { 30 | IMUData imu; 31 | MagnetometerData mag; 32 | } AHRSData; 33 | /////////////////////////////////////////////////////// 34 | 35 | char *int32ToStr(char *buff, int32_t val, uint8_t buff_len); 36 | char *uint32ToStr(char *buff, uint32_t val, uint8_t buff_len); 37 | 38 | char* ftoa(float f, char *buf, int precision); 39 | void waitTicks(int ticks); 40 | /////////////////////////////////////////////////////// 41 | #endif // COMMONS_H 42 | -------------------------------------------------------------------------------- /inc/time/kov_time.h: -------------------------------------------------------------------------------- 1 | #ifndef __KOV_TIME_H 2 | #define __KOV_TIME_H 3 | 4 | #include 5 | #include 6 | 7 | typedef struct kov_date { 8 | uint16_t day; 9 | uint8_t month; 10 | uint16_t year; 11 | } kov_date_t; 12 | 13 | typedef struct kov_time { 14 | uint8_t hours; 15 | uint8_t minutes; 16 | uint8_t seconds; 17 | uint8_t microseconds; 18 | } kov_time_t; 19 | 20 | typedef uint32_t kov_timestamp_t; 21 | 22 | void RTC_Init(void); 23 | kov_timestamp_t RTC_CurrentTimestamp(void); 24 | bool RTC_Adjust(kov_timestamp_t ts); 25 | 26 | //!WARNING!! These functions will work until 2106 February 07. Then need to change to 64 bit! or apply some offset :) 27 | void RTC_GetDateTime(kov_timestamp_t ts, kov_date_t *date, kov_time_t *time); 28 | kov_timestamp_t RTC_GetTimestampDateTime(const kov_date_t *date,const kov_time_t *time); 29 | kov_timestamp_t RTC_GetTimestamp(int32_t year, int32_t month, int32_t day, 30 | int32_t hours, int32_t minutes, int32_t seconds); 31 | 32 | const char *RTC_TimeStr(const kov_time_t *t); 33 | const char *RTC_DateStr(const kov_date_t *d); 34 | const char *RTC_DateTimeStr(const kov_date_t *d, const kov_time_t *t); 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /inc/commons/madgwick.h: -------------------------------------------------------------------------------- 1 | //===================================================================================================== 2 | // MadgwickAHRS.h 3 | //===================================================================================================== 4 | // 5 | // Implementation of Madgwick's IMU and AHRS algorithms. 6 | // See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms 7 | // 8 | // Date Author Notes 9 | // 29/09/2011 SOH Madgwick Initial release 10 | // 02/10/2011 SOH Madgwick Optimised for reduced CPU load 11 | // 12 | //===================================================================================================== 13 | #ifndef MADGWICK_H 14 | #define MADGWICK_H 15 | 16 | //---------------------------------------------------------------------------------------------------- 17 | // Variable declaration 18 | extern volatile float q0, q1, q2, q3; // quaternion of sensor frame relative to auxiliary frame 19 | 20 | void MadgwickInit(float freq, float gain); 21 | void MadgwickAHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz); 22 | void MadgwickAHRSupdateIMU(float gx, float gy, float gz, float ax, float ay, float az); 23 | 24 | #endif 25 | -------------------------------------------------------------------------------- /src/flash/settings.c: -------------------------------------------------------------------------------- 1 | #include "flash/settings.h" 2 | #include "flash/flash.h" 3 | 4 | #define SETTINGS_OFFSET (BANK1_PAGES - 1) * BANK1_PAGE_SIZE 5 | static SettingsV1 currentSettings; 6 | 7 | 8 | SettingsV1* getCurrentSettings() { 9 | if(currentSettings.data.values.magic != SETTINGS_MAGIC_V1) { 10 | currentSettings = settingsReadV1(); 11 | } 12 | return ¤tSettings; 13 | } 14 | 15 | SettingsV1 defaultSettingsV1() { 16 | SettingsV1 result; 17 | memset(&result, 0, sizeof(SettingsV1)); 18 | //result.data.values.magic != SETTINGS_MAGIC_V1; 19 | return result; 20 | } 21 | 22 | SettingsV1 settingsReadV1() { 23 | SettingsV1 result; 24 | memset(&result, 0, sizeof(SettingsV1)); 25 | if(!flashInit()) 26 | return result; 27 | 28 | if(!flashRead(SETTINGS_OFFSET, &result, sizeof(SettingsV1))) { 29 | memset(&result, 0, sizeof(SettingsV1)); 30 | } 31 | 32 | if(result.data.values.magic != SETTINGS_MAGIC_V1) { 33 | result = defaultSettingsV1(); 34 | } 35 | 36 | currentSettings = result; 37 | return result; 38 | } 39 | 40 | bool settingsWriteV1(SettingsV1* settings) { 41 | if(!flashInit()) 42 | return false; 43 | flashUnlock(); 44 | settings->data.values.magic = SETTINGS_MAGIC_V1; 45 | bool result = flashWrite(SETTINGS_OFFSET, settings, sizeof(SettingsV1)); 46 | flashLock(); 47 | return result; 48 | } -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/mad_footprints.pretty/Nokia5110_blue.kicad_mod: -------------------------------------------------------------------------------- 1 | (module Nokia5110_blue (layer F.Cu) (tedit 5CF0C6C5) 2 | (fp_text reference REF** (at 11.43 -22.86) (layer F.SilkS) 3 | (effects (font (size 1 1) (thickness 0.15))) 4 | ) 5 | (fp_text value Nokia5110_blue (at 11.43 -30.48) (layer F.Fab) 6 | (effects (font (size 1 1) (thickness 0.15))) 7 | ) 8 | (fp_line (start -12.7 -46.99) (end 33.3 -46.99) (layer F.SilkS) (width 0.12)) 9 | (fp_line (start 33.3 -46.99) (end 33.3 3.01) (layer F.SilkS) (width 0.12)) 10 | (fp_line (start 33.3 3.01) (end -12.7 3.01) (layer F.SilkS) (width 0.12)) 11 | (fp_line (start -12.7 3.01) (end -12.7 -46.99) (layer F.SilkS) (width 0.12)) 12 | (pad 1 thru_hole rect (at 0 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 13 | (pad 2 thru_hole circle (at 2.54 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 14 | (pad 3 thru_hole circle (at 5.08 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 15 | (pad 4 thru_hole circle (at 7.62 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 16 | (pad 5 thru_hole circle (at 10.16 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 17 | (pad 6 thru_hole circle (at 12.7 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 18 | (pad 7 thru_hole circle (at 15.24 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 19 | (pad 8 thru_hole circle (at 17.78 0) (size 1.524 1.524) (drill 0.762) (layers *.Cu *.Mask)) 20 | ) 21 | -------------------------------------------------------------------------------- /src/flash/flash.c: -------------------------------------------------------------------------------- 1 | #include "flash/flash.h" 2 | #include "stm32f10x_flash.h" 3 | 4 | #define BANK1_START_ADDR ((uint32_t)0x08008000) 5 | #define BANK1_END_ADDR (BANK1_START_ADDR + (uint32_t)(BANK1_PAGES * BANK1_PAGE_SIZE)) 6 | 7 | volatile FLASH_Status FLASHStatus = FLASH_COMPLETE; 8 | 9 | bool flashInit() { 10 | return true; 11 | } 12 | 13 | bool flashLock() { 14 | FLASH_LockBank1(); 15 | } 16 | 17 | bool flashUnlock() { 18 | FLASH_UnlockBank1(); 19 | } 20 | 21 | bool flashRead(uint32_t startAddress, void* buffer, uint16_t size) { 22 | if(BANK1_START_ADDR + startAddress + size > BANK1_END_ADDR) { 23 | size = BANK1_END_ADDR - (BANK1_START_ADDR + startAddress); 24 | } 25 | memcpy(buffer, BANK1_START_ADDR + startAddress, size); 26 | } 27 | 28 | bool flashWrite(uint32_t startAddress, void* buffer, uint16_t size) { 29 | uint32_t endAddress = BANK1_START_ADDR + startAddress + size; 30 | uint16_t startPage = (BANK1_START_ADDR + startAddress) / BANK1_PAGE_SIZE; 31 | uint16_t endPage = (BANK1_START_ADDR + startAddress + size) / BANK1_PAGE_SIZE; 32 | flashUnlock(); 33 | FLASH_ClearFlag(FLASH_FLAG_EOP | FLASH_FLAG_PGERR | FLASH_FLAG_WRPRTERR); 34 | for(uint16_t page = 0; page <= (endPage - startPage); page++) { 35 | FLASHStatus = FLASH_ErasePage(BANK1_START_ADDR + (BANK1_PAGE_SIZE * page)); 36 | } 37 | 38 | uint32_t *data = buffer; 39 | for(uint32_t address = BANK1_START_ADDR + startAddress; (address < endAddress) && (FLASHStatus == FLASH_COMPLETE); address += 4) { 40 | FLASHStatus = FLASH_ProgramWord(address, data); 41 | data++; 42 | } 43 | 44 | flashLock(); 45 | } 46 | -------------------------------------------------------------------------------- /src/accelerometer/mpu6050/mpu6050_i2c.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "accelerometer/mpu6050/mpu6050_i2c.h" 3 | #include "commons/commons.h" 4 | #include "i2c1/i2c1.h" 5 | 6 | #define MPU6050_Port GPIOB 7 | #define MPU6050_RCC_Port RCC_APB2Periph_GPIOB 8 | #define MPU6050_VCC_PIN GPIO_Pin_8 9 | 10 | void mpu6050_pin_config() { 11 | GPIO_InitTypeDef GPIO_cfg; 12 | RCC_APB2PeriphClockCmd(MPU6050_RCC_Port, ENABLE); 13 | 14 | GPIO_cfg.GPIO_Pin = MPU6050_VCC_PIN; 15 | GPIO_cfg.GPIO_Speed = GPIO_Speed_10MHz; 16 | GPIO_cfg.GPIO_Mode = GPIO_Mode_Out_PP; 17 | GPIO_Init(MPU6050_Port, &GPIO_cfg); 18 | } 19 | 20 | void mpu6050_power(bool on) { 21 | void (*gpio_change)(GPIO_TypeDef*, uint16_t) = on ? GPIO_SetBits : GPIO_ResetBits; 22 | gpio_change(MPU6050_Port, MPU6050_VCC_PIN); 23 | } 24 | /////////////////////////////////////////////////////// 25 | 26 | void mpu6050_init(void) { 27 | MPU6050_Initialize(); 28 | } 29 | /////////////////////////////////////////////////////// 30 | 31 | void mpu6050ReadAsync(mpu6050_raw_data_t *data, volatile int8_t *finishCode, i2c1_pf_callback cb) { 32 | i2c1_read_buff_async(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, data->raw8, 14, finishCode, cb); 33 | } 34 | /////////////////////////////////////////////////////// 35 | 36 | void mpu6050ReadSync(mpu6050_raw_data_t *data) { 37 | i2c1_read_buff_sync(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_ACCEL_XOUT_H, data->raw8, 14); 38 | } 39 | /////////////////////////////////////////////////////// 40 | 41 | void mpu6050FixData(mpu6050_raw_data_t *imuRaw) { 42 | for (int i = 0; i < 7; ++i) 43 | imuRaw->raw16[i] = (int16_t) ((uint16_t) imuRaw->raw8[2 * i] << 8) | imuRaw->raw8[2 * i + 1]; 44 | } 45 | /////////////////////////////////////////////////////// 46 | -------------------------------------------------------------------------------- /Readme.md: -------------------------------------------------------------------------------- 1 | # Mad navigator 2 | 3 | ## Description 4 | 5 | Mad navigator is simple compass-like device shows direction to predefined geo point. 6 | This repository contains firmware source code and schematics for device. 7 | 8 | [![Developed by Mad Devs](https://maddevs.io/badge-light.svg)](https://maddevs.io) 9 | 10 | ## Technologies and modules 11 | 12 | - Microcontroller [STM32F100RBT6](https://www.st.com/content/ccc/resource/technical/document/reference_manual/a2/2d/02/4b/78/57/41/a3/CD00246267.pdf/files/CD00246267.pdf/jcr:content/translations/en.CD00246267.pdf) 13 | - LCD display [Nokia 5110](https://www.sparkfun.com/datasheets/LCD/Monochrome/Nokia5110.pdf) 14 | - Accelerometer + gyroscope [MPU6050](https://store.invensense.com/datasheets/invensense/MPU-6050_DataSheet_V3%204.pdf) 15 | - GPS receiver [Neo-6M](https://www.u-blox.com/sites/default/files/products/documents/NEO-6_DataSheet_%28GPS.G6-HW-09005%29.pdf) 16 | - Magnetometer [QMC5883L](https://nettigo.pl/attachments/440) 17 | 18 | ## Prerequisites 19 | 20 | - make 21 | - arm-none-eabi-* (gcc, gdb, ar etc.) 22 | - st-link utils 23 | 24 | ## Folder structure 25 | 26 | root: Makefile, qtcreator project file (not neccessary to use it), LinkerScript 27 | 28 | inc: All include files 29 | 30 | src: All firmware source files 31 | 32 | startup: Standard startup code 33 | 34 | kicad_schema: Schematic 35 | 36 | ## How to run/debug 37 | 38 | ### RUN 39 | 40 | Just use `make clean all program` . Make sure that your device connected via USB and you have all needed permissions. 41 | You don't need to `make clean` every time. 42 | 43 | ### DEBUG 44 | 45 | 1. `make clean all program` 46 | 2. run `st-util`. It will launch gdb server on localhost:4242. 47 | 3. use arm-none-eabi-gdb 48 | 49 | ## Extended documentation 50 | 51 | !TODO add link to medium 52 | 53 | -------------------------------------------------------------------------------- /inc/stm32f10x_it.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_it.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 08-April-2011 7 | * @brief This file contains the headers of the interrupt handlers. 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 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __STM32F10x_IT_H 24 | #define __STM32F10x_IT_H 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | /* Includes ------------------------------------------------------------------*/ 31 | #include "stm32f10x.h" 32 | 33 | /* Exported types ------------------------------------------------------------*/ 34 | /* Exported constants --------------------------------------------------------*/ 35 | /* Exported macro ------------------------------------------------------------*/ 36 | /* Exported functions ------------------------------------------------------- */ 37 | 38 | void NMI_Handler(void); 39 | void HardFault_Handler(void); 40 | void MemManage_Handler(void); 41 | void BusFault_Handler(void); 42 | void UsageFault_Handler(void); 43 | void SVC_Handler(void); 44 | void DebugMon_Handler(void); 45 | void PendSV_Handler(void); 46 | void SysTick_Handler(void); 47 | 48 | #ifdef __cplusplus 49 | } 50 | #endif 51 | 52 | #endif /* __STM32F10x_IT_H */ 53 | 54 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 55 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/mad_footprints.pretty/NEO6-M.kicad_mod: -------------------------------------------------------------------------------- 1 | (module NEO6-M (layer F.Cu) (tedit 5CEFC514) 2 | (fp_text reference REF** (at 0.9 -0.1) (layer F.SilkS) 3 | (effects (font (size 1 1) (thickness 0.15))) 4 | ) 5 | (fp_text value NEO6-M (at 0 -3.1) (layer F.Fab) 6 | (effects (font (size 1 1) (thickness 0.15))) 7 | ) 8 | (pad 12 smd rect (at 5.8 -7.2) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 9 | (pad 11 smd rect (at 5.8 -6.1) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 10 | (pad 10 smd rect (at 5.8 -5) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 11 | (pad 9 smd rect (at 5.8 -3.9) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 12 | (pad 8 smd rect (at 5.8 -2.8) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 13 | (pad 7 smd rect (at 5.8 0.2) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 14 | (pad 6 smd rect (at 5.8 1.3) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 15 | (pad 5 smd rect (at 5.8 2.4) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 16 | (pad 4 smd rect (at 5.8 3.5) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 17 | (pad 3 smd rect (at 5.8 4.6) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 18 | (pad 2 smd rect (at 5.8 5.7) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 19 | (pad 1 smd rect (at 5.8 6.8) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 20 | (pad 18 smd rect (at -5.6 0.2) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 21 | (pad 24 smd rect (at -5.6 6.8) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 22 | (pad 20 smd rect (at -5.6 2.4) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 23 | (pad 19 smd rect (at -5.6 1.3) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 24 | (pad 14 smd rect (at -5.6 -6.1) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 25 | (pad 13 smd rect (at -5.6 -7.2) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 26 | (pad 16 smd rect (at -5.6 -3.9) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 27 | (pad 23 smd rect (at -5.6 5.7) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 28 | (pad 17 smd rect (at -5.6 -2.8) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 29 | (pad 15 smd rect (at -5.6 -5) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 30 | (pad 22 smd rect (at -5.6 4.6) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 31 | (pad 21 smd rect (at -5.6 3.5) (size 0.9 0.8) (layers F.Cu F.Paste F.Mask)) 32 | ) 33 | -------------------------------------------------------------------------------- /CMSIS/device/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 | -------------------------------------------------------------------------------- /src/magnetometer/qmc5883l/qmc5883l.c: -------------------------------------------------------------------------------- 1 | #include "magnetometer/qmc5883l/qmc5883l.h" 2 | 3 | #define CURRENT_RNG QMC5883L_CTRL1_RNG_2 4 | 5 | void qmc5883l_init() { 6 | uint8_t ctrl1 = (QMC5883L_CTRL1_MODE_CONTINUOUS << QMC5883L_CTRL1_MODE_OFFSET) | 7 | (QMC5883L_CTRL1_ODR_100 << QMC5883L_CTRL1_ODR_OFFSET) | 8 | (CURRENT_RNG << QMC5883L_CTRL1_RNG_OFFSET) | 9 | (QMC5883L_CTRL1_OSR_128 << QMC5883L_CTRL1_OSR_OFFSET); 10 | uint8_t ctrl2 = QMC5883L_CTRL2_INT_DIS; 11 | 12 | i2c1_write_buff_sync(QMC5883L_I2C_ADDR, QMC5883L_CTRL1, &ctrl1, 1); 13 | i2c1_write_buff_sync(QMC5883L_I2C_ADDR, QMC5883L_CTRL2, &ctrl2, 1); 14 | } 15 | /////////////////////////////////////////////////////// 16 | 17 | #define QMC5883L_Port GPIOB 18 | #define QMC5883L_RCC_Port RCC_APB2Periph_GPIOB 19 | #define QMC5883L_VCC_PIN GPIO_Pin_9 20 | 21 | void qmc5883l_pin_config() { 22 | GPIO_InitTypeDef GPIO_cfg; 23 | RCC_APB2PeriphClockCmd(QMC5883L_RCC_Port, ENABLE); 24 | 25 | GPIO_cfg.GPIO_Pin = QMC5883L_VCC_PIN; 26 | GPIO_cfg.GPIO_Speed = GPIO_Speed_10MHz; 27 | GPIO_cfg.GPIO_Mode = GPIO_Mode_Out_PP; 28 | GPIO_Init(QMC5883L_Port, &GPIO_cfg); 29 | } 30 | /////////////////////////////////////////////////////// 31 | 32 | void qmc5883l_power(bool on) { 33 | void (*gpio_change)(GPIO_TypeDef*, uint16_t) = on ? GPIO_SetBits : GPIO_ResetBits; 34 | gpio_change(QMC5883L_Port, QMC5883L_VCC_PIN); 35 | } 36 | /////////////////////////////////////////////////////// 37 | 38 | void qmc5883lReadHeadingSync(qmc5883l_raw_data_t *data) { 39 | i2c1_read_buff_sync(QMC5883L_I2C_ADDR, QMC5883L_X_LSB, data->raw8, 6); 40 | } 41 | /////////////////////////////////////////////////////// 42 | 43 | 44 | void qmc5883lReadHeadingAsync(qmc5883l_raw_data_t *data, 45 | volatile int8_t *finishCode, 46 | i2c1_pf_callback cb) { 47 | i2c1_read_buff_async(QMC5883L_I2C_ADDR, QMC5883L_X_LSB, data->raw8, 6, finishCode, cb); 48 | } 49 | /////////////////////////////////////////////////////// 50 | 51 | float qmc5883l_gain() { 52 | switch (CURRENT_RNG) { 53 | case(QMC5883L_CTRL1_RNG_2): 54 | return 12000.0f; 55 | case(QMC5883L_CTRL1_RNG_8): 56 | return 3000.0f; 57 | } 58 | return 1.0f; 59 | } 60 | /////////////////////////////////////////////////////// 61 | 62 | void qmc5883lFixRawData(qmc5883l_raw_data_t *data) { 63 | //LSB then MSB. need to reorder. so: 64 | for (int i = 0; i < 3; ++i) 65 | data->raw16[i] = (int16_t) (((uint16_t) data->raw8[2*i + 1] << 8) | data->raw8[2*i]); 66 | } 67 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /inc/magnetometer/lis3mdl/lis3mdl_i2c.h: -------------------------------------------------------------------------------- 1 | #ifndef LIS3MDL_I2C_H 2 | #define LIS3MDL_I2C_H 3 | 4 | 5 | #include 6 | #include 7 | #include "stm32f10x.h" 8 | 9 | #define LIS3MDL_ADDRESS_HIGH 0x1e 10 | #define LIS3MDL_ADDRESS_LOW 0x1c 11 | 12 | #define LIS3MDL_PERFORMANCE_LOW_POWER 0x00 13 | #define LIS3MDL_PERFORMANCE_MEDIUM 0x01 14 | #define LIS3MDL_PERFORMANCE_HIGH 0x02 15 | #define LIS3MDL_PERFORMANCE_ULTRA_HIGH 0x03 16 | 17 | #define LIS3MDL_PRECISION_LP 18 | 19 | #define LIS3MDL_DATA_RATE_0_625_HZ 0x00 20 | #define LIS3MDL_DATA_RATE_1_25_HZ 0x01 21 | #define LIS3MDL_DATA_RATE_2_5_HZ 0x02 22 | #define LIS3MDL_DATA_RATE_5_HZ 0x03 23 | #define LIS3MDL_DATA_RATE_10_HZ 0x04 24 | #define LIS3MDL_DATA_RATE_20_HZ 0x05 25 | #define LIS3MDL_DATA_RATE_40_HZ 0x06 26 | #define LIS3MDL_DATA_RATE_80_HZ 0x07 27 | 28 | #define LIS3MDL_MODE_CONTINUOUS 0x00 29 | #define LIS3MDL_MODE_SINGLE 0x01 30 | #define LIS3MDL_MODE_POWER_DOWN 0x03 31 | 32 | #define LIS3MDL_SCALE_4_GAUSS 0x00 33 | #define LIS3MDL_SCALE_8_GAUSS 0x01 34 | #define LIS3MDL_SCALE_12_GAUSS 0x02 35 | #define LIS3MDL_SCALE_16_GAUSS 0x03 36 | 37 | #define LIS3MDL_AXIS_X 0 38 | #define LIS3MDL_AXIS_Y 1 39 | #define LIS3MDL_AXIS_Z 2 40 | 41 | #define LIS3MDL_STATUS_ZYXOR 0x80 42 | #define LIS3MDL_STATUS_ZOR 0x40 43 | #define LIS3MDL_STATUS_YOR 0x20 44 | #define LIS3MDL_STATUS_XOR 0x10 45 | #define LIS3MDL_STATUS_ZYXDA 0x08 46 | #define LIS3MDL_STATUS_ZDA 0x04 47 | #define LIS3MDL_STATUS_YDA 0x02 48 | #define LIS3MDL_STATUS_XDA 0x01 49 | 50 | #define LIS3MDL_DEVICE_ID 0x3d 51 | 52 | typedef struct { 53 | uint8_t address; 54 | uint16_t scale; 55 | int16_t min[3]; 56 | int16_t max[3]; 57 | } lis3mdl_t; 58 | 59 | typedef int8_t lis3mdl_err_t; 60 | 61 | lis3mdl_err_t LIS3MDL_setup(lis3mdl_t* lis3mdl, uint8_t address); 62 | lis3mdl_err_t LIS3MDL_reset(lis3mdl_t* lis3mdl); 63 | 64 | void LIS3MDL_clearMinMax(lis3mdl_t* lis3mdl); 65 | void LIS3MDL_setMinMax(lis3mdl_t* lis3mdl, uint8_t axis, int16_t min, int16_t max); 66 | 67 | float LIS3MDL_currentGain(const lis3mdl_t *lis3mdl); 68 | 69 | void LIS3MDL_enableTemperature(lis3mdl_t* lis3mdl, bool enable); 70 | void LIS3MDL_enableODR(lis3mdl_t *lis3mdl, bool enable); 71 | 72 | void LIS3MDL_setPerformance(lis3mdl_t* lis3mdl, uint8_t performance); 73 | void LIS3MDL_setDataRate(lis3mdl_t* lis3mdl, uint8_t dataRate); 74 | void LIS3MDL_setMode(lis3mdl_t* lis3mdl, uint8_t mode); 75 | void LIS3MDL_setScale(lis3mdl_t* lis3mdl, uint8_t scale); 76 | 77 | void LIS3MDL_read3AxisSync(lis3mdl_t* lis3mdl, int16_t value[3]); 78 | void LIS3MDL_readAxis(lis3mdl_t* lis3mdl, uint8_t axis, int16_t* value); 79 | 80 | void LIS3MDL_readTemperature(lis3mdl_t* lis3mdl, int16_t* value); 81 | lis3mdl_err_t LIS3MDL_readDeviceId(lis3mdl_t* lis3mdl, uint8_t* deviceId); 82 | void LIS3MDL_readStatus(lis3mdl_t* lis3mdl, uint8_t* status); 83 | 84 | #endif // LIS3MDL_I2C_H 85 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | ARM_TOOLCHAIN_PREFIX = arm-none-eabi- 2 | CC=$(ARM_TOOLCHAIN_PREFIX)gcc 3 | AS=$(ARM_TOOLCHAIN_PREFIX)as 4 | LD=$(ARM_TOOLCHAIN_PREFIX)ld 5 | OBJDUMP=$(ARM_TOOLCHAIN_PREFIX)objdump 6 | OBJCOPY=$(ARM_TOOLCHAIN_PREFIX)objcopy 7 | SIZE=$(ARM_TOOLCHAIN_PREFIX)size 8 | 9 | BUILD_DIR=build 10 | BIN_DIR=bin 11 | CMSIS_DIR=CMSIS 12 | STD_PERIPH_DRIVER_DIR=StdPeriph_Driver 13 | 14 | LIBS = -lm 15 | DEFS = -DSTM32 -DSTM32F1 -DSTM32F100RBTx -DSTM32F10X_MD_VL -DUSE_STDPERIPH_DRIVER -DBEARING_HIGH_PRECISION 16 | WARN_LEVEL = -Wall 17 | 18 | #device and program 19 | PRG = kov 20 | MMCU = -mcpu=cortex-m3 -mthumb -mfloat-abi=soft 21 | OPTIMIZE = -O0 -g3 22 | INCLUDES = -Iinc -Iinc/gps -Iinc/accelerometer -Iinc/magnetometer -Iinc/nokiaLcd -I$(CMSIS_DIR)/core -I$(CMSIS_DIR)/device -I$(STD_PERIPH_DRIVER_DIR)/inc 23 | 24 | CFLAGS = $(INCLUDES) $(MMCU) $(OPTIMIZE) $(DEFS) $(WARN_LEVEL) -fmessage-length=0 -ffunction-sections -MMD -MP 25 | 26 | LDFLAGS = $(MMCU) -Wl,-T,LinkerScript.ld -Wl,-Map,$(BIN_DIR)/$(PRG).map -Wl,--gc-sections -Wl,-print-memory-usage 27 | 28 | OSRC0=$(wildcard *.c) $(wildcard src/*.c) $(wildcard src/accelerometer/*.c) $(wildcard src/accelerometer/*/*.c) 29 | OSRC1=$(wildcard src/*/*.c) $(wildcard src/display/*.c) $(wildcard src/display/*/*.c) $(wildcard src/magnetometer/*.c) $(wildcard src/magnetometer/*/*.c) 30 | OSRC2=$(wildcard $(CMSIS_DIR)/*/*.c) 31 | OSRC3=$(wildcard $(STD_PERIPH_DRIVER_DIR)/*/*.c) 32 | OSRC4=$(wildcard startup/*.s) 33 | 34 | OBJECTS=$(patsubst %.c, %.o, $(OSRC0) $(OSRC1) $(OSRC2) $(OSRC3)) $(patsubst %.s, %.o, $(OSRC4)) 35 | 36 | all: directories $(PRG) 37 | startup/startup_stm32.o: startup/startup_stm32.s 38 | $(AS) $(INCLUDES) $(MMCU) -g -o $@ $^ 39 | 40 | $(PRG): $(BIN_DIR)/$(PRG).elf $(BIN_DIR)/lst $(BIN_DIR)/$(PRG).bin 41 | 42 | $(BIN_DIR)/$(PRG).elf: $(OBJECTS) 43 | $(CC) $(LDFLAGS) -o $@ $^ $(LIBS) 44 | 45 | $(BIN_DIR)/lst: $(BIN_DIR)/$(PRG).lst 46 | $(BIN_DIR)/%.lst: $(BIN_DIR)/%.elf 47 | $(OBJDUMP) -h -S $< > $@ 48 | 49 | $(BIN_DIR)/bin: $(BIN_DIR)/$(PRG).bin 50 | $(BIN_DIR)/%.bin: $(BIN_DIR)/%.elf 51 | $(OBJCOPY) -O binary $< $@ 52 | 53 | directories: 54 | @mkdir -p $(BUILD_DIR) 55 | @mkdir -p $(BIN_DIR) 56 | 57 | clean: 58 | @rm -rf $(BUILD_DIR)/* 59 | @rm -rf $(BIN_DIR)/* 60 | @rm -rf CMSIS/core/*.o CMSIS/core/*.d 61 | @rm -rf StdPeriph_Driver/src/*.o StdPeriph_Driver/src/*.d 62 | @rm -rf src/commons/*.o src/commons/*.d 63 | @rm -rf src/display/*.o src/display/*.d 64 | @rm -rf src/magnetometer/*.o src/magnetometer/lis3mdl/*.o src/magnetometer/hmc5883l/*.o src/magnetometer/qmc5883l/*.o 65 | @rm -rf src/accelerometer/*.o src/accelerometer/mpu6050/*.o 66 | @rm -rf src/gps/*.o src/gps/*.d 67 | @rm -rf src/power/*.o src/power/*.d 68 | @rm -rf src/time/*.o src/time/*.d 69 | @rm -rf startup/*.o 70 | 71 | mrproper: 72 | @rm -rf $(BUILD_DIR) 73 | @rm -rf $(BIN_DIR) 74 | 75 | program: 76 | @st-flash write $(BIN_DIR)/$(PRG).bin 0x8000000 77 | 78 | flash: 79 | @st-flash write $(BIN_DIR)/$(PRG).bin 0x8000000 80 | -------------------------------------------------------------------------------- /src/display/nokiaLcd/nokia5110_buffered.c: -------------------------------------------------------------------------------- 1 | /* 2 | * stm32f10x_pcd8544_buffered.c 3 | * 4 | * Created on: Apr 29, 2019 5 | * Author: raistlin 6 | */ 7 | 8 | #include 9 | #include "display/nokiaLcd/nokia5110_buffered.h" 10 | #include "display/nokiaLcd/nokia5110.h" 11 | #include "commons/commons.h" 12 | 13 | static uint8_t screenBuffer[DISPLAY_WIDTH][DISPLAY_BANKS]; 14 | static void nokia5110_bufferWriteChar(int8_t x, int8_t y, char c); 15 | 16 | void nokia5110_bufferInit(void){ 17 | memset(screenBuffer, 0, DISPLAY_WIDTH * DISPLAY_BANKS); 18 | nokia5110_init(); 19 | } 20 | /////////////////////////////////////////////////////// 21 | 22 | void nokia5110_syncBufferPart(int16_t x0, int16_t y0, int16_t x1, int16_t y1) { 23 | for(int16_t y = y0; y < y1; y++) { 24 | // nokia5110_gotoXY((int8_t)y, (int8_t)x0); 25 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x40 | (uint8_t)y); 26 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x80 | (uint8_t)x0); 27 | 28 | for(int16_t x = x0; x < x1; x++) { 29 | nokia5110_spi_writeByte(PCD8544_MODE_Data, screenBuffer[x][y]); 30 | } 31 | } 32 | } 33 | /////////////////////////////////////////////////////// 34 | 35 | void nokia5110_syncBuffer() { 36 | nokia5110_syncBufferPart(0, 0, DISPLAY_WIDTH, DISPLAY_BANKS); 37 | } 38 | /////////////////////////////////////////////////////// 39 | 40 | void nokia5110_clearBuffer() { 41 | memset(screenBuffer, 0, DISPLAY_WIDTH * DISPLAY_BANKS); 42 | } 43 | /////////////////////////////////////////////////////// 44 | 45 | void nokia5110_bufferWriteString(int16_t x, int16_t y, const char *s) { 46 | uint8_t i = 0; 47 | for (; *s; ++s, ++i) { 48 | nokia5110_bufferWriteChar((int8_t)((x+i)*DISPLAY_BANKS), (int8_t)y, *s); 49 | } 50 | } 51 | /////////////////////////////////////////////////////// 52 | 53 | void nokia5110_bufferWriteChar(int8_t x, int8_t y, char c) { 54 | char tmp = c - 0x20; 55 | for(int8_t i = 0; i < DISPLAY_BANKS; i++) 56 | screenBuffer[x + i][y] = font6_8[(uint8_t)tmp][i]; 57 | } 58 | /////////////////////////////////////////////////////// 59 | 60 | void nokia5110_bufferSetPixel(int16_t x, int16_t y) { 61 | uint8_t bank = (uint8_t)(y >> 3); 62 | uint8_t bit = (uint8_t)(y - (bank << 3)); 63 | uint8_t c = (uint8_t)((1 << bit) | screenBuffer[x][bank]); 64 | screenBuffer[x][bank] = c; 65 | } 66 | /////////////////////////////////////////////////////// 67 | 68 | void nokia5110_bufferClearPixel(int16_t x, int16_t y) { 69 | uint8_t bank = (uint8_t)(y >> 3); 70 | uint8_t bit = (uint8_t)(y - (bank << 3)); 71 | uint8_t c = (0xff ^ (1 << bit)) & screenBuffer[x][bank]; 72 | screenBuffer[x][bank] = c; 73 | } 74 | /////////////////////////////////////////////////////// 75 | 76 | uint8_t nokia5110_bufferGetPixel(int16_t x, int16_t y) { 77 | uint8_t bank = (uint8_t)y >> 3; 78 | uint8_t bit = (uint8_t)(y - (bank << 3)); 79 | return screenBuffer[x][bank] & (1 << bit); 80 | // uint8_t c = screenBuffer[x][bank] >> bit; 81 | // return c & 1; 82 | } 83 | /////////////////////////////////////////////////////// 84 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/mad_symbols.lib: -------------------------------------------------------------------------------- 1 | EESchema-LIBRARY Version 2.4 2 | #encoding utf-8 3 | # 4 | # 24AA32A 5 | # 6 | DEF 24AA32A U 0 40 Y Y 1 F N 7 | F0 "U" 0 350 50 H V C CNN 8 | F1 "24AA32A" 0 50 50 H V C CNN 9 | F2 "" 0 -750 50 H I C CNN 10 | F3 "" 0 -750 50 H I C CNN 11 | DRAW 12 | S -350 250 350 -250 0 1 0 N 13 | X A0 1 -450 150 100 R 50 50 1 1 I 14 | X A1 2 -450 50 100 R 50 50 1 1 I 15 | X A2 3 -450 -50 100 R 50 50 1 1 I 16 | X GND 4 -450 -150 100 R 50 50 1 1 I 17 | X SDA 5 450 -150 100 L 50 50 1 1 B 18 | X SCL 6 450 -50 100 L 50 50 1 1 I 19 | X WP 7 450 50 100 L 50 50 1 1 I 20 | X VCC 8 450 150 100 L 50 50 1 1 I 21 | ENDDRAW 22 | ENDDEF 23 | # 24 | # LIS3MDL 25 | # 26 | DEF LIS3MDL L 0 40 Y Y 1 F N 27 | F0 "L" 350 400 50 H V C CNN 28 | F1 "LIS3MDL" -350 400 50 H V C CNN 29 | F2 "" -750 550 50 H I C CNN 30 | F3 "" -750 550 50 H I C CNN 31 | DRAW 32 | S -400 350 400 -350 0 1 0 f 33 | X SCL/SPC 1 500 100 100 L 50 50 1 1 O 34 | X CS 10 -500 100 100 R 50 50 1 1 I 35 | X SDA 11 -100 450 100 D 50 50 1 1 I 36 | X RES 12 100 450 100 D 50 50 1 1 I 37 | X RES 2 500 0 100 L 50 50 1 1 O 38 | X GND 3 500 -100 100 L 50 50 1 1 W 39 | X C1 4 500 -200 100 L 50 50 1 1 I 40 | X VDD 5 100 -450 100 U 50 50 1 1 I 41 | X VDD_IO 6 -100 -450 100 U 50 50 1 1 I 42 | X INT 7 -500 -200 100 R 50 50 1 1 I 43 | X DRDY 8 -500 -100 100 R 50 50 1 1 I 44 | X SD0/SA1 9 -500 0 100 R 50 50 1 1 I 45 | ENDDRAW 46 | ENDDEF 47 | # 48 | # NEO-6M 49 | # 50 | DEF NEO-6M N 0 40 Y Y 1 F N 51 | F0 "N" 0 750 50 H V C CNN 52 | F1 "NEO-6M" 0 450 50 H V C CNN 53 | F2 "" -1590 900 50 H I C CNN 54 | F3 "" -1590 900 50 H I C CNN 55 | DRAW 56 | S -600 650 600 -650 0 1 0 f 57 | X RESERVED 1 700 -550 100 L 50 50 1 1 N 58 | X GND 10 700 350 100 L 50 50 1 1 I 59 | X RF_IN 11 700 450 100 L 50 50 1 1 I 60 | X GND 12 700 550 100 L 50 50 1 1 I 61 | X GND 13 -700 -550 100 R 50 50 1 1 I 62 | X MOSI/CFG_COM0 14 -700 -450 100 R 50 50 1 1 B 63 | X MOSI/CFG_COM1 15 -700 -350 100 R 50 50 1 1 I 64 | X CFG_GPS0/SCK 16 -700 -250 100 R 50 50 1 1 I 65 | X RESERVED 17 -700 -150 100 R 50 50 1 1 N 66 | X SDA2 18 -700 -50 100 R 50 50 1 1 B 67 | X SCL2 19 -700 50 100 R 50 50 1 1 B 68 | X SS_N 2 700 -450 100 L 50 50 1 1 I 69 | X TxD1 20 -700 150 100 R 50 50 1 1 O 70 | X RxD1 21 -700 250 100 R 50 50 1 1 I 71 | X V_BCKP 22 -700 350 100 R 50 50 1 1 W 72 | X VCC 23 -700 450 100 R 50 50 1 1 W 73 | X GND 24 -700 550 100 R 50 50 1 1 I 74 | X TIMEPULSE 3 700 -350 100 L 50 50 1 1 O 75 | X EXTINIT0 4 700 -250 100 L 50 50 1 1 I 76 | X USB_DM 5 700 -150 100 L 50 50 1 1 B 77 | X USB_DP 6 700 -50 100 L 50 50 1 1 B 78 | X VDDUSB 7 700 50 100 L 50 50 1 1 W 79 | X RESERVED 8 700 150 100 L 50 50 1 1 U 80 | X VCC_RF 9 700 250 100 L 50 50 1 1 w 81 | ENDDRAW 82 | ENDDEF 83 | # 84 | # NOKIA5110 85 | # 86 | DEF NOKIA5110 N 0 40 Y Y 1 F N 87 | F0 "N" 0 250 50 H V C CNN 88 | F1 "NOKIA5110" 0 100 50 H V C CNN 89 | F2 "" 200 50 50 H I C CNN 90 | F3 "" 200 50 50 H I C CNN 91 | DRAW 92 | S -450 200 450 -200 0 1 0 f 93 | X RST 1 -350 -300 100 U 50 50 1 1 I 94 | X CE 2 -250 -300 100 U 50 50 1 1 I 95 | X DC 3 -150 -300 100 U 50 50 1 1 I 96 | X MOSI 4 -50 -300 100 U 50 50 1 1 I 97 | X CLK 5 50 -300 100 U 50 50 1 1 I 98 | X VCC 6 150 -300 100 U 50 50 1 1 W 99 | X BL 7 250 -300 100 U 50 50 1 1 I 100 | X GND 8 350 -300 100 U 50 50 1 1 W 101 | ENDDRAW 102 | ENDDEF 103 | # 104 | #End Library 105 | -------------------------------------------------------------------------------- /src/commons/commons.c: -------------------------------------------------------------------------------- 1 | #include "commons/commons.h" 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | char *uint32ToStr(char *buff, uint32_t val, uint8_t buff_len){ 8 | char* pbe = &buff[buff_len-1]; 9 | for (*pbe = 0; val && (pbe >= buff);) { 10 | *(--pbe) = (val % 10) + '0'; 11 | val /= 10; 12 | } 13 | return pbe; 14 | } 15 | ////////////////////////////////////////////////////////////////////////// 16 | 17 | char *int32ToStr(char *buff, 18 | int32_t val, 19 | uint8_t buff_len){ 20 | int8_t neg = val < 0; 21 | char* pbe = &buff[buff_len-1]; 22 | val *= neg ? -1 : 1; 23 | 24 | for (*pbe = 0; val && (pbe > buff);) { 25 | *(--pbe) = (val % 10) + '0'; 26 | val /= 10; 27 | } 28 | 29 | if (neg && (pbe > buff)) 30 | *(--pbe) = '-'; 31 | return pbe; 32 | } 33 | ////////////////////////////////////////////////////////////////////////// 34 | 35 | void waitTicks(int ticks) { 36 | while (ticks--); 37 | } 38 | /////////////////////////////////////////////////////// 39 | 40 | #define MAX_PRECISION 10 41 | char *ftoa(float f, char *buf, int precision) { 42 | static const float rounders[MAX_PRECISION + 1] = { 43 | 0.5f, // 0 44 | 0.05f, // 1 45 | 0.005f, // 2 46 | 0.0005f, // 3 47 | 0.00005f, // 4 48 | 0.000005f, // 5 49 | 0.0000005f, // 6 50 | 0.00000005f, // 7 51 | 0.000000005f, // 8 52 | 0.0000000005f, // 9 53 | 0.00000000005f // 10 54 | }; 55 | 56 | char *ptr = buf; 57 | char *p = ptr; 58 | char *p1; 59 | char c; 60 | int32_t intPart; 61 | 62 | // check precision bounds 63 | if (precision > MAX_PRECISION) 64 | precision = MAX_PRECISION; 65 | 66 | // sign stuff 67 | if (f < 0) { 68 | f = -f; 69 | *ptr++ = '-'; 70 | } 71 | 72 | if (precision < 0) { // negative precision == automatic precision guess 73 | if (f < 1.0f) precision = 6; 74 | else if (f < 10.0f) precision = 5; 75 | else if (f < 100.0f) precision = 4; 76 | else if (f < 1000.0f) precision = 3; 77 | else if (f < 10000.0f) precision = 2; 78 | else if (f < 100000.0f) precision = 1; 79 | else precision = 0; 80 | } 81 | 82 | // round value according the precision 83 | if (precision) 84 | f += rounders[precision]; 85 | 86 | // integer part... 87 | intPart = (int32_t)f; 88 | f -= intPart; 89 | 90 | if (!intPart) 91 | *ptr++ = '0'; 92 | else { 93 | // save start pointer 94 | p = ptr; 95 | 96 | // convert (reverse order) 97 | while (intPart) { 98 | *p++ = '0' + intPart % 10; 99 | intPart /= 10; 100 | } 101 | // save end pos 102 | p1 = p; 103 | // reverse result 104 | while (p > ptr) { 105 | c = *--p; 106 | *p = *ptr; 107 | *ptr++ = c; 108 | } 109 | // restore end pos 110 | ptr = p1; 111 | } 112 | 113 | // decimal part 114 | if (precision) { 115 | // place decimal point 116 | *ptr++ = '.'; 117 | // convert 118 | while (precision--) { 119 | f *= 10.0f; 120 | c = (char)f; 121 | *ptr++ = '0' + c; 122 | f -= c; 123 | } 124 | } 125 | // terminating zero 126 | *ptr = 0; 127 | return buf; 128 | } 129 | /////////////////////////////////////////////////////// 130 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file Project/STM32F10x_StdPeriph_Template/stm32f10x_conf.h 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 08-April-2011 7 | * @brief Library configuration 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 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __STM32F10x_CONF_H 24 | #define __STM32F10x_CONF_H 25 | 26 | /* Includes ------------------------------------------------------------------*/ 27 | /* Uncomment/Comment the line below to enable/disable peripheral header file inclusion */ 28 | #include "stm32f10x_adc.h" 29 | #include "stm32f10x_bkp.h" 30 | #include "stm32f10x_can.h" 31 | #include "stm32f10x_cec.h" 32 | #include "stm32f10x_crc.h" 33 | #include "stm32f10x_dac.h" 34 | #include "stm32f10x_dbgmcu.h" 35 | #include "stm32f10x_dma.h" 36 | #include "stm32f10x_exti.h" 37 | #include "stm32f10x_flash.h" 38 | #include "stm32f10x_fsmc.h" 39 | #include "stm32f10x_gpio.h" 40 | #include "stm32f10x_i2c.h" 41 | #include "stm32f10x_iwdg.h" 42 | #include "stm32f10x_pwr.h" 43 | #include "stm32f10x_rcc.h" 44 | #include "stm32f10x_rtc.h" 45 | #include "stm32f10x_sdio.h" 46 | #include "stm32f10x_spi.h" 47 | #include "stm32f10x_tim.h" 48 | #include "stm32f10x_usart.h" 49 | #include "stm32f10x_wwdg.h" 50 | #include "misc.h" /* High level functions for NVIC and SysTick (add-on to CMSIS functions) */ 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 which reports 64 | * the name of the source file and the source line number of the call 65 | * that failed. If expr is true, it returns no value. 66 | * @retval None 67 | */ 68 | #define assert_param(expr) ((expr) ? (void)0 : assert_failed((uint8_t *)__FILE__, __LINE__)) 69 | /* Exported functions ------------------------------------------------------- */ 70 | void assert_failed(uint8_t* file, uint32_t line); 71 | #else 72 | #define assert_param(expr) ((void)0) 73 | #endif /* USE_FULL_ASSERT */ 74 | 75 | #endif /* __STM32F10x_CONF_H */ 76 | 77 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 78 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/navigatorschema-rescue.lib: -------------------------------------------------------------------------------- 1 | EESchema-LIBRARY Version 2.4 2 | #encoding utf-8 3 | # 4 | # 24AA32A-mad_symbols 5 | # 6 | DEF 24AA32A-mad_symbols U 0 40 Y Y 1 F N 7 | F0 "U" 0 350 50 H V C CNN 8 | F1 "24AA32A-mad_symbols" 0 50 50 H V C CNN 9 | F2 "" 0 -750 50 H I C CNN 10 | F3 "" 0 -750 50 H I C CNN 11 | DRAW 12 | S -350 250 350 -250 0 1 0 N 13 | X A0 1 -450 150 100 R 50 50 1 1 I 14 | X A1 2 -450 50 100 R 50 50 1 1 I 15 | X A2 3 -450 -50 100 R 50 50 1 1 I 16 | X GND 4 -450 -150 100 R 50 50 1 1 I 17 | X SDA 5 450 -150 100 L 50 50 1 1 B 18 | X SCL 6 450 -50 100 L 50 50 1 1 I 19 | X WP 7 450 50 100 L 50 50 1 1 I 20 | X VCC 8 450 150 100 L 50 50 1 1 I 21 | ENDDRAW 22 | ENDDEF 23 | # 24 | # LIS3MDL-mad_symbols 25 | # 26 | DEF LIS3MDL-mad_symbols L 0 40 Y Y 1 F N 27 | F0 "L" 350 400 50 H V C CNN 28 | F1 "LIS3MDL-mad_symbols" -350 400 50 H V C CNN 29 | F2 "" -750 550 50 H I C CNN 30 | F3 "" -750 550 50 H I C CNN 31 | DRAW 32 | S -400 350 400 -350 0 1 0 f 33 | X SCL/SPC 1 500 100 100 L 50 50 1 1 O 34 | X CS 10 -500 100 100 R 50 50 1 1 I 35 | X SDA 11 -100 450 100 D 50 50 1 1 I 36 | X RES 12 100 450 100 D 50 50 1 1 I 37 | X RES 2 500 0 100 L 50 50 1 1 O 38 | X GND 3 500 -100 100 L 50 50 1 1 W 39 | X C1 4 500 -200 100 L 50 50 1 1 I 40 | X VDD 5 100 -450 100 U 50 50 1 1 I 41 | X VDD_IO 6 -100 -450 100 U 50 50 1 1 I 42 | X INT 7 -500 -200 100 R 50 50 1 1 I 43 | X DRDY 8 -500 -100 100 R 50 50 1 1 I 44 | X SD0/SA1 9 -500 0 100 R 50 50 1 1 I 45 | ENDDRAW 46 | ENDDEF 47 | # 48 | # NEO-6M-mad_symbols 49 | # 50 | DEF NEO-6M-mad_symbols N 0 40 Y Y 1 F N 51 | F0 "N" 0 750 50 H V C CNN 52 | F1 "NEO-6M-mad_symbols" 0 450 50 H V C CNN 53 | F2 "" -1590 900 50 H I C CNN 54 | F3 "" -1590 900 50 H I C CNN 55 | DRAW 56 | S -600 650 600 -650 0 1 0 f 57 | X RESERVED 1 700 -550 100 L 50 50 1 1 N 58 | X GND 10 700 350 100 L 50 50 1 1 I 59 | X RF_IN 11 700 450 100 L 50 50 1 1 I 60 | X GND 12 700 550 100 L 50 50 1 1 I 61 | X GND 13 -700 -550 100 R 50 50 1 1 I 62 | X MOSI/CFG_COM0 14 -700 -450 100 R 50 50 1 1 B 63 | X MOSI/CFG_COM1 15 -700 -350 100 R 50 50 1 1 I 64 | X CFG_GPS0/SCK 16 -700 -250 100 R 50 50 1 1 I 65 | X RESERVED 17 -700 -150 100 R 50 50 1 1 N 66 | X SDA2 18 -700 -50 100 R 50 50 1 1 B 67 | X SCL2 19 -700 50 100 R 50 50 1 1 B 68 | X SS_N 2 700 -450 100 L 50 50 1 1 I 69 | X TxD1 20 -700 150 100 R 50 50 1 1 O 70 | X RxD1 21 -700 250 100 R 50 50 1 1 I 71 | X V_BCKP 22 -700 350 100 R 50 50 1 1 W 72 | X VCC 23 -700 450 100 R 50 50 1 1 W 73 | X GND 24 -700 550 100 R 50 50 1 1 I 74 | X TIMEPULSE 3 700 -350 100 L 50 50 1 1 O 75 | X EXTINIT0 4 700 -250 100 L 50 50 1 1 I 76 | X USB_DM 5 700 -150 100 L 50 50 1 1 B 77 | X USB_DP 6 700 -50 100 L 50 50 1 1 B 78 | X VDDUSB 7 700 50 100 L 50 50 1 1 W 79 | X RESERVED 8 700 150 100 L 50 50 1 1 U 80 | X VCC_RF 9 700 250 100 L 50 50 1 1 w 81 | ENDDRAW 82 | ENDDEF 83 | # 84 | # NOKIA5110-mad_symbols 85 | # 86 | DEF NOKIA5110-mad_symbols N 0 40 Y Y 1 F N 87 | F0 "N" 0 250 50 H V C CNN 88 | F1 "NOKIA5110-mad_symbols" 0 100 50 H V C CNN 89 | F2 "" 200 50 50 H I C CNN 90 | F3 "" 200 50 50 H I C CNN 91 | DRAW 92 | S -450 200 450 -200 0 1 0 f 93 | X RST 1 -350 -300 100 U 50 50 1 1 I 94 | X CE 2 -250 -300 100 U 50 50 1 1 I 95 | X DC 3 -150 -300 100 U 50 50 1 1 I 96 | X MOSI 4 -50 -300 100 U 50 50 1 1 I 97 | X CLK 5 50 -300 100 U 50 50 1 1 I 98 | X VCC 6 150 -300 100 U 50 50 1 1 W 99 | X BL 7 250 -300 100 U 50 50 1 1 I 100 | X GND 8 350 -300 100 U 50 50 1 1 W 101 | ENDDRAW 102 | ENDDEF 103 | # 104 | #End Library 105 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_wwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_wwdg.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 WWDG 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_WWDG_H 25 | #define __STM32F10x_WWDG_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 WWDG 39 | * @{ 40 | */ 41 | 42 | /** @defgroup WWDG_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup WWDG_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup WWDG_Prescaler 55 | * @{ 56 | */ 57 | 58 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000) 59 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080) 60 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100) 61 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180) 62 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ 63 | ((PRESCALER) == WWDG_Prescaler_2) || \ 64 | ((PRESCALER) == WWDG_Prescaler_4) || \ 65 | ((PRESCALER) == WWDG_Prescaler_8)) 66 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) 67 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) 68 | 69 | /** 70 | * @} 71 | */ 72 | 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup WWDG_Exported_Macros 78 | * @{ 79 | */ 80 | /** 81 | * @} 82 | */ 83 | 84 | /** @defgroup WWDG_Exported_Functions 85 | * @{ 86 | */ 87 | 88 | void WWDG_DeInit(void); 89 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); 90 | void WWDG_SetWindowValue(uint8_t WindowValue); 91 | void WWDG_EnableIT(void); 92 | void WWDG_SetCounter(uint8_t Counter); 93 | void WWDG_Enable(uint8_t Counter); 94 | FlagStatus WWDG_GetFlagStatus(void); 95 | void WWDG_ClearFlag(void); 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /* __STM32F10x_WWDG_H */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** 108 | * @} 109 | */ 110 | 111 | /** 112 | * @} 113 | */ 114 | 115 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 116 | -------------------------------------------------------------------------------- /inc/display/nokiaLcd/nokia5110.h: -------------------------------------------------------------------------------- 1 | /******************************************************************************* 2 | * File : stm32f10x_pcd8544.h 3 | * Description : STM32F10x library for NOKIA 5110 LCD driver, PCD8544 4 | * Author : Aytac Dilek 5 | * Note : GNU General Public License, version 3 (GPL-3.0) 6 | *******************************************************************************/ 7 | #ifndef __STM32F10X_PCD8544_H 8 | #define __STM32F10X_PCD8544_H 9 | 10 | /* Includes ********************************************************************/ 11 | #include "stm32f10x_gpio.h" 12 | #include "stm32f10x_spi.h" 13 | #include 14 | 15 | /* Defines *********************************************************************/ 16 | #define PCD8544_SPI_PER SPI1 17 | #define PCD8544_SPI_PORT GPIOA 18 | #define PCD8544_SPI_CLOCK RCC_APB2Periph_SPI1 19 | #define PCD8544_GPIO_PORT GPIOA 20 | #define PCD8544_GPIO_CLOCK RCC_APB2Periph_GPIOA 21 | 22 | #define PCD8544_LED_PORT GPIOA 23 | #define PCD8544_LED_PIN GPIO_Pin_2 24 | #define PCD8544_RST_PORT GPIOA 25 | #define PCD8544_RST_PIN GPIO_Pin_1 26 | #define PCD8544_CE_PORT GPIOA 27 | #define PCD8544_CE_PIN GPIO_Pin_4 28 | #define PCD8544_DC_PORT GPIOA 29 | #define PCD8544_DC_PIN GPIO_Pin_3 30 | #define PCD8544_CLK_PORT GPIOA 31 | #define PCD8544_CLK_PIN GPIO_Pin_5 32 | #define PCD8544_MISO_PORT GPIOA 33 | #define PCD8544_MISO_PIN GPIO_Pin_6 34 | #define PCD8544_MOSI_PORT GPIOA 35 | #define PCD8544_MOSI_PIN GPIO_Pin_7 36 | 37 | #define PCD8544_WIDTH 84 38 | #define PCD8544_HEIGHT 48 39 | 40 | /* Macros **********************************************************************/ 41 | #define NOKIA5110_LED_ON() GPIO_SetBits(PCD8544_LED_PORT, PCD8544_LED_PIN) 42 | #define NOKIA5110_LED_OFF() GPIO_ResetBits(PCD8544_LED_PORT, PCD8544_LED_PIN) 43 | #define NOKIA5110_RST_LOW() GPIO_ResetBits(PCD8544_RST_PORT, PCD8544_RST_PIN) 44 | #define NOKIA5110_RST_HIGH() GPIO_SetBits(PCD8544_RST_PORT, PCD8544_RST_PIN) 45 | #define NOKIA5110_DC_LOW() GPIO_ResetBits(PCD8544_DC_PORT, PCD8544_DC_PIN) 46 | #define NOKIA5110_DC_HIGH() GPIO_SetBits(PCD8544_DC_PORT, PCD8544_DC_PIN) 47 | #define NOKIA5110_DIN_LOW() GPIO_ResetBits(PCD8544_MOSI_PORT, PCD8544_MOSI_PIN) 48 | #define NOKIA5110_DIN_HIGH() GPIO_SetBits(PCD8544_MOSI_PORT, PCD8544_MOSI_PIN) 49 | #define NOKIA5110_CLK_LOW() GPIO_ResetBits(PCD8544_CLK_PORT, PCD8544_CLK_PIN) 50 | #define NOKIA5110_CLK_HIGH() GPIO_SetBits(PCD8544_CLK_PORT, PCD8544_CLK_PIN) 51 | #define NOKIA5110_CE_LOW() GPIO_ResetBits(PCD8544_CE_PORT, PCD8544_CE_PIN) 52 | #define NOKIA5110_CE_HIGH() GPIO_SetBits(PCD8544_CE_PORT, PCD8544_CE_PIN) 53 | 54 | /* Enumerations ****************************************************************/ 55 | typedef enum{ 56 | PCD8544_MODE_Command = 0, 57 | PCD8544_MODE_Data = 1, 58 | }PCD8544_MODE_TypeDef; 59 | 60 | #define DISPLAY_BANKS 6 61 | #define DISPLAY_WIDTH 84 62 | 63 | /////////////////////////////////////////////////////// 64 | 65 | /* Global Functions ************************************************************/ 66 | 67 | void nokia5110_init(void); 68 | void nokia5110_clear(void); 69 | void nokia5110_gotoXY(int8_t col, int8_t row); 70 | void nokia5110_writeChar(char c); 71 | void nokia5110_writeString(const char *s); 72 | void nokia5110_setContrast(uint8_t contrast); 73 | void nokia5110_light(bool on); 74 | void nokia5110_printText(int16_t x, int16_t y, const char *value); 75 | void nokia5110_setPixel(int16_t x, int16_t y); 76 | 77 | void nokia5110_spi_writeByte(PCD8544_MODE_TypeDef mode, uint8_t data); 78 | 79 | #endif // __STM32F10X_PCD8544_H 80 | -------------------------------------------------------------------------------- /inc/magnetometer/hmc5883/hmc5883.h: -------------------------------------------------------------------------------- 1 | #ifndef HMC5883_H 2 | #define HMC5883_H 3 | 4 | /* Includes */ 5 | #include 6 | #include 7 | #include "i2c1/i2c1.h" 8 | #include "stm32f10x.h" 9 | 10 | //#define HMC5883L_ADDRESS 0x1E // this device only has one address 11 | #define HMC5883L_ADDRESS 0x0d 12 | #define HMC5883L_DEFAULT_ADDRESS (HMC5883L_ADDRESS << 1) 13 | 14 | #define HMC5883L_RA_CONFIG_A 0x00 15 | #define HMC5883L_RA_CONFIG_B 0x01 16 | #define HMC5883L_RA_MODE 0x02 17 | #define HMC5883L_RA_DATAX_H 0x03 18 | #define HMC5883L_RA_DATAX_L 0x04 19 | #define HMC5883L_RA_DATAY_H 0x05 20 | #define HMC5883L_RA_DATAY_L 0x06 21 | #define HMC5883L_RA_DATAZ_H 0x07 22 | #define HMC5883L_RA_DATAZ_L 0x08 23 | #define HMC5883L_RA_STATUS 0x09 24 | #define HMC5883L_RA_ID_A 0x0A 25 | #define HMC5883L_RA_ID_B 0x0B 26 | #define HMC5883L_RA_ID_C 0x0C 27 | 28 | #define HMC5883L_CRA_AVERAGE_BIT 6 29 | #define HMC5883L_CRA_AVERAGE_LENGTH 2 30 | #define HMC5883L_CRA_RATE_BIT 4 31 | #define HMC5883L_CRA_RATE_LENGTH 3 32 | #define HMC5883L_CRA_BIAS_BIT 1 33 | #define HMC5883L_CRA_BIAS_LENGTH 2 34 | 35 | #define HMC5883L_AVERAGING_1 0x00 36 | #define HMC5883L_AVERAGING_2 0x01 37 | #define HMC5883L_AVERAGING_4 0x02 38 | #define HMC5883L_AVERAGING_8 0x03 39 | 40 | #define HMC5883L_RATE_0P75 0x00 41 | #define HMC5883L_RATE_1P5 0x01 42 | #define HMC5883L_RATE_3 0x02 43 | #define HMC5883L_RATE_7P5 0x03 44 | #define HMC5883L_RATE_15 0x04 45 | #define HMC5883L_RATE_30 0x05 46 | #define HMC5883L_RATE_75 0x06 47 | 48 | #define HMC5883L_BIAS_NORMAL 0x00 49 | #define HMC5883L_BIAS_POSITIVE 0x01 50 | #define HMC5883L_BIAS_NEGATIVE 0x02 51 | 52 | #define HMC5883L_CRB_GAIN_BIT 7 53 | #define HMC5883L_CRB_GAIN_LENGTH 3 54 | 55 | #define HMC5883L_GAIN_1370 0x00 56 | #define HMC5883L_GAIN_1090 0x01 57 | #define HMC5883L_GAIN_820 0x02 58 | #define HMC5883L_GAIN_660 0x03 59 | #define HMC5883L_GAIN_440 0x04 60 | #define HMC5883L_GAIN_390 0x05 61 | #define HMC5883L_GAIN_330 0x06 62 | #define HMC5883L_GAIN_220 0x07 63 | 64 | #define HMC5883L_MODEREG_BIT 1 65 | #define HMC5883L_MODEREG_LENGTH 2 66 | 67 | #define HMC5883L_MODE_CONTINUOUS 0x00 68 | #define HMC5883L_MODE_SINGLE 0x01 69 | #define HMC5883L_MODE_IDLE 0x02 70 | 71 | #define HMC5883L_STATUS_LOCK_BIT 1 72 | #define HMC5883L_STATUS_READY_BIT 0 73 | 74 | void hmc5883l_Init(void); 75 | bool hmc5883l_TestConnection(void); 76 | 77 | // CONFIG_A register 78 | uint8_t hmc5883l_GetSampleAveraging(void); 79 | void hmc5883l_SetSampleAveraging(uint8_t averaging); 80 | uint8_t hmc5883l_GetDataRate(void); 81 | void hmc5883l_SetDataRate(uint8_t rate); 82 | uint8_t hmc5883l_GetMeasurementBias(void); 83 | void hmc5883l_SetMeasurementBias(uint8_t bias); 84 | 85 | // CONFIG_B register 86 | uint8_t hmc5883l_GetGain(void); 87 | float hmc5883l_GetGainF(void); 88 | void hmc5883l_SetGain(uint8_t gain); 89 | 90 | // MODE register 91 | uint8_t hmc5883l_GetMode(void); 92 | void hmc5883l_SetMode(uint8_t mode); 93 | 94 | // DATA* registers 95 | void hmc5883l_HeadingSync(uint8_t *buff, uint8_t len); 96 | void hmc5883l_HeadingAsync(uint8_t *buff, uint8_t len, volatile int8_t *finishCode, i2c1_pf_callback cb); 97 | 98 | // STATUS register 99 | bool hmc5883l_GetLockStatus(void); 100 | bool hmc5883l_GetReadyStatus(void); 101 | 102 | #endif // HMC5883_H 103 | -------------------------------------------------------------------------------- /src/commons/bearing.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "commons/bearing.h" 3 | #include "commons/commons.h" 4 | 5 | 6 | //android Location.bearingTo() 7 | #ifdef BEARING_HIGH_PRECISION 8 | static float initialBearingHP(float lat1, 9 | float lon1, 10 | float lat2, 11 | float lon2) { 12 | #define MAXITERS 20 13 | #define majAxis 6378137.0f 14 | #define smajAxis 6356752.3142f // WGS84 semi-major axis 15 | #define fraction (majAxis - smajAxis) / majAxis 16 | #define EXP 1.0e-6f 17 | 18 | float L, U1, U2; 19 | float cosU1, cosU2, sinU1, sinU2; 20 | float cosU1cosU2, sinU1sinU2; 21 | float sigma, cosSqAlpha, cos2SM, cosSigma, sinSigma, cosLambda, sinLambda; 22 | float lambda, bearing; 23 | 24 | // Convert lat/long to radians 25 | lat1 *= deg2rad_coeff; 26 | lat2 *= deg2rad_coeff; 27 | lon1 *= deg2rad_coeff; 28 | lon2 *= deg2rad_coeff; 29 | 30 | L = lon2 - lon1; 31 | U1 = atanf((1.0f - fraction) * tanf(lat1)); 32 | U2 = atanf((1.0f - fraction) * tanf(lat2)); 33 | 34 | cosU1 = cosf(U1); 35 | cosU2 = cosf(U2); 36 | sinU1 = sinf(U1); 37 | sinU2 = sinf(U2); 38 | cosU1cosU2 = cosU1 * cosU2; 39 | sinU1sinU2 = sinU1 * sinU2; 40 | 41 | sigma = 0.0; 42 | cosSqAlpha = 0.0; 43 | cos2SM = 0.0; 44 | cosSigma = 0.0; 45 | sinSigma = 0.0; 46 | cosLambda = 0.0; 47 | sinLambda = 0.0; 48 | lambda = L; // initial guess 49 | 50 | for (int iter = 0; iter < MAXITERS; iter++) { 51 | float lambdaOrig, t1, t2, sinSqSigma, sinAlpha, C, delta; 52 | lambdaOrig = lambda; 53 | cosLambda = cosf(lambda); 54 | sinLambda = sinf(lambda); 55 | t1 = cosU2 * sinLambda; 56 | t2 = cosU1 * sinU2 - sinU1 * cosU2 * cosLambda; 57 | sinSqSigma = t1 * t1 + t2 * t2; 58 | sinSigma = sqrtf(sinSqSigma); 59 | cosSigma = sinU1sinU2 + cosU1cosU2 * cosLambda; 60 | sigma = atan2f(sinSigma, cosSigma); 61 | sinAlpha = (sinSigma == 0.0f) ? 0.0f : cosU1cosU2 * sinLambda / sinSigma; 62 | cosSqAlpha = 1.0f - sinAlpha * sinAlpha; 63 | cos2SM = (cosSqAlpha == 0.0f) ? 0.0f : cosSigma - 2.0f * sinU1sinU2 / cosSqAlpha; 64 | C = (fraction / 16.0f) * cosSqAlpha * (4.0f + fraction * (4.0f - 3.0f * cosSqAlpha)); 65 | lambda = L + (1.0f - C) * fraction * sinAlpha * (sigma + C * sinSigma * (cos2SM + C * cosSigma * (-1.0f + 2.0f * cos2SM * cos2SM))); // (11) 66 | delta = (lambda - lambdaOrig) / lambda; 67 | if (fabsf(delta) < EXP) 68 | break; 69 | } 70 | bearing = atan2f(cosU2 * sinLambda, 71 | cosU1 * sinU2 - sinU1 * cosU2 * cosLambda); 72 | return bearing * rad2deg_coeff; 73 | } 74 | /////////////////////////////////////////////////////// 75 | #endif 76 | 77 | #ifndef BEARING_HIGH_PRECISION 78 | //https://software.intel.com/en-us/blogs/2012/11/30/calculating-a-bearing-between-points-in-location-aware-apps 79 | static float initialBearingLP(float lat1, 80 | float lon1, 81 | float lat2, 82 | float lon2) { 83 | float phi1 = lat1 * deg2rad_coeff; 84 | float phi2 = lat2 * deg2rad_coeff; 85 | float lam1 = lon1 * deg2rad_coeff; 86 | float lam2 = lon2 * deg2rad_coeff; 87 | float deltaL = lam2 - lam1; 88 | float cosPhi2 = cosf(phi2); 89 | float bearing = atan2f(sinf(deltaL) * cosPhi2, 90 | cosf(phi1) * sinf(phi2) - sinf(phi1) * cosPhi2 * cosf(deltaL)); 91 | return bearing * rad2deg_coeff; 92 | } 93 | /////////////////////////////////////////////////////// 94 | #endif 95 | 96 | float initialBearing(float lat1, 97 | float lon1, 98 | float lat2, 99 | float lon2) { 100 | float ib; 101 | #ifdef BEARING_HIGH_PRECISION 102 | ib = initialBearingHP(lat1, lon1, lat2, lon2); 103 | #else 104 | ib = initialBearingLP(lat1, lon1, lat2, lon2); 105 | #endif 106 | return fmodf(ib+360.f, 360.0f); //+360 % 360 107 | } 108 | /////////////////////////////////////////////////////// 109 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_dbgmcu.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_dbgmcu.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 DBGMCU 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_DBGMCU_H 25 | #define __STM32F10x_DBGMCU_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 DBGMCU 39 | * @{ 40 | */ 41 | 42 | /** @defgroup DBGMCU_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup DBGMCU_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | #define DBGMCU_SLEEP ((uint32_t)0x00000001) 55 | #define DBGMCU_STOP ((uint32_t)0x00000002) 56 | #define DBGMCU_STANDBY ((uint32_t)0x00000004) 57 | #define DBGMCU_IWDG_STOP ((uint32_t)0x00000100) 58 | #define DBGMCU_WWDG_STOP ((uint32_t)0x00000200) 59 | #define DBGMCU_TIM1_STOP ((uint32_t)0x00000400) 60 | #define DBGMCU_TIM2_STOP ((uint32_t)0x00000800) 61 | #define DBGMCU_TIM3_STOP ((uint32_t)0x00001000) 62 | #define DBGMCU_TIM4_STOP ((uint32_t)0x00002000) 63 | #define DBGMCU_CAN1_STOP ((uint32_t)0x00004000) 64 | #define DBGMCU_I2C1_SMBUS_TIMEOUT ((uint32_t)0x00008000) 65 | #define DBGMCU_I2C2_SMBUS_TIMEOUT ((uint32_t)0x00010000) 66 | #define DBGMCU_TIM8_STOP ((uint32_t)0x00020000) 67 | #define DBGMCU_TIM5_STOP ((uint32_t)0x00040000) 68 | #define DBGMCU_TIM6_STOP ((uint32_t)0x00080000) 69 | #define DBGMCU_TIM7_STOP ((uint32_t)0x00100000) 70 | #define DBGMCU_CAN2_STOP ((uint32_t)0x00200000) 71 | #define DBGMCU_TIM15_STOP ((uint32_t)0x00400000) 72 | #define DBGMCU_TIM16_STOP ((uint32_t)0x00800000) 73 | #define DBGMCU_TIM17_STOP ((uint32_t)0x01000000) 74 | #define DBGMCU_TIM12_STOP ((uint32_t)0x02000000) 75 | #define DBGMCU_TIM13_STOP ((uint32_t)0x04000000) 76 | #define DBGMCU_TIM14_STOP ((uint32_t)0x08000000) 77 | #define DBGMCU_TIM9_STOP ((uint32_t)0x10000000) 78 | #define DBGMCU_TIM10_STOP ((uint32_t)0x20000000) 79 | #define DBGMCU_TIM11_STOP ((uint32_t)0x40000000) 80 | 81 | #define IS_DBGMCU_PERIPH(PERIPH) ((((PERIPH) & 0x800000F8) == 0x00) && ((PERIPH) != 0x00)) 82 | /** 83 | * @} 84 | */ 85 | 86 | /** @defgroup DBGMCU_Exported_Macros 87 | * @{ 88 | */ 89 | 90 | /** 91 | * @} 92 | */ 93 | 94 | /** @defgroup DBGMCU_Exported_Functions 95 | * @{ 96 | */ 97 | 98 | uint32_t DBGMCU_GetREVID(void); 99 | uint32_t DBGMCU_GetDEVID(void); 100 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState); 101 | 102 | #ifdef __cplusplus 103 | } 104 | #endif 105 | 106 | #endif /* __STM32F10x_DBGMCU_H */ 107 | /** 108 | * @} 109 | */ 110 | 111 | /** 112 | * @} 113 | */ 114 | 115 | /** 116 | * @} 117 | */ 118 | 119 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 120 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_rtc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_rtc.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 RTC 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_RTC_H 25 | #define __STM32F10x_RTC_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 RTC 39 | * @{ 40 | */ 41 | 42 | /** @defgroup RTC_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup RTC_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup RTC_interrupts_define 55 | * @{ 56 | */ 57 | 58 | #define RTC_IT_OW ((uint16_t)0x0004) /*!< Overflow interrupt */ 59 | #define RTC_IT_ALR ((uint16_t)0x0002) /*!< Alarm interrupt */ 60 | #define RTC_IT_SEC ((uint16_t)0x0001) /*!< Second interrupt */ 61 | #define IS_RTC_IT(IT) ((((IT) & (uint16_t)0xFFF8) == 0x00) && ((IT) != 0x00)) 62 | #define IS_RTC_GET_IT(IT) (((IT) == RTC_IT_OW) || ((IT) == RTC_IT_ALR) || \ 63 | ((IT) == RTC_IT_SEC)) 64 | /** 65 | * @} 66 | */ 67 | 68 | /** @defgroup RTC_interrupts_flags 69 | * @{ 70 | */ 71 | 72 | #define RTC_FLAG_RTOFF ((uint16_t)0x0020) /*!< RTC Operation OFF flag */ 73 | #define RTC_FLAG_RSF ((uint16_t)0x0008) /*!< Registers Synchronized flag */ 74 | #define RTC_FLAG_OW ((uint16_t)0x0004) /*!< Overflow flag */ 75 | #define RTC_FLAG_ALR ((uint16_t)0x0002) /*!< Alarm flag */ 76 | #define RTC_FLAG_SEC ((uint16_t)0x0001) /*!< Second flag */ 77 | #define IS_RTC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint16_t)0xFFF0) == 0x00) && ((FLAG) != 0x00)) 78 | #define IS_RTC_GET_FLAG(FLAG) (((FLAG) == RTC_FLAG_RTOFF) || ((FLAG) == RTC_FLAG_RSF) || \ 79 | ((FLAG) == RTC_FLAG_OW) || ((FLAG) == RTC_FLAG_ALR) || \ 80 | ((FLAG) == RTC_FLAG_SEC)) 81 | #define IS_RTC_PRESCALER(PRESCALER) ((PRESCALER) <= 0xFFFFF) 82 | 83 | /** 84 | * @} 85 | */ 86 | 87 | /** 88 | * @} 89 | */ 90 | 91 | /** @defgroup RTC_Exported_Macros 92 | * @{ 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | 99 | /** @defgroup RTC_Exported_Functions 100 | * @{ 101 | */ 102 | 103 | void RTC_ITConfig(uint16_t RTC_IT, FunctionalState NewState); 104 | void RTC_EnterConfigMode(void); 105 | void RTC_ExitConfigMode(void); 106 | uint32_t RTC_GetCounter(void); 107 | void RTC_SetCounter(uint32_t CounterValue); 108 | void RTC_SetPrescaler(uint32_t PrescalerValue); 109 | void RTC_SetAlarm(uint32_t AlarmValue); 110 | uint32_t RTC_GetDivider(void); 111 | void RTC_WaitForLastTask(void); 112 | void RTC_WaitForSynchro(void); 113 | FlagStatus RTC_GetFlagStatus(uint16_t RTC_FLAG); 114 | void RTC_ClearFlag(uint16_t RTC_FLAG); 115 | ITStatus RTC_GetITStatus(uint16_t RTC_IT); 116 | void RTC_ClearITPendingBit(uint16_t RTC_IT); 117 | 118 | #ifdef __cplusplus 119 | } 120 | #endif 121 | 122 | #endif /* __STM32F10x_RTC_H */ 123 | /** 124 | * @} 125 | */ 126 | 127 | /** 128 | * @} 129 | */ 130 | 131 | /** 132 | * @} 133 | */ 134 | 135 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 136 | -------------------------------------------------------------------------------- /src/commons/quaternion.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include "commons/quaternion.h" 3 | 4 | static quaternion_t quaternion_new(void); 5 | static void quaternion_scale(quaternion_t *q, float f); 6 | static float quaternion_len(const quaternion_t *q); 7 | static void quaternion_normalize(quaternion_t *q); 8 | 9 | static quaternion_t quaternion_inv(const quaternion_t *q); 10 | static quaternion_t quaternion_mul_qq(const quaternion_t *a, const quaternion_t *b); 11 | static quaternion_t quaternion_mul_qv(const quaternion_t *a, const vector3d_t *b); 12 | 13 | quaternion_t quaternion_new() { 14 | quaternion_t r = {.w = 0.0f, .x = 0.0f, .y = 0.0f, .z = 0.0f}; 15 | return r; 16 | } 17 | /////////////////////////////////////////////////////// 18 | 19 | quaternion_t quaternion_new_wxyz(float w, 20 | float x, 21 | float y, 22 | float z) { 23 | quaternion_t r = {.w = w, .x = x, .y = y, .z = z}; 24 | return r; 25 | } 26 | /////////////////////////////////////////////////////// 27 | 28 | quaternion_t quaternion_new_vec(const vector3d_t *rv, 29 | float angleRads) { 30 | quaternion_t r; 31 | vector3d_t rotate_vector = vector_new(rv->x, rv->y, rv->z); 32 | vector_normalize(&rotate_vector); 33 | r.w = cosf(angleRads / 2.0f); 34 | r.x = rotate_vector.x * sinf(angleRads / 2.0f); 35 | r.y = rotate_vector.y * sinf(angleRads / 2.0f); 36 | r.z = rotate_vector.z * sinf(angleRads / 2.0f); 37 | return r; 38 | } 39 | /////////////////////////////////////////////////////// 40 | 41 | void quaternion_scale(quaternion_t *q, float f) { 42 | q->w *= f; q->x *= f; 43 | q->y *= f; q->z *= f; 44 | } 45 | /////////////////////////////////////////////////////// 46 | 47 | float quaternion_len(const quaternion_t *q) { 48 | return sqrtf(q->w*q->w + q->x*q->x + q->y*q->y + q->z*q->z); 49 | } 50 | /////////////////////////////////////////////////////// 51 | 52 | void quaternion_normalize(quaternion_t *q) { 53 | float len = quaternion_len(q); 54 | quaternion_scale(q, len); 55 | } 56 | /////////////////////////////////////////////////////// 57 | 58 | quaternion_t quaternion_inv(const quaternion_t *q) { 59 | quaternion_t r = quaternion_new_wxyz(q->w, -q->x, -q->y, -q->z); 60 | quaternion_normalize(&r); 61 | return r; 62 | } 63 | /////////////////////////////////////////////////////// 64 | 65 | quaternion_t quaternion_mul_qq(const quaternion_t *a, 66 | const quaternion_t *b) { 67 | quaternion_t r = quaternion_new(); 68 | r.w = a->w * b->w - a->x * b->x - a->y * b->y - a->z * b->z; 69 | r.x = a->w * b->x + a->x * b->w + a->y * b->z - a->z * b->y; 70 | r.y = a->w * b->y - a->x * b->z + a->y * b->w + a->z * b->x; 71 | r.z = a->w * b->z + a->x * b->y - a->y * b->x + a->z * b->w; 72 | return r; 73 | } 74 | /////////////////////////////////////////////////////// 75 | 76 | quaternion_t quaternion_mul_qv(const quaternion_t *a, 77 | const vector3d_t *b) { 78 | quaternion_t r = quaternion_new(); 79 | r.w = -a->x * b->x - a->y * b->y - a->z * b->z; 80 | r.x = a->w * b->x + a->y * b->z - a->z * b->y; 81 | r.y = a->w * b->y - a->x * b->z + a->z * b->x; 82 | r.z = a->w * b->z + a->x * b->y - a->y * b->x; 83 | return r; 84 | } 85 | /////////////////////////////////////////////////////// 86 | 87 | vector3d_t quaternion_transform_vec(const quaternion_t *q, 88 | const vector3d_t *v) { 89 | /*Quaternion t = quatMulVector(q, v); 90 | Quaternion qi = invert(q); 91 | Quaternion qmq = quatMulQuat(t, qi); 92 | return new Vector3D(qmq.x, qmq.y, qmq.z);*/ 93 | quaternion_t t = quaternion_mul_qv(q, v); 94 | quaternion_t qi = quaternion_inv(q); 95 | quaternion_t qmq = quaternion_mul_qq(&t, &qi); 96 | return vector_new(qmq.x, qmq.y, qmq.z); 97 | } 98 | /////////////////////////////////////////////////////// 99 | 100 | float quaternion_pitch(const quaternion_t *q) { 101 | return asinf(-2.0f * (q->x*q->z - q->w*q->y)); 102 | } 103 | /////////////////////////////////////////////////////// 104 | 105 | float quaternion_roll(const quaternion_t *q) { 106 | return atan2f(q->w*q->x + q->y*q->z, 0.5f - q->x*q->x - q->y*q->y); 107 | } 108 | /////////////////////////////////////////////////////// 109 | 110 | float quaternion_yaw(const quaternion_t *q) { 111 | return atan2f(q->x*q->y + q->w*q->z, 0.5f - q->y*q->y - q->z*q->z); 112 | } 113 | /////////////////////////////////////////////////////// 114 | -------------------------------------------------------------------------------- /navigator.pro: -------------------------------------------------------------------------------- 1 | TEMPLATE = app 2 | CONFIG += console 3 | CONFIG -= app_bundle 4 | CONFIG -= qt 5 | 6 | DEFINES += STM32 \ 7 | STM32F1 \ 8 | STM32F100RBTx \ 9 | DEBUG \ 10 | STM32F10X_MD_VL \ 11 | USE_STDPERIPH_DRIVER \ 12 | BEARING_HIGH_PRECISION 13 | 14 | INCLUDEPATH += inc \ 15 | CMSIS/core \ 16 | CMSIS/device \ 17 | StdPeriph_Driver/inc \ 18 | inc/gps \ 19 | inc/mpu6050 \ 20 | inc/nokiaLcd 21 | 22 | SOURCES += \ 23 | src/commons/bearing.c \ 24 | src/commons/commons.c \ 25 | src/commons/madgwick.c \ 26 | src/commons/magnetic.c \ 27 | src/commons/quaternion.c \ 28 | src/commons/vector3d.c \ 29 | src/display/display.c \ 30 | src/display/nokiaLcd/nokia5110.c \ 31 | src/display/nokiaLcd/nokia5110_buffered.c \ 32 | src/display/nokiaLcd/nokia5110_font.c \ 33 | src/gps/nmea.c \ 34 | src/magnetometer/hmc5883/hmc5883.c \ 35 | src/i2c1/i2c1.c \ 36 | src/magnetometer/lis3mdl/lis3mdl_i2c.c \ 37 | src/magnetometer/qmc5883l/qmc5883l.c \ 38 | src/main.c \ 39 | src/stm32vldiscovery_utils.c \ 40 | src/syscalls.c \ 41 | src/system_stm32f10x.c \ 42 | src/gps/neo6m.c \ 43 | src/accelerometer/mpu6050/MPU6050.c \ 44 | src/accelerometer/mpu6050/mpu6050_i2c.c \ 45 | src/nokiaLcd/stm32f10x_pcd8544.c \ 46 | src/power/power.c \ 47 | CMSIS/core/core_cm3.c \ 48 | StdPeriph_Driver/src/misc.c \ 49 | StdPeriph_Driver/src/stm32f10x_adc.c \ 50 | StdPeriph_Driver/src/stm32f10x_bkp.c \ 51 | StdPeriph_Driver/src/stm32f10x_can.c \ 52 | StdPeriph_Driver/src/stm32f10x_cec.c \ 53 | StdPeriph_Driver/src/stm32f10x_crc.c \ 54 | StdPeriph_Driver/src/stm32f10x_dac.c \ 55 | StdPeriph_Driver/src/stm32f10x_dbgmcu.c \ 56 | StdPeriph_Driver/src/stm32f10x_dma.c \ 57 | StdPeriph_Driver/src/stm32f10x_exti.c \ 58 | StdPeriph_Driver/src/stm32f10x_flash.c \ 59 | StdPeriph_Driver/src/stm32f10x_fsmc.c \ 60 | StdPeriph_Driver/src/stm32f10x_gpio.c \ 61 | StdPeriph_Driver/src/stm32f10x_i2c.c \ 62 | StdPeriph_Driver/src/stm32f10x_iwdg.c \ 63 | StdPeriph_Driver/src/stm32f10x_pwr.c \ 64 | StdPeriph_Driver/src/stm32f10x_rcc.c \ 65 | StdPeriph_Driver/src/stm32f10x_rtc.c \ 66 | StdPeriph_Driver/src/stm32f10x_sdio.c \ 67 | StdPeriph_Driver/src/stm32f10x_spi.c \ 68 | StdPeriph_Driver/src/stm32f10x_tim.c \ 69 | StdPeriph_Driver/src/stm32f10x_usart.c \ 70 | StdPeriph_Driver/src/stm32f10x_wwdg.c \ 71 | src/time/kov_time.c 72 | 73 | DISTFILES += \ 74 | Makefile \ 75 | startup/startup_stm32.s \ 76 | LinkerScript.ld 77 | 78 | HEADERS += \ 79 | CMSIS/core/core_cm3.h \ 80 | CMSIS/device/stm32f10x.h \ 81 | CMSIS/device/system_stm32f10x.h \ 82 | inc/commons/bearing.h \ 83 | inc/commons/commons.h \ 84 | inc/commons/madgwick.h \ 85 | inc/commons/magnetic.h \ 86 | inc/commons/quaternion.h \ 87 | inc/commons/vector3d.h \ 88 | inc/display/display.h \ 89 | inc/display/nokiaLcd/nokia5110.h \ 90 | inc/display/nokiaLcd/nokia5110_buffered.h \ 91 | inc/display/nokiaLcd/nokia5110_font.h \ 92 | inc/gps/nmea.h \ 93 | inc/magnetometer/hmc5883/hmc5883.h \ 94 | inc/i2c1/i2c1.h \ 95 | inc/magnetometer/lis3mdl/lis3mdl_i2c.h \ 96 | inc/magnetometer/qmc5883l/qmc5883l.h \ 97 | inc/stm32f10x_it.h \ 98 | inc/gps/neo6m.h \ 99 | inc/accelerometer/mpu6050/MPU6050.h \ 100 | inc/accelerometer/mpu6050/mpu6050_i2c.h \ 101 | inc/nokiaLcd/stm32f10x_pcd8544.h \ 102 | inc/stm32vldiscovery_utils.h \ 103 | StdPeriph_Driver/inc/misc.h \ 104 | StdPeriph_Driver/inc/stm32f10x_adc.h \ 105 | StdPeriph_Driver/inc/stm32f10x_bkp.h \ 106 | StdPeriph_Driver/inc/stm32f10x_can.h \ 107 | StdPeriph_Driver/inc/stm32f10x_cec.h \ 108 | StdPeriph_Driver/inc/stm32f10x_conf.h \ 109 | StdPeriph_Driver/inc/stm32f10x_crc.h \ 110 | StdPeriph_Driver/inc/stm32f10x_dac.h \ 111 | StdPeriph_Driver/inc/stm32f10x_dbgmcu.h \ 112 | StdPeriph_Driver/inc/stm32f10x_dma.h \ 113 | StdPeriph_Driver/inc/stm32f10x_exti.h \ 114 | StdPeriph_Driver/inc/stm32f10x_flash.h \ 115 | StdPeriph_Driver/inc/stm32f10x_fsmc.h \ 116 | StdPeriph_Driver/inc/stm32f10x_gpio.h \ 117 | StdPeriph_Driver/inc/stm32f10x_i2c.h \ 118 | StdPeriph_Driver/inc/stm32f10x_iwdg.h \ 119 | StdPeriph_Driver/inc/stm32f10x_pwr.h \ 120 | StdPeriph_Driver/inc/stm32f10x_rcc.h \ 121 | StdPeriph_Driver/inc/stm32f10x_rtc.h \ 122 | StdPeriph_Driver/inc/stm32f10x_sdio.h \ 123 | StdPeriph_Driver/inc/stm32f10x_spi.h \ 124 | StdPeriph_Driver/inc/stm32f10x_tim.h \ 125 | StdPeriph_Driver/inc/stm32f10x_usart.h \ 126 | StdPeriph_Driver/inc/stm32f10x_wwdg.h \ 127 | inc/time/kov_time.h \ 128 | inc/hardware.h 129 | 130 | -------------------------------------------------------------------------------- /kicad_schema/navigator_schema/navigatorschema.pro: -------------------------------------------------------------------------------- 1 | update=Пн 03 июн 2019 16:25:29 2 | version=1 3 | last_client=kicad 4 | [general] 5 | version=1 6 | RootSch= 7 | BoardNm= 8 | [cvpcb] 9 | version=1 10 | NetIExt=net 11 | [eeschema] 12 | version=1 13 | LibDir= 14 | [eeschema/libraries] 15 | [pcbnew] 16 | version=1 17 | PageLayoutDescrFile= 18 | LastNetListRead=kov_schema.net 19 | CopperLayerCount=2 20 | BoardThickness=1.6 21 | AllowMicroVias=0 22 | AllowBlindVias=0 23 | RequireCourtyardDefinitions=0 24 | ProhibitOverlappingCourtyards=1 25 | MinTrackWidth=0.2 26 | MinViaDiameter=0.4 27 | MinViaDrill=0.3 28 | MinMicroViaDiameter=0.2 29 | MinMicroViaDrill=0.09999999999999999 30 | MinHoleToHole=0.25 31 | TrackWidth1=0.25 32 | TrackWidth2=0.2 33 | ViaDiameter1=0.8 34 | ViaDrill1=0.4 35 | dPairWidth1=0.2 36 | dPairGap1=0.25 37 | dPairViaGap1=0.25 38 | SilkLineWidth=0.12 39 | SilkTextSizeV=1 40 | SilkTextSizeH=1 41 | SilkTextSizeThickness=0.15 42 | SilkTextItalic=0 43 | SilkTextUpright=1 44 | CopperLineWidth=0.2 45 | CopperTextSizeV=1.5 46 | CopperTextSizeH=1.5 47 | CopperTextThickness=0.3 48 | CopperTextItalic=0 49 | CopperTextUpright=1 50 | EdgeCutLineWidth=0.05 51 | CourtyardLineWidth=0.05 52 | OthersLineWidth=0.15 53 | OthersTextSizeV=1 54 | OthersTextSizeH=1 55 | OthersTextSizeThickness=0.15 56 | OthersTextItalic=0 57 | OthersTextUpright=1 58 | SolderMaskClearance=0.051 59 | SolderMaskMinWidth=0.25 60 | SolderPasteClearance=0 61 | SolderPasteRatio=-0 62 | [pcbnew/Layer.F.Cu] 63 | Name=F.Cu 64 | Type=0 65 | Enabled=1 66 | [pcbnew/Layer.In1.Cu] 67 | Name=In1.Cu 68 | Type=0 69 | Enabled=0 70 | [pcbnew/Layer.In2.Cu] 71 | Name=In2.Cu 72 | Type=0 73 | Enabled=0 74 | [pcbnew/Layer.In3.Cu] 75 | Name=In3.Cu 76 | Type=0 77 | Enabled=0 78 | [pcbnew/Layer.In4.Cu] 79 | Name=In4.Cu 80 | Type=0 81 | Enabled=0 82 | [pcbnew/Layer.In5.Cu] 83 | Name=In5.Cu 84 | Type=0 85 | Enabled=0 86 | [pcbnew/Layer.In6.Cu] 87 | Name=In6.Cu 88 | Type=0 89 | Enabled=0 90 | [pcbnew/Layer.In7.Cu] 91 | Name=In7.Cu 92 | Type=0 93 | Enabled=0 94 | [pcbnew/Layer.In8.Cu] 95 | Name=In8.Cu 96 | Type=0 97 | Enabled=0 98 | [pcbnew/Layer.In9.Cu] 99 | Name=In9.Cu 100 | Type=0 101 | Enabled=0 102 | [pcbnew/Layer.In10.Cu] 103 | Name=In10.Cu 104 | Type=0 105 | Enabled=0 106 | [pcbnew/Layer.In11.Cu] 107 | Name=In11.Cu 108 | Type=0 109 | Enabled=0 110 | [pcbnew/Layer.In12.Cu] 111 | Name=In12.Cu 112 | Type=0 113 | Enabled=0 114 | [pcbnew/Layer.In13.Cu] 115 | Name=In13.Cu 116 | Type=0 117 | Enabled=0 118 | [pcbnew/Layer.In14.Cu] 119 | Name=In14.Cu 120 | Type=0 121 | Enabled=0 122 | [pcbnew/Layer.In15.Cu] 123 | Name=In15.Cu 124 | Type=0 125 | Enabled=0 126 | [pcbnew/Layer.In16.Cu] 127 | Name=In16.Cu 128 | Type=0 129 | Enabled=0 130 | [pcbnew/Layer.In17.Cu] 131 | Name=In17.Cu 132 | Type=0 133 | Enabled=0 134 | [pcbnew/Layer.In18.Cu] 135 | Name=In18.Cu 136 | Type=0 137 | Enabled=0 138 | [pcbnew/Layer.In19.Cu] 139 | Name=In19.Cu 140 | Type=0 141 | Enabled=0 142 | [pcbnew/Layer.In20.Cu] 143 | Name=In20.Cu 144 | Type=0 145 | Enabled=0 146 | [pcbnew/Layer.In21.Cu] 147 | Name=In21.Cu 148 | Type=0 149 | Enabled=0 150 | [pcbnew/Layer.In22.Cu] 151 | Name=In22.Cu 152 | Type=0 153 | Enabled=0 154 | [pcbnew/Layer.In23.Cu] 155 | Name=In23.Cu 156 | Type=0 157 | Enabled=0 158 | [pcbnew/Layer.In24.Cu] 159 | Name=In24.Cu 160 | Type=0 161 | Enabled=0 162 | [pcbnew/Layer.In25.Cu] 163 | Name=In25.Cu 164 | Type=0 165 | Enabled=0 166 | [pcbnew/Layer.In26.Cu] 167 | Name=In26.Cu 168 | Type=0 169 | Enabled=0 170 | [pcbnew/Layer.In27.Cu] 171 | Name=In27.Cu 172 | Type=0 173 | Enabled=0 174 | [pcbnew/Layer.In28.Cu] 175 | Name=In28.Cu 176 | Type=0 177 | Enabled=0 178 | [pcbnew/Layer.In29.Cu] 179 | Name=In29.Cu 180 | Type=0 181 | Enabled=0 182 | [pcbnew/Layer.In30.Cu] 183 | Name=In30.Cu 184 | Type=0 185 | Enabled=0 186 | [pcbnew/Layer.B.Cu] 187 | Name=B.Cu 188 | Type=0 189 | Enabled=1 190 | [pcbnew/Layer.B.Adhes] 191 | Enabled=1 192 | [pcbnew/Layer.F.Adhes] 193 | Enabled=1 194 | [pcbnew/Layer.B.Paste] 195 | Enabled=1 196 | [pcbnew/Layer.F.Paste] 197 | Enabled=1 198 | [pcbnew/Layer.B.SilkS] 199 | Enabled=1 200 | [pcbnew/Layer.F.SilkS] 201 | Enabled=1 202 | [pcbnew/Layer.B.Mask] 203 | Enabled=1 204 | [pcbnew/Layer.F.Mask] 205 | Enabled=1 206 | [pcbnew/Layer.Dwgs.User] 207 | Enabled=1 208 | [pcbnew/Layer.Cmts.User] 209 | Enabled=1 210 | [pcbnew/Layer.Eco1.User] 211 | Enabled=1 212 | [pcbnew/Layer.Eco2.User] 213 | Enabled=1 214 | [pcbnew/Layer.Edge.Cuts] 215 | Enabled=1 216 | [pcbnew/Layer.Margin] 217 | Enabled=1 218 | [pcbnew/Layer.B.CrtYd] 219 | Enabled=1 220 | [pcbnew/Layer.F.CrtYd] 221 | Enabled=1 222 | [pcbnew/Layer.B.Fab] 223 | Enabled=1 224 | [pcbnew/Layer.F.Fab] 225 | Enabled=1 226 | [pcbnew/Layer.Rescue] 227 | Enabled=0 228 | [pcbnew/Netclasses] 229 | [pcbnew/Netclasses/Default] 230 | Name=Default 231 | Clearance=0.2 232 | TrackWidth=0.25 233 | ViaDiameter=0.8 234 | ViaDrill=0.4 235 | uViaDiameter=0.3 236 | uViaDrill=0.1 237 | dPairWidth=0.2 238 | dPairGap=0.25 239 | dPairViaGap=0.25 240 | -------------------------------------------------------------------------------- /inc/magnetometer/qmc5883l/qmc5883l.h: -------------------------------------------------------------------------------- 1 | #ifndef QMC5883L_H 2 | #define QMC5883L_H 3 | 4 | #include 5 | #include 6 | #include "i2c1/i2c1.h" 7 | 8 | #define QMC5883L_I2C_ADDR (0x0d << 1) 9 | 10 | #define QMC5883L_X_LSB 0x00 11 | #define QMC5883L_X_MSB 0x01 12 | 13 | #define QMC5883L_Y_LSB 0x02 14 | #define QMC5883L_Y_MSB 0x03 15 | 16 | #define QMC5883L_Z_LSB 0x04 17 | #define QMC5883L_Z_MSB 0x05 18 | 19 | #define QMC5883L_STATUS1 0x06 20 | #define QMC5883L_STATUS1_DRDY (1 << 0) 21 | #define QMC5883L_STATUS1_OVL (1 << 1) 22 | #define QMC5883L_STATUS1_DOR (1 << 2) 23 | 24 | #define QMC5883L_TEMP_LSB 0x07 25 | #define QMC5883L_TEMP_MSB 0x08 26 | 27 | #define QMC5883L_CTRL1 0x09 28 | /*Two 8-bits registers are used to control the device configurations. 29 | Control register 1 is located in address 09H, it sets the operational modes (MODE). output data update rate 30 | (ODR), magnetic field measurement range or sensitivity of the sensors (RNG) and over sampling rate (OSR). 31 | Control register 2 is located in address 0AH. It controls Interrupt Pin enabling (INT_ENB), Point roll over function 32 | enabling(POL_PNT) and soft reset (SOFT_RST).*/ 33 | /* 34 | Two bits of MODE registers can transfer mode of operations in the device, the two modes are Standby, and 35 | Continuous measurements. The default mode after Power-on-Reset (POR) is standby. There is no any restriction 36 | in the transferring between the modes.*/ 37 | #define QMC5883L_CTRL1_MODE_OFFSET 0 38 | #define QMC5883L_CTRL1_MODE_STANDBY 0x00 39 | #define QMC5883L_CTRL1_MODE_CONTINUOUS 0x01 40 | 41 | /* 42 | Output data rate is controlled by ODR registers. Four data update frequencies can be selected: 10Hz, 50Hz, 43 | 100Hz and 200Hz. For most of compassing applications, we recommend 10 Hz for low power consumption. For 44 | gaming, the high update rate such as 100Hz or 200Hz can be used.*/ 45 | #define QMC5883L_CTRL1_ODR_OFFSET 2 46 | #define QMC5883L_CTRL1_ODR_10 0x00 47 | #define QMC5883L_CTRL1_ODR_50 0x01 48 | #define QMC5883L_CTRL1_ODR_100 0x02 49 | #define QMC5883L_CTRL1_ODR_200 0x03 50 | 51 | /*Field ranges of the magnetic sensor can be selected through the register RNG. The full scale field range is 52 | determined by the application environments. For magnetic clear environment, low field range such as +/- 2gauss 53 | can be used. The field range goes hand in hand with the sensitivity of the magnetic sensor. The lowest field range 54 | has the highest sensitivity, therefore, higher resolution.*/ 55 | #define QMC5883L_CTRL1_RNG_OFFSET 4 56 | #define QMC5883L_CTRL1_RNG_2 0x00 57 | #define QMC5883L_CTRL1_RNG_8 0x01 58 | 59 | /*Over sample Rate (OSR) registers are used to control bandwidth of an internal digital filter. Larger OSR value 60 | leads to smaller filter bandwidth, less in-band noise and higher power consumption. It could be used to reach a 61 | good balance between noise and power. Four over sample ratio can be selected, 64, 128, 256 or 512.*/ 62 | #define QMC5883L_CTRL1_OSR_OFFSET 6 63 | #define QMC5883L_CTRL1_OSR_512 0x00 64 | #define QMC5883L_CTRL1_OSR_256 0x01 65 | #define QMC5883L_CTRL1_OSR_128 0x02 66 | #define QMC5883L_CTRL1_OSR_64 0x03 67 | 68 | 69 | #define QMC5883L_CTRL2 0x09 70 | 71 | /*Interrupt enabling is controlled by register INT_ENB in control register 2. Once the interrupt is enabled, it will flag 72 | when new data is in Data Output Registers. 73 | INT_ENB: “0”: enable interrupt PIN, “1”: disable interrupt PIN*/ 74 | #define QMC5883L_CTRL2_INT_ENB 0x00 75 | #define QMC5883L_CTRL2_INT_DIS 0x01 76 | 77 | /*Pointer roll-over function is controlled by ROL_PNT register. When the point roll-over function is enabled, the I2C 78 | data pointer automatically rolls between 00H ~ 06H, if I2C read begins at any address among 00H~06H. 79 | ROL_PNT: “0”: Normal, “1”: Enable pointer roll-over function*/ 80 | #define QMC5883L_CTRL2_ROL_PNT_EN (1 << 6) 81 | 82 | /*Soft Reset can be done by changing the register SOFT_RST to set. Soft reset can be invoked at any time of any 83 | mode. For example, if soft reset occurs at the middle of continuous mode reading, QMC5883L immediately 84 | switches to standby mode due to mode register is reset to “00” in default. 85 | SOFT_RST: “0”: Normal“1”: Soft reset, restore default value of all registers.*/ 86 | #define QMC5883L_CTRL2_SOFT_RST (1 << 7) 87 | 88 | #define QMC5883L_STATUS2 0x0c 89 | 90 | void qmc5883l_init(void); 91 | void qmc5883l_pin_config(void); 92 | void qmc5883l_power(bool on); 93 | 94 | typedef union { 95 | uint8_t raw8[6]; 96 | uint16_t raw16[3]; 97 | struct { 98 | int16_t mx, my, mz; 99 | } data; 100 | } qmc5883l_raw_data_t; 101 | 102 | void qmc5883lReadHeadingSync(qmc5883l_raw_data_t *data); 103 | void qmc5883lReadHeadingAsync(qmc5883l_raw_data_t *data, volatile int8_t *finishCode, i2c1_pf_callback cb); 104 | void qmc5883lFixRawData(qmc5883l_raw_data_t *data); 105 | 106 | float qmc5883l_gain(void); 107 | 108 | #endif // QMC5883L_H 109 | -------------------------------------------------------------------------------- /src/display/nokiaLcd/nokia5110_font.c: -------------------------------------------------------------------------------- 1 | #include "display/nokiaLcd/nokia5110_font.h" 2 | 3 | uint8_t font6_8[92][6] = { 4 | { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }, // sp 5 | { 0x00, 0x00, 0x00, 0x2f, 0x00, 0x00 }, // ! 6 | { 0x00, 0x00, 0x07, 0x00, 0x07, 0x00 }, // " 7 | { 0x00, 0x14, 0x7f, 0x14, 0x7f, 0x14 }, // # 8 | { 0x00, 0x24, 0x2a, 0x7f, 0x2a, 0x12 }, // $ 9 | { 0x00, 0x62, 0x64, 0x08, 0x13, 0x23 }, // % 10 | { 0x00, 0x36, 0x49, 0x55, 0x22, 0x50 }, // & 11 | { 0x00, 0x00, 0x05, 0x03, 0x00, 0x00 }, // ' 12 | { 0x00, 0x00, 0x1c, 0x22, 0x41, 0x00 }, // ( 13 | { 0x00, 0x00, 0x41, 0x22, 0x1c, 0x00 }, // ) 14 | { 0x00, 0x14, 0x08, 0x3E, 0x08, 0x14 }, // * 15 | { 0x00, 0x08, 0x08, 0x3E, 0x08, 0x08 }, // + 16 | { 0x00, 0x00, 0x00, 0xA0, 0x60, 0x00 }, // , 17 | { 0x00, 0x08, 0x08, 0x08, 0x08, 0x08 }, // - 18 | { 0x00, 0x00, 0x60, 0x60, 0x00, 0x00 }, // . 19 | { 0x00, 0x20, 0x10, 0x08, 0x04, 0x02 }, // / 20 | { 0x00, 0x3E, 0x51, 0x49, 0x45, 0x3E }, // 0 21 | { 0x00, 0x00, 0x42, 0x7F, 0x40, 0x00 }, // 1 22 | { 0x00, 0x42, 0x61, 0x51, 0x49, 0x46 }, // 2 23 | { 0x00, 0x21, 0x41, 0x45, 0x4B, 0x31 }, // 3 24 | { 0x00, 0x18, 0x14, 0x12, 0x7F, 0x10 }, // 4 25 | { 0x00, 0x27, 0x45, 0x45, 0x45, 0x39 }, // 5 26 | { 0x00, 0x3C, 0x4A, 0x49, 0x49, 0x30 }, // 6 27 | { 0x00, 0x01, 0x71, 0x09, 0x05, 0x03 }, // 7 28 | { 0x00, 0x36, 0x49, 0x49, 0x49, 0x36 }, // 8 29 | { 0x00, 0x06, 0x49, 0x49, 0x29, 0x1E }, // 9 30 | { 0x00, 0x00, 0x36, 0x36, 0x00, 0x00 }, // : 31 | { 0x00, 0x00, 0x56, 0x36, 0x00, 0x00 }, // ; 32 | { 0x00, 0x08, 0x14, 0x22, 0x41, 0x00 }, // < 33 | { 0x00, 0x14, 0x14, 0x14, 0x14, 0x14 }, // = 34 | { 0x00, 0x00, 0x41, 0x22, 0x14, 0x08 }, // > 35 | { 0x00, 0x02, 0x01, 0x51, 0x09, 0x06 }, // ? 36 | { 0x00, 0x32, 0x49, 0x59, 0x51, 0x3E }, // @ 37 | { 0x00, 0x7C, 0x12, 0x11, 0x12, 0x7C }, // A 38 | { 0x00, 0x7F, 0x49, 0x49, 0x49, 0x36 }, // B 39 | { 0x00, 0x3E, 0x41, 0x41, 0x41, 0x22 }, // C 40 | { 0x00, 0x7F, 0x41, 0x41, 0x22, 0x1C }, // D 41 | { 0x00, 0x7F, 0x49, 0x49, 0x49, 0x41 }, // E 42 | { 0x00, 0x7F, 0x09, 0x09, 0x09, 0x01 }, // F 43 | { 0x00, 0x3E, 0x41, 0x49, 0x49, 0x7A }, // G 44 | { 0x00, 0x7F, 0x08, 0x08, 0x08, 0x7F }, // H 45 | { 0x00, 0x00, 0x41, 0x7F, 0x41, 0x00 }, // I 46 | { 0x00, 0x20, 0x40, 0x41, 0x3F, 0x01 }, // J 47 | { 0x00, 0x7F, 0x08, 0x14, 0x22, 0x41 }, // K 48 | { 0x00, 0x7F, 0x40, 0x40, 0x40, 0x40 }, // L 49 | { 0x00, 0x7F, 0x02, 0x0C, 0x02, 0x7F }, // M 50 | { 0x00, 0x7F, 0x04, 0x08, 0x10, 0x7F }, // N 51 | { 0x00, 0x3E, 0x41, 0x41, 0x41, 0x3E }, // O 52 | { 0x00, 0x7F, 0x09, 0x09, 0x09, 0x06 }, // P 53 | { 0x00, 0x3E, 0x41, 0x51, 0x21, 0x5E }, // Q 54 | { 0x00, 0x7F, 0x09, 0x19, 0x29, 0x46 }, // R 55 | { 0x00, 0x46, 0x49, 0x49, 0x49, 0x31 }, // S 56 | { 0x00, 0x01, 0x01, 0x7F, 0x01, 0x01 }, // T 57 | { 0x00, 0x3F, 0x40, 0x40, 0x40, 0x3F }, // U 58 | { 0x00, 0x1F, 0x20, 0x40, 0x20, 0x1F }, // V 59 | { 0x00, 0x3F, 0x40, 0x38, 0x40, 0x3F }, // W 60 | { 0x00, 0x63, 0x14, 0x08, 0x14, 0x63 }, // X 61 | { 0x00, 0x07, 0x08, 0x70, 0x08, 0x07 }, // Y 62 | { 0x00, 0x61, 0x51, 0x49, 0x45, 0x43 }, // Z 63 | { 0x00, 0x00, 0x7F, 0x41, 0x41, 0x00 }, // [ 64 | { 0x00, 0x55, 0x2A, 0x55, 0x2A, 0x55 }, // 55 65 | { 0x00, 0x00, 0x41, 0x41, 0x7F, 0x00 }, // ] 66 | { 0x00, 0x04, 0x02, 0x01, 0x02, 0x04 }, // ^ 67 | { 0x00, 0x40, 0x40, 0x40, 0x40, 0x40 }, // _ 68 | { 0x00, 0x00, 0x01, 0x02, 0x04, 0x00 }, // ' 69 | { 0x00, 0x20, 0x54, 0x54, 0x54, 0x78 }, // a 70 | { 0x00, 0x7F, 0x48, 0x44, 0x44, 0x38 }, // b 71 | { 0x00, 0x38, 0x44, 0x44, 0x44, 0x20 }, // c 72 | { 0x00, 0x38, 0x44, 0x44, 0x48, 0x7F }, // d 73 | { 0x00, 0x38, 0x54, 0x54, 0x54, 0x18 }, // e 74 | { 0x00, 0x08, 0x7E, 0x09, 0x01, 0x02 }, // f 75 | { 0x00, 0x18, 0xA4, 0xA4, 0xA4, 0x7C }, // g 76 | { 0x00, 0x7F, 0x08, 0x04, 0x04, 0x78 }, // h 77 | { 0x00, 0x00, 0x44, 0x7D, 0x40, 0x00 }, // i 78 | { 0x00, 0x40, 0x80, 0x84, 0x7D, 0x00 }, // j 79 | { 0x00, 0x7F, 0x10, 0x28, 0x44, 0x00 }, // k 80 | { 0x00, 0x00, 0x41, 0x7F, 0x40, 0x00 }, // l 81 | { 0x00, 0x7C, 0x04, 0x18, 0x04, 0x78 }, // m 82 | { 0x00, 0x7C, 0x08, 0x04, 0x04, 0x78 }, // n 83 | { 0x00, 0x38, 0x44, 0x44, 0x44, 0x38 }, // o 84 | { 0x00, 0xFC, 0x24, 0x24, 0x24, 0x18 }, // p 85 | { 0x00, 0x18, 0x24, 0x24, 0x18, 0xFC }, // q 86 | { 0x00, 0x7C, 0x08, 0x04, 0x04, 0x08 }, // r 87 | { 0x00, 0x48, 0x54, 0x54, 0x54, 0x20 }, // s 88 | { 0x00, 0x04, 0x3F, 0x44, 0x40, 0x20 }, // t 89 | { 0x00, 0x3C, 0x40, 0x40, 0x20, 0x7C }, // u 90 | { 0x00, 0x1C, 0x20, 0x40, 0x20, 0x1C }, // v 91 | { 0x00, 0x3C, 0x40, 0x30, 0x40, 0x3C }, // w 92 | { 0x00, 0x44, 0x28, 0x10, 0x28, 0x44 }, // x 93 | { 0x00, 0x1C, 0xA0, 0xA0, 0xA0, 0x7C }, // y 94 | { 0x00, 0x44, 0x64, 0x54, 0x4C, 0x44 }, // z 95 | { 0x00, 0x00, 0x06, 0x09, 0x09, 0x06 } // horiz lines 96 | }; 97 | /////////////////////////////////////////////////////// 98 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_pwr.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_pwr.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 PWR 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_PWR_H 25 | #define __STM32F10x_PWR_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 PWR 39 | * @{ 40 | */ 41 | 42 | /** @defgroup PWR_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup PWR_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup PVD_detection_level 55 | * @{ 56 | */ 57 | 58 | #define PWR_PVDLevel_2V2 ((uint32_t)0x00000000) 59 | #define PWR_PVDLevel_2V3 ((uint32_t)0x00000020) 60 | #define PWR_PVDLevel_2V4 ((uint32_t)0x00000040) 61 | #define PWR_PVDLevel_2V5 ((uint32_t)0x00000060) 62 | #define PWR_PVDLevel_2V6 ((uint32_t)0x00000080) 63 | #define PWR_PVDLevel_2V7 ((uint32_t)0x000000A0) 64 | #define PWR_PVDLevel_2V8 ((uint32_t)0x000000C0) 65 | #define PWR_PVDLevel_2V9 ((uint32_t)0x000000E0) 66 | #define IS_PWR_PVD_LEVEL(LEVEL) (((LEVEL) == PWR_PVDLevel_2V2) || ((LEVEL) == PWR_PVDLevel_2V3)|| \ 67 | ((LEVEL) == PWR_PVDLevel_2V4) || ((LEVEL) == PWR_PVDLevel_2V5)|| \ 68 | ((LEVEL) == PWR_PVDLevel_2V6) || ((LEVEL) == PWR_PVDLevel_2V7)|| \ 69 | ((LEVEL) == PWR_PVDLevel_2V8) || ((LEVEL) == PWR_PVDLevel_2V9)) 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup Regulator_state_is_STOP_mode 75 | * @{ 76 | */ 77 | 78 | #define PWR_Regulator_ON ((uint32_t)0x00000000) 79 | #define PWR_Regulator_LowPower ((uint32_t)0x00000001) 80 | #define IS_PWR_REGULATOR(REGULATOR) (((REGULATOR) == PWR_Regulator_ON) || \ 81 | ((REGULATOR) == PWR_Regulator_LowPower)) 82 | /** 83 | * @} 84 | */ 85 | 86 | /** @defgroup STOP_mode_entry 87 | * @{ 88 | */ 89 | 90 | #define PWR_STOPEntry_WFI ((uint8_t)0x01) 91 | #define PWR_STOPEntry_WFE ((uint8_t)0x02) 92 | #define IS_PWR_STOP_ENTRY(ENTRY) (((ENTRY) == PWR_STOPEntry_WFI) || ((ENTRY) == PWR_STOPEntry_WFE)) 93 | 94 | /** 95 | * @} 96 | */ 97 | 98 | /** @defgroup PWR_Flag 99 | * @{ 100 | */ 101 | 102 | #define PWR_FLAG_WU ((uint32_t)0x00000001) 103 | #define PWR_FLAG_SB ((uint32_t)0x00000002) 104 | #define PWR_FLAG_PVDO ((uint32_t)0x00000004) 105 | #define IS_PWR_GET_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB) || \ 106 | ((FLAG) == PWR_FLAG_PVDO)) 107 | 108 | #define IS_PWR_CLEAR_FLAG(FLAG) (((FLAG) == PWR_FLAG_WU) || ((FLAG) == PWR_FLAG_SB)) 109 | /** 110 | * @} 111 | */ 112 | 113 | /** 114 | * @} 115 | */ 116 | 117 | /** @defgroup PWR_Exported_Macros 118 | * @{ 119 | */ 120 | 121 | /** 122 | * @} 123 | */ 124 | 125 | /** @defgroup PWR_Exported_Functions 126 | * @{ 127 | */ 128 | 129 | void PWR_DeInit(void); 130 | void PWR_BackupAccessCmd(FunctionalState NewState); 131 | void PWR_PVDCmd(FunctionalState NewState); 132 | void PWR_PVDLevelConfig(uint32_t PWR_PVDLevel); 133 | void PWR_WakeUpPinCmd(FunctionalState NewState); 134 | void PWR_EnterSTOPMode(uint32_t PWR_Regulator, uint8_t PWR_STOPEntry); 135 | void PWR_EnterSTANDBYMode(void); 136 | FlagStatus PWR_GetFlagStatus(uint32_t PWR_FLAG); 137 | void PWR_ClearFlag(uint32_t PWR_FLAG); 138 | 139 | #ifdef __cplusplus 140 | } 141 | #endif 142 | 143 | #endif /* __STM32F10x_PWR_H */ 144 | /** 145 | * @} 146 | */ 147 | 148 | /** 149 | * @} 150 | */ 151 | 152 | /** 153 | * @} 154 | */ 155 | 156 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 157 | -------------------------------------------------------------------------------- /src/syscalls.c: -------------------------------------------------------------------------------- 1 | /** 2 | ***************************************************************************** 3 | ** 4 | ** File : syscalls.c 5 | ** 6 | ** Abstract : System Workbench Minimal System calls file 7 | ** 8 | ** For more information about which c-functions 9 | ** need which of these lowlevel functions 10 | ** please consult the Newlib libc-manual 11 | ** 12 | ** Environment : System Workbench for MCU 13 | ** 14 | ** Distribution: The file is distributed �as is,� without any warranty 15 | ** of any kind. 16 | ** 17 | ***************************************************************************** 18 | ** 19 | **

© COPYRIGHT(c) 2014 Ac6

20 | ** 21 | ** Redistribution and use in source and binary forms, with or without modification, 22 | ** are permitted provided that the following conditions are met: 23 | ** 1. Redistributions of source code must retain the above copyright notice, 24 | ** this list of conditions and the following disclaimer. 25 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 26 | ** this list of conditions and the following disclaimer in the documentation 27 | ** and/or other materials provided with the distribution. 28 | ** 3. Neither the name of Ac6 nor the names of its contributors 29 | ** may be used to endorse or promote products derived from this software 30 | ** without specific prior written permission. 31 | ** 32 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 33 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 34 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 35 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 36 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 37 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 38 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 39 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 40 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 41 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 42 | ** 43 | ***************************************************************************** 44 | */ 45 | 46 | /* Includes */ 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | 57 | /* Variables */ 58 | //#undef errno 59 | extern int errno; 60 | extern int __io_putchar(int ch) __attribute__((weak)); 61 | extern int __io_getchar(void) __attribute__((weak)); 62 | 63 | register char * stack_ptr asm("sp"); 64 | 65 | char *__env[1] = { 0 }; 66 | char **environ = __env; 67 | 68 | 69 | /* Functions */ 70 | void initialise_monitor_handles() 71 | { 72 | } 73 | 74 | int _getpid(void) 75 | { 76 | return 1; 77 | } 78 | 79 | int _kill(int pid, int sig) 80 | { 81 | errno = EINVAL; 82 | return -1; 83 | } 84 | 85 | void _exit (int status) 86 | { 87 | _kill(status, -1); 88 | while (1) {} /* Make sure we hang here */ 89 | } 90 | 91 | int _read (int file, char *ptr, int len) 92 | { 93 | int DataIdx; 94 | 95 | for (DataIdx = 0; DataIdx < len; DataIdx++) 96 | { 97 | *ptr++ = __io_getchar(); 98 | } 99 | 100 | return len; 101 | } 102 | 103 | int _write(int file, char *ptr, int len) 104 | { 105 | int DataIdx; 106 | 107 | for (DataIdx = 0; DataIdx < len; DataIdx++) 108 | { 109 | __io_putchar(*ptr++); 110 | } 111 | return len; 112 | } 113 | 114 | caddr_t _sbrk(int incr) 115 | { 116 | extern char end asm("end"); 117 | static char *heap_end; 118 | char *prev_heap_end; 119 | 120 | if (heap_end == 0) 121 | heap_end = &end; 122 | 123 | prev_heap_end = heap_end; 124 | if (heap_end + incr > stack_ptr) 125 | { 126 | // write(1, "Heap and stack collision\n", 25); 127 | // abort(); 128 | errno = ENOMEM; 129 | return (caddr_t) -1; 130 | } 131 | 132 | heap_end += incr; 133 | 134 | return (caddr_t) prev_heap_end; 135 | } 136 | 137 | int _close(int file) 138 | { 139 | return -1; 140 | } 141 | 142 | 143 | int _fstat(int file, struct stat *st) 144 | { 145 | st->st_mode = S_IFCHR; 146 | return 0; 147 | } 148 | 149 | int _isatty(int file) 150 | { 151 | return 1; 152 | } 153 | 154 | int _lseek(int file, int ptr, int dir) 155 | { 156 | return 0; 157 | } 158 | 159 | int _open(char *path, int flags, ...) 160 | { 161 | /* Pretend like we always fail */ 162 | return -1; 163 | } 164 | 165 | int _wait(int *status) 166 | { 167 | errno = ECHILD; 168 | return -1; 169 | } 170 | 171 | int _unlink(char *name) 172 | { 173 | errno = ENOENT; 174 | return -1; 175 | } 176 | 177 | int _times(struct tms *buf) 178 | { 179 | return -1; 180 | } 181 | 182 | int _stat(char *file, struct stat *st) 183 | { 184 | st->st_mode = S_IFCHR; 185 | return 0; 186 | } 187 | 188 | int _link(char *old, char *new) 189 | { 190 | errno = EMLINK; 191 | return -1; 192 | } 193 | 194 | int _fork(void) 195 | { 196 | errno = EAGAIN; 197 | return -1; 198 | } 199 | 200 | int _execve(char *name, char **argv, char **env) 201 | { 202 | errno = ENOMEM; 203 | return -1; 204 | } 205 | -------------------------------------------------------------------------------- /src/gps/neo6m.c: -------------------------------------------------------------------------------- 1 | /* 2 | * neo6m.c 3 | * 4 | * Created on: Apr 18, 2019 5 | * Author: raistlin 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | #include "commons/commons.h" 12 | #include "gps/neo6m.h" 13 | #include "stm32vldiscovery_utils.h" 14 | #include "time/kov_time.h" 15 | #include "nmea.h" 16 | 17 | #define NEO6_PORT GPIOB 18 | #define NEO6_PIN_TX GPIO_Pin_10 19 | #define NEO6_PIN_RX GPIO_Pin_11 20 | #define NEO6_PIN_VCC GPIO_Pin_12 21 | 22 | volatile bool neo6m_needParseCurrentBuff = false; 23 | 24 | //if something goes wrong - make volatile 25 | static char internalBuffer[NMEA_MAX_LENGTH+1] = {0}; 26 | static uint8_t bufferPos = 0; 27 | /////////////////////////////////////////////////////// 28 | 29 | void neo6m_power(bool on) { 30 | void (*gpio_change)(GPIO_TypeDef*, uint16_t) = on ? GPIO_SetBits : GPIO_ResetBits; 31 | gpio_change(NEO6_PORT, NEO6_PIN_VCC); 32 | } 33 | /////////////////////////////////////////////////////// 34 | 35 | void neo6m_init() { 36 | /* USART configuration structure for USART3 */ 37 | USART_InitTypeDef usart3_cfg; 38 | /* Bit configuration structure for GPIOA PIN9 and PIN10 */ 39 | GPIO_InitTypeDef gpiob_cfg; 40 | NVIC_InitTypeDef nvicInitCfg; 41 | 42 | /* Enalbe clock for USART3, AFIO and GPIOB */ 43 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_USART3, ENABLE); 44 | RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOB, ENABLE); 45 | 46 | gpiob_cfg.GPIO_Pin = NEO6_PIN_TX; 47 | gpiob_cfg.GPIO_Speed = GPIO_Speed_50MHz; 48 | gpiob_cfg.GPIO_Mode = GPIO_Mode_AF_PP; 49 | GPIO_Init(NEO6_PORT, &gpiob_cfg); 50 | 51 | gpiob_cfg.GPIO_Pin = NEO6_PIN_RX; 52 | gpiob_cfg.GPIO_Speed = GPIO_Speed_50MHz; 53 | gpiob_cfg.GPIO_Mode = GPIO_Mode_IN_FLOATING; 54 | GPIO_Init(NEO6_PORT, &gpiob_cfg); 55 | 56 | gpiob_cfg.GPIO_Pin = NEO6_PIN_VCC; 57 | gpiob_cfg.GPIO_Speed = GPIO_Speed_50MHz; 58 | gpiob_cfg.GPIO_Mode = GPIO_Mode_Out_PP; 59 | GPIO_Init(NEO6_PORT, &gpiob_cfg); 60 | 61 | /* Baud rate 9600, 8-bit data, One stop bit 62 | * No parity, Do Rx, No HW flow control 63 | */ 64 | usart3_cfg.USART_BaudRate = 9600; 65 | usart3_cfg.USART_WordLength = USART_WordLength_8b; 66 | usart3_cfg.USART_StopBits = USART_StopBits_1; 67 | usart3_cfg.USART_Parity = USART_Parity_No; 68 | usart3_cfg.USART_Mode = USART_Mode_Rx; 69 | usart3_cfg.USART_HardwareFlowControl = USART_HardwareFlowControl_None; 70 | /* Configure USART3 */ 71 | USART_Init(USART3, &usart3_cfg); 72 | /* Enable RXNE interrupt */ 73 | USART_ITConfig(USART3, USART_IT_RXNE, ENABLE); 74 | 75 | /* Enable USART3 global interrupt */ 76 | /* Enable the USART3 event Interrupt */ 77 | nvicInitCfg.NVIC_IRQChannel = USART3_IRQn; 78 | nvicInitCfg.NVIC_IRQChannelPreemptionPriority = 1; 79 | nvicInitCfg.NVIC_IRQChannelSubPriority = 0; 80 | nvicInitCfg.NVIC_IRQChannelCmd = DISABLE; 81 | NVIC_Init(&nvicInitCfg); 82 | 83 | /* Enable USART3 */ 84 | USART_Cmd(USART3, ENABLE); 85 | } 86 | /////////////////////////////////////////////////////// 87 | 88 | static void put_char(char c) { 89 | internalBuffer[bufferPos] = c; 90 | internalBuffer[++bufferPos] = 0; 91 | } 92 | 93 | static volatile uint8_t cs = 0; 94 | static void rx_start(char c) { 95 | if (c != '$' && c != '!') 96 | return; 97 | cs = 1; 98 | put_char(c); 99 | } 100 | 101 | static inline void rx_put(char c) { 102 | put_char(c); 103 | if (c == '\n' && internalBuffer[bufferPos-2] == '\r') { 104 | NVIC_DisableIRQ(USART3_IRQn); 105 | neo6m_needParseCurrentBuff = true; 106 | cs = 0; 107 | bufferPos = 0; 108 | } 109 | 110 | if (bufferPos > NMEA_MAX_LENGTH) { 111 | cs = 0; 112 | bufferPos = 0; 113 | } 114 | } 115 | /////////////////////////////////////////////////////// 116 | 117 | void USART3_IRQHandler() { 118 | static void (*dfa[2])(char) = { rx_start, rx_put }; 119 | uint16_t sr = USART3->SR; 120 | char c = (char)USART_ReceiveData(USART3); 121 | if (!(sr & USART_SR_RXNE)) 122 | return; 123 | dfa[cs](c); 124 | } 125 | /////////////////////////////////////////////////////// 126 | 127 | static void convert_rmc2neo6(const struct nmea_sentence_rmc *rmc, neo6m_data_t *neo6) { 128 | neo6->date = rmc->date; 129 | if (rmc->date.year < 1900) 130 | neo6->date.year += 2000; //HACK! 131 | neo6->time = rmc->time; 132 | 133 | neo6->speed_knots = nmea_tofloat(&rmc->speed); 134 | neo6->magnetic_variation = nmea_tofloat(&rmc->variation); 135 | neo6->course = nmea_tofloat(&rmc->course); 136 | 137 | neo6->longitude = nmea_tocoord(&rmc->longitude); 138 | neo6->latitude = nmea_tocoord(&rmc->latitude); 139 | } 140 | /////////////////////////////////////////////////////// 141 | 142 | neo6m_data_t neo6m_parseCurrentBuff(bool *valid) { 143 | neo6m_data_t res; 144 | *valid = false; 145 | memset(&res, 0, sizeof(neo6m_data_t)); 146 | enum nmea_sentence_id sid = nmea_sentence_id(internalBuffer, false); 147 | struct nmea_sentence_rmc rmc; 148 | switch(sid) { 149 | case NMEA_SENTENCE_RMC: 150 | if ((*valid = nmea_parse_rmc(&rmc, internalBuffer))) { 151 | if ((*valid = rmc.valid)) 152 | convert_rmc2neo6(&rmc, &res); 153 | } 154 | break; 155 | default: 156 | break; 157 | } 158 | NVIC_EnableIRQ(USART3_IRQn); 159 | return res; 160 | } 161 | /////////////////////////////////////////////////////// 162 | 163 | void neo6m_interrupts(bool enable) { 164 | if (enable) NVIC_EnableIRQ(USART3_IRQn); 165 | else NVIC_DisableIRQ(USART3_IRQn); 166 | } 167 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /StdPeriph_Driver/src/stm32f10x_dbgmcu.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_dbgmcu.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the DBGMCU 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_dbgmcu.h" 24 | 25 | /** @addtogroup STM32F10x_StdPeriph_Driver 26 | * @{ 27 | */ 28 | 29 | /** @defgroup DBGMCU 30 | * @brief DBGMCU driver modules 31 | * @{ 32 | */ 33 | 34 | /** @defgroup DBGMCU_Private_TypesDefinitions 35 | * @{ 36 | */ 37 | 38 | /** 39 | * @} 40 | */ 41 | 42 | /** @defgroup DBGMCU_Private_Defines 43 | * @{ 44 | */ 45 | 46 | #define IDCODE_DEVID_MASK ((uint32_t)0x00000FFF) 47 | /** 48 | * @} 49 | */ 50 | 51 | /** @defgroup DBGMCU_Private_Macros 52 | * @{ 53 | */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /** @defgroup DBGMCU_Private_Variables 60 | * @{ 61 | */ 62 | 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @defgroup DBGMCU_Private_FunctionPrototypes 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup DBGMCU_Private_Functions 76 | * @{ 77 | */ 78 | 79 | /** 80 | * @brief Returns the device revision identifier. 81 | * @param None 82 | * @retval Device revision identifier 83 | */ 84 | uint32_t DBGMCU_GetREVID(void) 85 | { 86 | return(DBGMCU->IDCODE >> 16); 87 | } 88 | 89 | /** 90 | * @brief Returns the device identifier. 91 | * @param None 92 | * @retval Device identifier 93 | */ 94 | uint32_t DBGMCU_GetDEVID(void) 95 | { 96 | return(DBGMCU->IDCODE & IDCODE_DEVID_MASK); 97 | } 98 | 99 | /** 100 | * @brief Configures the specified peripheral and low power mode behavior 101 | * when the MCU under Debug mode. 102 | * @param DBGMCU_Periph: specifies the peripheral and low power mode. 103 | * This parameter can be any combination of the following values: 104 | * @arg DBGMCU_SLEEP: Keep debugger connection during SLEEP mode 105 | * @arg DBGMCU_STOP: Keep debugger connection during STOP mode 106 | * @arg DBGMCU_STANDBY: Keep debugger connection during STANDBY mode 107 | * @arg DBGMCU_IWDG_STOP: Debug IWDG stopped when Core is halted 108 | * @arg DBGMCU_WWDG_STOP: Debug WWDG stopped when Core is halted 109 | * @arg DBGMCU_TIM1_STOP: TIM1 counter stopped when Core is halted 110 | * @arg DBGMCU_TIM2_STOP: TIM2 counter stopped when Core is halted 111 | * @arg DBGMCU_TIM3_STOP: TIM3 counter stopped when Core is halted 112 | * @arg DBGMCU_TIM4_STOP: TIM4 counter stopped when Core is halted 113 | * @arg DBGMCU_CAN1_STOP: Debug CAN2 stopped when Core is halted 114 | * @arg DBGMCU_I2C1_SMBUS_TIMEOUT: I2C1 SMBUS timeout mode stopped when Core is halted 115 | * @arg DBGMCU_I2C2_SMBUS_TIMEOUT: I2C2 SMBUS timeout mode stopped when Core is halted 116 | * @arg DBGMCU_TIM5_STOP: TIM5 counter stopped when Core is halted 117 | * @arg DBGMCU_TIM6_STOP: TIM6 counter stopped when Core is halted 118 | * @arg DBGMCU_TIM7_STOP: TIM7 counter stopped when Core is halted 119 | * @arg DBGMCU_TIM8_STOP: TIM8 counter stopped when Core is halted 120 | * @arg DBGMCU_CAN2_STOP: Debug CAN2 stopped when Core is halted 121 | * @arg DBGMCU_TIM15_STOP: TIM15 counter stopped when Core is halted 122 | * @arg DBGMCU_TIM16_STOP: TIM16 counter stopped when Core is halted 123 | * @arg DBGMCU_TIM17_STOP: TIM17 counter stopped when Core is halted 124 | * @arg DBGMCU_TIM9_STOP: TIM9 counter stopped when Core is halted 125 | * @arg DBGMCU_TIM10_STOP: TIM10 counter stopped when Core is halted 126 | * @arg DBGMCU_TIM11_STOP: TIM11 counter stopped when Core is halted 127 | * @arg DBGMCU_TIM12_STOP: TIM12 counter stopped when Core is halted 128 | * @arg DBGMCU_TIM13_STOP: TIM13 counter stopped when Core is halted 129 | * @arg DBGMCU_TIM14_STOP: TIM14 counter stopped when Core is halted 130 | * @param NewState: new state of the specified peripheral in Debug mode. 131 | * This parameter can be: ENABLE or DISABLE. 132 | * @retval None 133 | */ 134 | void DBGMCU_Config(uint32_t DBGMCU_Periph, FunctionalState NewState) 135 | { 136 | /* Check the parameters */ 137 | assert_param(IS_DBGMCU_PERIPH(DBGMCU_Periph)); 138 | assert_param(IS_FUNCTIONAL_STATE(NewState)); 139 | 140 | if (NewState != DISABLE) 141 | { 142 | DBGMCU->CR |= DBGMCU_Periph; 143 | } 144 | else 145 | { 146 | DBGMCU->CR &= ~DBGMCU_Periph; 147 | } 148 | } 149 | 150 | /** 151 | * @} 152 | */ 153 | 154 | /** 155 | * @} 156 | */ 157 | 158 | /** 159 | * @} 160 | */ 161 | 162 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 163 | -------------------------------------------------------------------------------- /inc/gps/nmea.h: -------------------------------------------------------------------------------- 1 | #ifndef NMEA_H 2 | #define NMEA_H 3 | 4 | /* 5 | * Copyright © 2014 Kosma Moczek 6 | * This program is free software. It comes without any warranty, to the extent 7 | * permitted by applicable law. You can redistribute it and/or modify it under 8 | * the terms of the Do What The Fuck You Want To Public License, Version 2, as 9 | * published by Sam Hocevar. See the COPYING file for more details. 10 | */ 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include "time/kov_time.h" 20 | #define NMEA_MAX_LENGTH 82 21 | 22 | enum nmea_sentence_id { 23 | NMEA_INVALID = -1, 24 | NMEA_UNKNOWN = 0, 25 | NMEA_SENTENCE_RMC, 26 | NMEA_SENTENCE_GGA, 27 | NMEA_SENTENCE_GSA, 28 | NMEA_SENTENCE_GLL, 29 | NMEA_SENTENCE_GST, 30 | NMEA_SENTENCE_GSV, 31 | NMEA_SENTENCE_VTG, 32 | NMEA_SENTENCE_ZDA, 33 | }; 34 | 35 | struct nmea_float { 36 | int_least32_t value; 37 | int_least32_t scale; 38 | }; 39 | 40 | struct nmea_sentence_rmc { 41 | kov_time_t time; 42 | kov_date_t date; 43 | bool valid; 44 | struct nmea_float latitude; 45 | struct nmea_float longitude; 46 | struct nmea_float speed; 47 | struct nmea_float course; 48 | struct nmea_float variation; 49 | }; 50 | 51 | struct nmea_sentence_gga { 52 | kov_time_t time; 53 | struct nmea_float latitude; 54 | struct nmea_float longitude; 55 | int fix_quality; 56 | int satellites_tracked; 57 | struct nmea_float hdop; 58 | struct nmea_float altitude; char altitude_units; 59 | struct nmea_float height; char height_units; 60 | struct nmea_float dgps_age; 61 | }; 62 | 63 | enum nmea_gll_status { 64 | NMEA_GLL_STATUS_DATA_VALID = 'A', 65 | NMEA_GLL_STATUS_DATA_NOT_VALID = 'V', 66 | }; 67 | 68 | // FAA mode added to some fields in NMEA 2.3. 69 | enum nmea_faa_mode { 70 | NMEA_FAA_MODE_AUTONOMOUS = 'A', 71 | NMEA_FAA_MODE_DIFFERENTIAL = 'D', 72 | NMEA_FAA_MODE_ESTIMATED = 'E', 73 | NMEA_FAA_MODE_MANUAL = 'M', 74 | NMEA_FAA_MODE_SIMULATED = 'S', 75 | NMEA_FAA_MODE_NOT_VALID = 'N', 76 | NMEA_FAA_MODE_PRECISE = 'P', 77 | }; 78 | 79 | struct nmea_sentence_gll { 80 | struct nmea_float latitude; 81 | struct nmea_float longitude; 82 | kov_time_t time; 83 | char status; 84 | char mode; 85 | }; 86 | 87 | struct nmea_sentence_gst { 88 | kov_time_t time; 89 | struct nmea_float rms_deviation; 90 | struct nmea_float semi_major_deviation; 91 | struct nmea_float semi_minor_deviation; 92 | struct nmea_float semi_major_orientation; 93 | struct nmea_float latitude_error_deviation; 94 | struct nmea_float longitude_error_deviation; 95 | struct nmea_float altitude_error_deviation; 96 | }; 97 | 98 | enum nmea_gsa_mode { 99 | NMEA_GPGSA_MODE_AUTO = 'A', 100 | NMEA_GPGSA_MODE_FORCED = 'M', 101 | }; 102 | 103 | enum nmea_gsa_fix_type { 104 | NMEA_GPGSA_FIX_NONE = 1, 105 | NMEA_GPGSA_FIX_2D = 2, 106 | NMEA_GPGSA_FIX_3D = 3, 107 | }; 108 | 109 | struct nmea_sentence_gsa { 110 | char mode; 111 | int fix_type; 112 | int sats[12]; 113 | struct nmea_float pdop; 114 | struct nmea_float hdop; 115 | struct nmea_float vdop; 116 | }; 117 | 118 | struct nmea_sat_info { 119 | int nr; 120 | int elevation; 121 | int azimuth; 122 | int snr; 123 | }; 124 | 125 | struct nmea_sentence_gsv { 126 | int total_msgs; 127 | int msg_nr; 128 | int total_sats; 129 | struct nmea_sat_info sats[4]; 130 | }; 131 | 132 | struct nmea_sentence_vtg { 133 | struct nmea_float true_track_degrees; 134 | struct nmea_float magnetic_track_degrees; 135 | struct nmea_float speed_knots; 136 | struct nmea_float speed_kph; 137 | enum nmea_faa_mode faa_mode; 138 | }; 139 | 140 | struct nmea_sentence_zda { 141 | kov_time_t time; 142 | kov_date_t date; 143 | int hour_offset; 144 | int minute_offset; 145 | }; 146 | 147 | /** 148 | * Calculate raw sentence checksum. Does not check sentence integrity. 149 | */ 150 | uint8_t nmea_checksum(const char *sentence); 151 | 152 | /** 153 | * Check sentence validity and checksum. Returns true for valid sentences. 154 | */ 155 | bool nmea_check(const char *sentence, bool strict); 156 | 157 | /** 158 | * Determine talker identifier. 159 | */ 160 | bool nmea_talker_id(char talker[3], const char *sentence); 161 | 162 | /** 163 | * Determine sentence identifier. 164 | */ 165 | enum nmea_sentence_id nmea_sentence_id(const char *sentence, bool strict); 166 | 167 | /** 168 | * Scanf-like processor for NMEA sentences. Supports the following formats: 169 | * c - single character (char *) 170 | * d - direction, returned as 1/-1, default 0 (int *) 171 | * f - fractional, returned as value + scale (int *, int *) 172 | * i - decimal, default zero (int *) 173 | * s - string (char *) 174 | * t - talker identifier and type (char *) 175 | * T - date/time stamp (int *, int *, int *) 176 | * Returns true on success. See library source code for details. 177 | */ 178 | bool nmea_scan(const char *sentence, const char *format, ...); 179 | 180 | /* 181 | * Parse a specific type of sentence. Return true on success. 182 | */ 183 | bool nmea_parse_rmc(struct nmea_sentence_rmc *frame, const char *sentence); 184 | bool nmea_parse_gga(struct nmea_sentence_gga *frame, const char *sentence); 185 | bool nmea_parse_gsa(struct nmea_sentence_gsa *frame, const char *sentence); 186 | bool nmea_parse_gll(struct nmea_sentence_gll *frame, const char *sentence); 187 | bool nmea_parse_gst(struct nmea_sentence_gst *frame, const char *sentence); 188 | bool nmea_parse_gsv(struct nmea_sentence_gsv *frame, const char *sentence); 189 | bool nmea_parse_vtg(struct nmea_sentence_vtg *frame, const char *sentence); 190 | bool nmea_parse_zda(struct nmea_sentence_zda *frame, const char *sentence); 191 | 192 | /** 193 | * Convert GPS UTC date/time representation to a UNIX timestamp. 194 | */ 195 | kov_timestamp_t nmea_gettime(const kov_date_t *date, const kov_time_t *time_); 196 | 197 | 198 | /** 199 | * Rescale a fixed-point value to a different scale. Rounds towards zero. 200 | */ 201 | int_least32_t nmea_rescale(struct nmea_float *f, int_least32_t new_scale); 202 | 203 | /** 204 | * Convert a fixed-point value to a floating-point value. 205 | * Returns NaN for "unknown" values. 206 | */ 207 | float nmea_tofloat(const struct nmea_float *f); 208 | 209 | /** 210 | * Convert a raw coordinate to a floating point DD.DDD... value. 211 | * Returns NaN for "unknown" values. 212 | */ 213 | float nmea_tocoord(const struct nmea_float *f); 214 | 215 | #endif // NMEA_H 216 | -------------------------------------------------------------------------------- /LinkerScript.ld: -------------------------------------------------------------------------------- 1 | /* 2 | ****************************************************************************** 3 | ** 4 | ** File : LinkerScript.ld 5 | ** 6 | ** Author : Auto-generated by Ac6 System Workbench 7 | ** 8 | ** Abstract : Linker script for STM32F100RBTx Device from STM32F1 series 9 | ** 8Kbytes RAM 10 | ** 128Kbytes ROM 11 | ** 12 | ** Set heap size, stack size and stack location according 13 | ** to application requirements. 14 | ** 15 | ** Set memory bank area and size if external memory is used. 16 | ** 17 | ** Target : STMicroelectronics STM32 18 | ** 19 | ** Distribution: The file is distributed �as is,� without any warranty 20 | ** of any kind. 21 | ** 22 | ***************************************************************************** 23 | ** @attention 24 | ** 25 | **

© COPYRIGHT(c) 2019 Ac6

26 | ** 27 | ** Redistribution and use in source and binary forms, with or without modification, 28 | ** are permitted provided that the following conditions are met: 29 | ** 1. Redistributions of source code must retain the above copyright notice, 30 | ** this list of conditions and the following disclaimer. 31 | ** 2. Redistributions in binary form must reproduce the above copyright notice, 32 | ** this list of conditions and the following disclaimer in the documentation 33 | ** and/or other materials provided with the distribution. 34 | ** 3. Neither the name of Ac6 nor the names of its contributors 35 | ** may be used to endorse or promote products derived from this software 36 | ** without specific prior written permission. 37 | ** 38 | ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 39 | ** AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 40 | ** IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 41 | ** DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 42 | ** FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 43 | ** DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 44 | ** SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 45 | ** CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 46 | ** OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 47 | ** OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 48 | ** 49 | ***************************************************************************** 50 | */ 51 | 52 | /* Entry Point */ 53 | ENTRY(Reset_Handler) 54 | 55 | /* Highest address of the user mode stack */ 56 | _estack = 0x20002000; /* end of RAM */ 57 | 58 | _Min_Heap_Size = 0; /* required amount of heap */ 59 | _Min_Stack_Size = 0x400; /* required amount of stack */ 60 | 61 | /* Memories definition */ 62 | MEMORY 63 | { 64 | RAM (xrw) : ORIGIN = 0x20000000, LENGTH = 8K 65 | ROM (rx) : ORIGIN = 0x08000000, LENGTH = 128K 66 | } 67 | 68 | /* Sections */ 69 | SECTIONS 70 | { 71 | /* The startup code into ROM memory */ 72 | .isr_vector : 73 | { 74 | . = ALIGN(4); 75 | KEEP(*(.isr_vector)) /* Startup code */ 76 | . = ALIGN(4); 77 | } >ROM 78 | 79 | /* The program code and other data into ROM memory */ 80 | .text : 81 | { 82 | . = ALIGN(4); 83 | *(.text) /* .text sections (code) */ 84 | *(.text*) /* .text* sections (code) */ 85 | *(.glue_7) /* glue arm to thumb code */ 86 | *(.glue_7t) /* glue thumb to arm code */ 87 | *(.eh_frame) 88 | 89 | KEEP (*(.init)) 90 | KEEP (*(.fini)) 91 | 92 | . = ALIGN(4); 93 | _etext = .; /* define a global symbols at end of code */ 94 | } >ROM 95 | 96 | /* Constant data into ROM memory*/ 97 | .rodata : 98 | { 99 | . = ALIGN(4); 100 | *(.rodata) /* .rodata sections (constants, strings, etc.) */ 101 | *(.rodata*) /* .rodata* sections (constants, strings, etc.) */ 102 | . = ALIGN(4); 103 | } >ROM 104 | 105 | /* Data for unwinding */ 106 | .ARM.extab : { 107 | . = ALIGN(4); 108 | *(.ARM.extab* .gnu.linkonce.armextab.*) 109 | . = ALIGN(4); 110 | } >ROM 111 | 112 | .ARM : { 113 | . = ALIGN(4); 114 | __exidx_start = .; 115 | *(.ARM.exidx*) 116 | __exidx_end = .; 117 | . = ALIGN(4); 118 | } >ROM 119 | /* !!!!!!!!!!!!!!!!!!!!!!!!*/ 120 | 121 | .preinit_array : 122 | { 123 | . = ALIGN(4); 124 | PROVIDE_HIDDEN (__preinit_array_start = .); 125 | KEEP (*(.preinit_array*)) 126 | PROVIDE_HIDDEN (__preinit_array_end = .); 127 | . = ALIGN(4); 128 | } >ROM 129 | 130 | .init_array : 131 | { 132 | . = ALIGN(4); 133 | PROVIDE_HIDDEN (__init_array_start = .); 134 | KEEP (*(SORT(.init_array.*))) 135 | KEEP (*(.init_array*)) 136 | PROVIDE_HIDDEN (__init_array_end = .); 137 | . = ALIGN(4); 138 | } >ROM 139 | 140 | .fini_array : 141 | { 142 | . = ALIGN(4); 143 | PROVIDE_HIDDEN (__fini_array_start = .); 144 | KEEP (*(SORT(.fini_array.*))) 145 | KEEP (*(.fini_array*)) 146 | PROVIDE_HIDDEN (__fini_array_end = .); 147 | . = ALIGN(4); 148 | } >ROM 149 | 150 | /* Used by the startup to initialize data */ 151 | _sidata = LOADADDR(.data); 152 | 153 | /* Initialized data sections into RAM memory */ 154 | .data : 155 | { 156 | . = ALIGN(4); 157 | _sdata = .; /* create a global symbol at data start */ 158 | *(.data) /* .data sections */ 159 | *(.data*) /* .data* sections */ 160 | 161 | . = ALIGN(4); 162 | _edata = .; /* define a global symbol at data end */ 163 | } >RAM AT> ROM 164 | 165 | 166 | /* Uninitialized data section into RAM memory */ 167 | . = ALIGN(4); 168 | .bss : 169 | { 170 | /* This is used by the startup in order to initialize the .bss secion */ 171 | _sbss = .; /* define a global symbol at bss start */ 172 | __bss_start__ = _sbss; 173 | *(.bss) 174 | *(.bss*) 175 | *(COMMON) 176 | 177 | . = ALIGN(4); 178 | _ebss = .; /* define a global symbol at bss end */ 179 | __bss_end__ = _ebss; 180 | } >RAM 181 | 182 | /* User_heap_stack section, used to check that there is enough RAM left */ 183 | ._user_heap_stack : 184 | { 185 | . = ALIGN(8); 186 | PROVIDE ( end = . ); 187 | PROVIDE ( _end = . ); 188 | . = . + _Min_Heap_Size; 189 | . = . + _Min_Stack_Size; 190 | . = ALIGN(8); 191 | } >RAM 192 | 193 | 194 | 195 | /* Remove information from the compiler libraries */ 196 | /DISCARD/ : 197 | { 198 | libc.a ( * ) 199 | libm.a ( * ) 200 | libgcc.a ( * ) 201 | } 202 | 203 | .ARM.attributes 0 : { *(.ARM.attributes) } 204 | } 205 | -------------------------------------------------------------------------------- /StdPeriph_Driver/src/stm32f10x_wwdg.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_wwdg.c 4 | * @author MCD Application Team 5 | * @version V3.5.0 6 | * @date 11-March-2011 7 | * @brief This file provides all the WWDG 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_wwdg.h" 24 | #include "stm32f10x_rcc.h" 25 | 26 | /** @addtogroup STM32F10x_StdPeriph_Driver 27 | * @{ 28 | */ 29 | 30 | /** @defgroup WWDG 31 | * @brief WWDG driver modules 32 | * @{ 33 | */ 34 | 35 | /** @defgroup WWDG_Private_TypesDefinitions 36 | * @{ 37 | */ 38 | 39 | /** 40 | * @} 41 | */ 42 | 43 | /** @defgroup WWDG_Private_Defines 44 | * @{ 45 | */ 46 | 47 | /* ----------- WWDG registers bit address in the alias region ----------- */ 48 | #define WWDG_OFFSET (WWDG_BASE - PERIPH_BASE) 49 | 50 | /* Alias word address of EWI bit */ 51 | #define CFR_OFFSET (WWDG_OFFSET + 0x04) 52 | #define EWI_BitNumber 0x09 53 | #define CFR_EWI_BB (PERIPH_BB_BASE + (CFR_OFFSET * 32) + (EWI_BitNumber * 4)) 54 | 55 | /* --------------------- WWDG registers bit mask ------------------------ */ 56 | 57 | /* CR register bit mask */ 58 | #define CR_WDGA_Set ((uint32_t)0x00000080) 59 | 60 | /* CFR register bit mask */ 61 | #define CFR_WDGTB_Mask ((uint32_t)0xFFFFFE7F) 62 | #define CFR_W_Mask ((uint32_t)0xFFFFFF80) 63 | #define BIT_Mask ((uint8_t)0x7F) 64 | 65 | /** 66 | * @} 67 | */ 68 | 69 | /** @defgroup WWDG_Private_Macros 70 | * @{ 71 | */ 72 | 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup WWDG_Private_Variables 78 | * @{ 79 | */ 80 | 81 | /** 82 | * @} 83 | */ 84 | 85 | /** @defgroup WWDG_Private_FunctionPrototypes 86 | * @{ 87 | */ 88 | 89 | /** 90 | * @} 91 | */ 92 | 93 | /** @defgroup WWDG_Private_Functions 94 | * @{ 95 | */ 96 | 97 | /** 98 | * @brief Deinitializes the WWDG peripheral registers to their default reset values. 99 | * @param None 100 | * @retval None 101 | */ 102 | void WWDG_DeInit(void) 103 | { 104 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, ENABLE); 105 | RCC_APB1PeriphResetCmd(RCC_APB1Periph_WWDG, DISABLE); 106 | } 107 | 108 | /** 109 | * @brief Sets the WWDG Prescaler. 110 | * @param WWDG_Prescaler: specifies the WWDG Prescaler. 111 | * This parameter can be one of the following values: 112 | * @arg WWDG_Prescaler_1: WWDG counter clock = (PCLK1/4096)/1 113 | * @arg WWDG_Prescaler_2: WWDG counter clock = (PCLK1/4096)/2 114 | * @arg WWDG_Prescaler_4: WWDG counter clock = (PCLK1/4096)/4 115 | * @arg WWDG_Prescaler_8: WWDG counter clock = (PCLK1/4096)/8 116 | * @retval None 117 | */ 118 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler) 119 | { 120 | uint32_t tmpreg = 0; 121 | /* Check the parameters */ 122 | assert_param(IS_WWDG_PRESCALER(WWDG_Prescaler)); 123 | /* Clear WDGTB[1:0] bits */ 124 | tmpreg = WWDG->CFR & CFR_WDGTB_Mask; 125 | /* Set WDGTB[1:0] bits according to WWDG_Prescaler value */ 126 | tmpreg |= WWDG_Prescaler; 127 | /* Store the new value */ 128 | WWDG->CFR = tmpreg; 129 | } 130 | 131 | /** 132 | * @brief Sets the WWDG window value. 133 | * @param WindowValue: specifies the window value to be compared to the downcounter. 134 | * This parameter value must be lower than 0x80. 135 | * @retval None 136 | */ 137 | void WWDG_SetWindowValue(uint8_t WindowValue) 138 | { 139 | __IO uint32_t tmpreg = 0; 140 | 141 | /* Check the parameters */ 142 | assert_param(IS_WWDG_WINDOW_VALUE(WindowValue)); 143 | /* Clear W[6:0] bits */ 144 | 145 | tmpreg = WWDG->CFR & CFR_W_Mask; 146 | 147 | /* Set W[6:0] bits according to WindowValue value */ 148 | tmpreg |= WindowValue & (uint32_t) BIT_Mask; 149 | 150 | /* Store the new value */ 151 | WWDG->CFR = tmpreg; 152 | } 153 | 154 | /** 155 | * @brief Enables the WWDG Early Wakeup interrupt(EWI). 156 | * @param None 157 | * @retval None 158 | */ 159 | void WWDG_EnableIT(void) 160 | { 161 | *(__IO uint32_t *) CFR_EWI_BB = (uint32_t)ENABLE; 162 | } 163 | 164 | /** 165 | * @brief Sets the WWDG counter value. 166 | * @param Counter: specifies the watchdog counter value. 167 | * This parameter must be a number between 0x40 and 0x7F. 168 | * @retval None 169 | */ 170 | void WWDG_SetCounter(uint8_t Counter) 171 | { 172 | /* Check the parameters */ 173 | assert_param(IS_WWDG_COUNTER(Counter)); 174 | /* Write to T[6:0] bits to configure the counter value, no need to do 175 | a read-modify-write; writing a 0 to WDGA bit does nothing */ 176 | WWDG->CR = Counter & BIT_Mask; 177 | } 178 | 179 | /** 180 | * @brief Enables WWDG and load the counter value. 181 | * @param Counter: specifies the watchdog counter value. 182 | * This parameter must be a number between 0x40 and 0x7F. 183 | * @retval None 184 | */ 185 | void WWDG_Enable(uint8_t Counter) 186 | { 187 | /* Check the parameters */ 188 | assert_param(IS_WWDG_COUNTER(Counter)); 189 | WWDG->CR = CR_WDGA_Set | Counter; 190 | } 191 | 192 | /** 193 | * @brief Checks whether the Early Wakeup interrupt flag is set or not. 194 | * @param None 195 | * @retval The new state of the Early Wakeup interrupt flag (SET or RESET) 196 | */ 197 | FlagStatus WWDG_GetFlagStatus(void) 198 | { 199 | return (FlagStatus)(WWDG->SR); 200 | } 201 | 202 | /** 203 | * @brief Clears Early Wakeup interrupt flag. 204 | * @param None 205 | * @retval None 206 | */ 207 | void WWDG_ClearFlag(void) 208 | { 209 | WWDG->SR = (uint32_t)RESET; 210 | } 211 | 212 | /** 213 | * @} 214 | */ 215 | 216 | /** 217 | * @} 218 | */ 219 | 220 | /** 221 | * @} 222 | */ 223 | 224 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 225 | -------------------------------------------------------------------------------- /src/time/kov_time.c: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "time/kov_time.h" 7 | #include "stm32f10x_rtc.h" 8 | #include "stm32f10x_rcc.h" 9 | #include "display/display.h" 10 | #include "commons/commons.h" 11 | 12 | #define BACKUP_MAGIC 0xAD74 13 | 14 | 15 | static volatile bool isInUpdate = false; 16 | static void RTC_config(void); 17 | 18 | kov_timestamp_t RTC_CurrentTimestamp() { 19 | return RTC_GetCounter(); 20 | } 21 | /////////////////////////////////////////////////////// 22 | 23 | bool RTC_Adjust(kov_timestamp_t ts) { 24 | if(!isInUpdate) { 25 | isInUpdate = true; //do we have some atomics? 26 | PWR_BackupAccessCmd(ENABLE); 27 | RTC_WaitForLastTask(); 28 | RTC_SetCounter(ts); 29 | RTC_WaitForLastTask(); 30 | PWR_BackupAccessCmd(DISABLE); 31 | isInUpdate = false; 32 | } 33 | return false; 34 | } 35 | /////////////////////////////////////////////////////// 36 | 37 | void RTC_Init(void) { 38 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); 39 | if (BKP_ReadBackupRegister(BKP_DR1) != BACKUP_MAGIC) { 40 | RTC_config(); 41 | BKP_WriteBackupRegister(BKP_DR1, BACKUP_MAGIC); 42 | } else { 43 | RTC_WaitForSynchro(); 44 | RTC_WaitForLastTask(); 45 | } 46 | RCC_ClearFlag(); 47 | } 48 | /////////////////////////////////////////////////////// 49 | 50 | void RTC_config(void) { 51 | RCC_APB1PeriphClockCmd(RCC_APB1Periph_PWR | RCC_APB1Periph_BKP, ENABLE); 52 | PWR_BackupAccessCmd(ENABLE); 53 | // Если RTC выключен - инициализировать 54 | if ((RCC->BDCR & RCC_BDCR_RTCEN) != RCC_BDCR_RTCEN) { 55 | // Сброс данных в резервной области 56 | RCC_BackupResetCmd(ENABLE); 57 | RCC_BackupResetCmd(DISABLE); 58 | // Установить источник тактирования кварц 32768 59 | RCC_LSEConfig(RCC_LSE_ON); 60 | while ((RCC->BDCR & RCC_BDCR_LSERDY) != RCC_BDCR_LSERDY) {} 61 | RCC_RTCCLKConfig(RCC_RTCCLKSource_LSE); 62 | RTC_SetPrescaler(0x7FFF); // Устанавливаем делитель 32767, чтобы часы считали секунды 63 | // Включаем RTC 64 | RCC_RTCCLKCmd(ENABLE); 65 | // Ждем синхронизацию 66 | RTC_WaitForSynchro(); 67 | } 68 | } 69 | 70 | /////////////////////////////////////////////////////// 71 | 72 | // Get current date 73 | //https://ru.m.wikipedia.org/wiki/%D0%AE%D0%BB%D0%B8%D0%B0%D0%BD%D1%81%D0%BA%D0%B0%D1%8F_%D0%B4%D0%B0%D1%82%D0%B0 74 | void RTC_GetDateTime(kov_timestamp_t ts, 75 | kov_date_t *date, 76 | kov_time_t *time) { 77 | uint32_t ct; 78 | uint32_t t1, a, b, c, d, e, m; 79 | uint32_t jd = 0; 80 | uint32_t jdn = 0; 81 | 82 | uint16_t year = 0; 83 | uint8_t mon = 0; 84 | uint16_t mday = 0; 85 | uint32_t hour = 0; 86 | uint32_t min = 0; 87 | uint32_t sec = 0; 88 | 89 | jd = ((ts+43200)/(86400>>1)) + (2440587<<1) + 1; 90 | jdn = jd>>1; 91 | 92 | ct = ts; 93 | t1 = ct / 60u; 94 | sec = ct - t1*60u; 95 | 96 | ct = t1; 97 | t1 = ct/60; 98 | min = ct - t1*60; 99 | 100 | ct = t1; 101 | t1 = ct/24; 102 | hour = ct - t1*24; 103 | 104 | a = jdn + 32044; 105 | b = (4*a + 3) / 146097; 106 | c = a - (146097*b) / 4; 107 | d = (4*c + 3) / 1461; 108 | e = c - (1461*d) / 4; 109 | m = (5*e + 2) / 153; 110 | mday = e - (153*m + 2) / 5 + 1; 111 | mon = m + 3 - 12 * (m / 10); 112 | year = 100*b + d - 4800 + (m / 10); 113 | 114 | date->year = year; 115 | date->month = mon; 116 | date->day = mday; 117 | time->hours = hour; 118 | time->minutes = min; 119 | time->seconds = sec; 120 | } 121 | 122 | #define JULIAN_DATE_BASE 2440588 123 | // Convert Date to Counter 124 | kov_timestamp_t RTC_GetTimestampDateTime(const kov_date_t *date, 125 | const kov_time_t *time) { 126 | return RTC_GetTimestamp(date->year, date->month, date->day, 127 | time->hours, time->minutes, time->seconds); 128 | } 129 | /////////////////////////////////////////////////////// 130 | 131 | kov_timestamp_t RTC_GetTimestamp(int32_t year, 132 | int32_t month, 133 | int32_t day, 134 | int32_t hours, 135 | int32_t minutes, 136 | int32_t seconds) { 137 | uint8_t a; 138 | uint16_t y; 139 | uint8_t m; 140 | kov_timestamp_t JDN; 141 | 142 | a = (14 - month) / 12; 143 | y = year + 4800 - a; 144 | m = month + (12 * a) - 3; 145 | 146 | JDN = day; 147 | JDN += (153*m + 2) / 5; 148 | JDN += 365 * y; 149 | JDN += y / 4; 150 | JDN += -y / 100; 151 | JDN += y / 400; 152 | JDN = JDN - 32045; 153 | JDN = JDN - JULIAN_DATE_BASE; 154 | JDN *= 86400; 155 | 156 | JDN += hours * 3600; 157 | JDN += minutes * 60; 158 | JDN += seconds; 159 | 160 | return JDN; 161 | } 162 | /////////////////////////////////////////////////////// 163 | 164 | const char *RTC_TimeStr(const kov_time_t *t) { 165 | static char buff[9] = {0}; 166 | if (t->hours < 10) 167 | buff[0] = '0'; 168 | if (t->hours) 169 | uint32ToStr(buff, t->hours, 3); 170 | else 171 | buff[1] = '0'; 172 | 173 | buff[2] = ':'; 174 | if (t->minutes < 10) 175 | buff[3] = '0'; 176 | if (t->minutes) 177 | uint32ToStr(&buff[3], t->minutes, 3); 178 | else 179 | buff[4] = '0'; 180 | 181 | buff[5] = ':'; 182 | if (t->seconds < 10) 183 | buff[6] = '0'; 184 | if (t->seconds) 185 | uint32ToStr(&buff[6], t->seconds, 3); 186 | else 187 | buff[7] = '0'; 188 | return buff; 189 | } 190 | /////////////////////////////////////////////////////// 191 | 192 | static const char *months[] = {"jan", "feb", "mar", "apr", "may", "jun", "jul", "aug", "sep", "oct", "nov", "dec"}; 193 | const char *RTC_DateStr(const kov_date_t *d) { 194 | static char buff[12] = {0}; 195 | if (d->day < 10) 196 | buff[0] = '0'; 197 | uint32ToStr(buff, d->day, 3); 198 | buff[2] = ' '; 199 | memcpy((void*)&buff[3], months[d->month - 1], 3); 200 | buff[6] = ' '; 201 | uint32ToStr(&buff[7], d->year, 5); 202 | return buff; 203 | } 204 | /////////////////////////////////////////////////////// 205 | 206 | const char *RTC_DateTimeStr(const kov_date_t *d, 207 | const kov_time_t *t) { 208 | static char buff[13] = {0}; 209 | if (d->day < 10) 210 | buff[0] = '0'; 211 | uint32ToStr(buff, d->day, 3); 212 | buff[2] = ' '; 213 | memcpy((void*)&buff[3], months[d->month - 1], 3); 214 | buff[6] = ' '; 215 | 216 | if (t->hours < 10) 217 | buff[7] = '0'; 218 | if (t->hours) 219 | uint32ToStr(&buff[7], t->hours, 3); 220 | else 221 | buff[8] = '0'; 222 | 223 | buff[9] = ':'; 224 | if (t->minutes < 10) 225 | buff[10] = '0'; 226 | if (t->minutes) 227 | uint32ToStr(&buff[10], t->minutes, 3); 228 | else 229 | buff[11] = '0'; 230 | 231 | return buff; 232 | } 233 | /////////////////////////////////////////////////////// 234 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_cec.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_cec.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 CEC 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_CEC_H 25 | #define __STM32F10x_CEC_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 CEC 39 | * @{ 40 | */ 41 | 42 | 43 | /** @defgroup CEC_Exported_Types 44 | * @{ 45 | */ 46 | 47 | /** 48 | * @brief CEC Init structure definition 49 | */ 50 | typedef struct 51 | { 52 | uint16_t CEC_BitTimingMode; /*!< Configures the CEC Bit Timing Error Mode. 53 | This parameter can be a value of @ref CEC_BitTiming_Mode */ 54 | uint16_t CEC_BitPeriodMode; /*!< Configures the CEC Bit Period Error Mode. 55 | This parameter can be a value of @ref CEC_BitPeriod_Mode */ 56 | }CEC_InitTypeDef; 57 | 58 | /** 59 | * @} 60 | */ 61 | 62 | /** @defgroup CEC_Exported_Constants 63 | * @{ 64 | */ 65 | 66 | /** @defgroup CEC_BitTiming_Mode 67 | * @{ 68 | */ 69 | #define CEC_BitTimingStdMode ((uint16_t)0x00) /*!< Bit timing error Standard Mode */ 70 | #define CEC_BitTimingErrFreeMode CEC_CFGR_BTEM /*!< Bit timing error Free Mode */ 71 | 72 | #define IS_CEC_BIT_TIMING_ERROR_MODE(MODE) (((MODE) == CEC_BitTimingStdMode) || \ 73 | ((MODE) == CEC_BitTimingErrFreeMode)) 74 | /** 75 | * @} 76 | */ 77 | 78 | /** @defgroup CEC_BitPeriod_Mode 79 | * @{ 80 | */ 81 | #define CEC_BitPeriodStdMode ((uint16_t)0x00) /*!< Bit period error Standard Mode */ 82 | #define CEC_BitPeriodFlexibleMode CEC_CFGR_BPEM /*!< Bit period error Flexible Mode */ 83 | 84 | #define IS_CEC_BIT_PERIOD_ERROR_MODE(MODE) (((MODE) == CEC_BitPeriodStdMode) || \ 85 | ((MODE) == CEC_BitPeriodFlexibleMode)) 86 | /** 87 | * @} 88 | */ 89 | 90 | 91 | /** @defgroup CEC_interrupts_definition 92 | * @{ 93 | */ 94 | #define CEC_IT_TERR CEC_CSR_TERR 95 | #define CEC_IT_TBTRF CEC_CSR_TBTRF 96 | #define CEC_IT_RERR CEC_CSR_RERR 97 | #define CEC_IT_RBTF CEC_CSR_RBTF 98 | #define IS_CEC_GET_IT(IT) (((IT) == CEC_IT_TERR) || ((IT) == CEC_IT_TBTRF) || \ 99 | ((IT) == CEC_IT_RERR) || ((IT) == CEC_IT_RBTF)) 100 | /** 101 | * @} 102 | */ 103 | 104 | 105 | /** @defgroup CEC_Own_Address 106 | * @{ 107 | */ 108 | #define IS_CEC_ADDRESS(ADDRESS) ((ADDRESS) < 0x10) 109 | /** 110 | * @} 111 | */ 112 | 113 | /** @defgroup CEC_Prescaler 114 | * @{ 115 | */ 116 | #define IS_CEC_PRESCALER(PRESCALER) ((PRESCALER) <= 0x3FFF) 117 | 118 | /** 119 | * @} 120 | */ 121 | 122 | /** @defgroup CEC_flags_definition 123 | * @{ 124 | */ 125 | 126 | /** 127 | * @brief ESR register flags 128 | */ 129 | #define CEC_FLAG_BTE ((uint32_t)0x10010000) 130 | #define CEC_FLAG_BPE ((uint32_t)0x10020000) 131 | #define CEC_FLAG_RBTFE ((uint32_t)0x10040000) 132 | #define CEC_FLAG_SBE ((uint32_t)0x10080000) 133 | #define CEC_FLAG_ACKE ((uint32_t)0x10100000) 134 | #define CEC_FLAG_LINE ((uint32_t)0x10200000) 135 | #define CEC_FLAG_TBTFE ((uint32_t)0x10400000) 136 | 137 | /** 138 | * @brief CSR register flags 139 | */ 140 | #define CEC_FLAG_TEOM ((uint32_t)0x00000002) 141 | #define CEC_FLAG_TERR ((uint32_t)0x00000004) 142 | #define CEC_FLAG_TBTRF ((uint32_t)0x00000008) 143 | #define CEC_FLAG_RSOM ((uint32_t)0x00000010) 144 | #define CEC_FLAG_REOM ((uint32_t)0x00000020) 145 | #define CEC_FLAG_RERR ((uint32_t)0x00000040) 146 | #define CEC_FLAG_RBTF ((uint32_t)0x00000080) 147 | 148 | #define IS_CEC_CLEAR_FLAG(FLAG) ((((FLAG) & (uint32_t)0xFFFFFF03) == 0x00) && ((FLAG) != 0x00)) 149 | 150 | #define IS_CEC_GET_FLAG(FLAG) (((FLAG) == CEC_FLAG_BTE) || ((FLAG) == CEC_FLAG_BPE) || \ 151 | ((FLAG) == CEC_FLAG_RBTFE) || ((FLAG)== CEC_FLAG_SBE) || \ 152 | ((FLAG) == CEC_FLAG_ACKE) || ((FLAG) == CEC_FLAG_LINE) || \ 153 | ((FLAG) == CEC_FLAG_TBTFE) || ((FLAG) == CEC_FLAG_TEOM) || \ 154 | ((FLAG) == CEC_FLAG_TERR) || ((FLAG) == CEC_FLAG_TBTRF) || \ 155 | ((FLAG) == CEC_FLAG_RSOM) || ((FLAG) == CEC_FLAG_REOM) || \ 156 | ((FLAG) == CEC_FLAG_RERR) || ((FLAG) == CEC_FLAG_RBTF)) 157 | 158 | /** 159 | * @} 160 | */ 161 | 162 | /** 163 | * @} 164 | */ 165 | 166 | /** @defgroup CEC_Exported_Macros 167 | * @{ 168 | */ 169 | 170 | /** 171 | * @} 172 | */ 173 | 174 | /** @defgroup CEC_Exported_Functions 175 | * @{ 176 | */ 177 | void CEC_DeInit(void); 178 | void CEC_Init(CEC_InitTypeDef* CEC_InitStruct); 179 | void CEC_Cmd(FunctionalState NewState); 180 | void CEC_ITConfig(FunctionalState NewState); 181 | void CEC_OwnAddressConfig(uint8_t CEC_OwnAddress); 182 | void CEC_SetPrescaler(uint16_t CEC_Prescaler); 183 | void CEC_SendDataByte(uint8_t Data); 184 | uint8_t CEC_ReceiveDataByte(void); 185 | void CEC_StartOfMessage(void); 186 | void CEC_EndOfMessageCmd(FunctionalState NewState); 187 | FlagStatus CEC_GetFlagStatus(uint32_t CEC_FLAG); 188 | void CEC_ClearFlag(uint32_t CEC_FLAG); 189 | ITStatus CEC_GetITStatus(uint8_t CEC_IT); 190 | void CEC_ClearITPendingBit(uint16_t CEC_IT); 191 | 192 | #ifdef __cplusplus 193 | } 194 | #endif 195 | 196 | #endif /* __STM32F10x_CEC_H */ 197 | 198 | /** 199 | * @} 200 | */ 201 | 202 | /** 203 | * @} 204 | */ 205 | 206 | /** 207 | * @} 208 | */ 209 | 210 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 211 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /src/display/nokiaLcd/nokia5110.c: -------------------------------------------------------------------------------- 1 | #include "display/nokiaLcd/nokia5110.h" 2 | #include "display/nokiaLcd/nokia5110_font.h" 3 | #include "commons/commons.h" 4 | 5 | /////////////////////////////////////////////////////// 6 | 7 | static void nokia5110_gpio_config(void); 8 | static void nokia5110_spi_config(void); 9 | 10 | void nokia5110_init(void){ 11 | /* Configure gpio pins */ 12 | nokia5110_gpio_config(); 13 | /* Configure spi pins */ 14 | nokia5110_spi_config(); 15 | /* Set pin initial state */ 16 | nokia5110_light(false); 17 | 18 | NOKIA5110_DC_HIGH(); // Mode = command; 19 | NOKIA5110_DIN_HIGH(); // Set In at high level; 20 | NOKIA5110_CLK_HIGH(); // Set CLK high; 21 | NOKIA5110_CE_HIGH(); // Unselect chip; 22 | 23 | /* Reset the LCD to a known state */ 24 | NOKIA5110_RST_LOW(); // Set LCD reset = 0; 25 | for (int i = 0; i < 5000; i++); //WTF? we need to use something else. 26 | NOKIA5110_RST_HIGH(); // LCD_RST = 1 27 | 28 | /* Configure LCD module */ 29 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x21); // Extended instruction set selected 30 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0xb7); // Set LCD voltage (defined by experimentation...) 31 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x04); // Set temperature control (TC2) 32 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x14); // Set Bias for 1/48 33 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x20); // Revert to standard instruction set 34 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x0c); // Set display on in "normal" mode (not inversed) 35 | nokia5110_clear(); // Clear display (still off) 36 | } 37 | /////////////////////////////////////////////////////// 38 | 39 | void nokia5110_clear(void) { 40 | uint8_t y, x; 41 | nokia5110_gotoXY(0, 0); 42 | for (y = 0; y < DISPLAY_BANKS; y++) { 43 | for (x = 0; x < DISPLAY_WIDTH; x++) { 44 | nokia5110_spi_writeByte(PCD8544_MODE_Data, 0x00); 45 | } 46 | } 47 | nokia5110_gotoXY(0, 0); 48 | } 49 | /////////////////////////////////////////////////////// 50 | 51 | void nokia5110_gotoXY(int8_t col, int8_t row){ 52 | col *= DISPLAY_BANKS; 53 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x40 | (uint8_t)row); 54 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x80 | (uint8_t)col); 55 | } 56 | /////////////////////////////////////////////////////// 57 | 58 | void nokia5110_writeChar(char c){ 59 | uint8_t line; 60 | c -= 32; 61 | for (line = 0; line < DISPLAY_BANKS; line++) { 62 | nokia5110_spi_writeByte(PCD8544_MODE_Data, font6_8[(uint8_t)c][line]); 63 | } 64 | } 65 | /////////////////////////////////////////////////////// 66 | 67 | void nokia5110_writeString(const char *s){ 68 | while (*s) 69 | nokia5110_writeChar(*s++); 70 | } 71 | /////////////////////////////////////////////////////// 72 | 73 | void nokia5110_setContrast(uint8_t contrast){ 74 | /* LCD Extended Commands. */ 75 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x21); 76 | /* Set LCD Vop (Contrast). */ 77 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x80 | contrast); 78 | /* LCD Standard Commands, horizontal addressing mode. */ 79 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x20); 80 | } 81 | /////////////////////////////////////////////////////// 82 | 83 | void nokia5110_gpio_config(void){ 84 | GPIO_InitTypeDef GPIO_InitStructure; 85 | 86 | /* Enable Clocks for GPIO */ 87 | RCC_APB2PeriphClockCmd(PCD8544_GPIO_CLOCK, ENABLE); 88 | 89 | /* Configure LED Pin */ 90 | GPIO_InitStructure.GPIO_Pin = PCD8544_LED_PIN; 91 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 92 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 93 | GPIO_Init(PCD8544_LED_PORT, &GPIO_InitStructure); 94 | 95 | /* Configure RST Pin */ 96 | GPIO_InitStructure.GPIO_Pin = PCD8544_RST_PIN; 97 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 98 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 99 | GPIO_Init(PCD8544_RST_PORT, &GPIO_InitStructure); 100 | 101 | /* Configure CE Pin */ 102 | GPIO_InitStructure.GPIO_Pin = PCD8544_CE_PIN; 103 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 104 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 105 | GPIO_Init(PCD8544_CE_PORT, &GPIO_InitStructure); 106 | 107 | /* Configure DC Pin */ 108 | GPIO_InitStructure.GPIO_Pin = PCD8544_DC_PIN; 109 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz; 110 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 111 | GPIO_Init(PCD8544_DC_PORT, &GPIO_InitStructure); 112 | } 113 | /////////////////////////////////////////////////////// 114 | 115 | void nokia5110_spi_config(void){ 116 | GPIO_InitTypeDef GPIO_InitStructure; 117 | SPI_InitTypeDef SPI_InitStructure; 118 | 119 | /* Enable Clocks for GPIO and SPI2 */ 120 | RCC_APB2PeriphClockCmd(PCD8544_GPIO_CLOCK, ENABLE); 121 | RCC_APB2PeriphClockCmd(PCD8544_SPI_CLOCK, ENABLE); 122 | // TODO: Modify the structure for SPI1 123 | 124 | /* Configure GPIO Pins for SPI peripheral*/ 125 | GPIO_InitStructure.GPIO_Pin = PCD8544_CLK_PIN | PCD8544_MISO_PIN | PCD8544_MOSI_PIN; 126 | //GPIO_InitStructure.GPIO_Pin = PCD8544_CLK_PIN | PCD8544_DC_PIN | PCD8544_MOSI_PIN; 127 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; 128 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_PP; 129 | GPIO_Init(PCD8544_SPI_PORT, &GPIO_InitStructure); 130 | 131 | /* Configure SPI Pins */ 132 | SPI_InitStructure.SPI_Direction = SPI_Direction_1Line_Tx; 133 | SPI_InitStructure.SPI_Mode = SPI_Mode_Master; 134 | SPI_InitStructure.SPI_DataSize = SPI_DataSize_8b; 135 | SPI_InitStructure.SPI_CPOL = SPI_CPOL_High; 136 | SPI_InitStructure.SPI_CPHA = SPI_CPHA_2Edge; 137 | SPI_InitStructure.SPI_NSS = SPI_NSS_Soft; 138 | SPI_InitStructure.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_8; 139 | SPI_InitStructure.SPI_FirstBit = SPI_FirstBit_MSB; 140 | SPI_InitStructure.SPI_CRCPolynomial = 7; 141 | SPI_Init(PCD8544_SPI_PER, &SPI_InitStructure); 142 | 143 | /* Enable SPI Peripheral */ 144 | SPI_Cmd(PCD8544_SPI_PER, ENABLE); 145 | } 146 | /////////////////////////////////////////////////////// 147 | 148 | void nokia5110_spi_writeByte(PCD8544_MODE_TypeDef mode, uint8_t data){ 149 | NOKIA5110_CE_LOW(); // SPI_CS = 0; 150 | if(mode == PCD8544_MODE_Command) { 151 | NOKIA5110_DC_LOW(); 152 | } else { 153 | NOKIA5110_DC_HIGH(); 154 | } 155 | 156 | /* Loop while DR register in not empty */ 157 | while (SPI_I2S_GetFlagStatus(PCD8544_SPI_PER, SPI_I2S_FLAG_TXE) == RESET); 158 | /* Send a Byte through the SPI peripheral */ 159 | SPI_I2S_SendData(PCD8544_SPI_PER, data); 160 | /* Be sure that the character goes to the shift register */ 161 | while (SPI_I2S_GetFlagStatus(PCD8544_SPI_PER, SPI_I2S_FLAG_TXE) == RESET); 162 | /* Wait until entire byte has been read (which we discard anyway) */ 163 | // while( SPI_I2S_GetFlagStatus(PCD8544_SPI_PER, SPI_I2S_FLAG_RXNE) != SET ); 164 | 165 | NOKIA5110_CE_HIGH(); // SPI_CS = 1; 166 | } 167 | /////////////////////////////////////////////////////// 168 | 169 | void nokia5110_light(bool on) { 170 | if (on) NOKIA5110_LED_ON(); 171 | else NOKIA5110_LED_OFF(); 172 | } 173 | /////////////////////////////////////////////////////// 174 | 175 | void nokia5110_setPos(uint8_t page, uint8_t x) { 176 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x40 | (page & 0x07)); 177 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x80 | x); 178 | } 179 | /////////////////////////////////////////////////////// 180 | 181 | void nokia5110_printText(int16_t x, int16_t y, const char *value) { 182 | nokia5110_gotoXY((int8_t)x, (int8_t)y); 183 | nokia5110_writeString(value); 184 | } 185 | 186 | void nokia5110_setPixel(int16_t x, int16_t y) { 187 | uint8_t bank = (uint8_t)(y >> 3); 188 | uint8_t bit = (uint8_t)(y - (bank << 3)); 189 | uint8_t c = (uint8_t)(1 << bit); 190 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x40 | (uint8_t)bank); 191 | nokia5110_spi_writeByte(PCD8544_MODE_Command, 0x80 | (uint8_t)x); 192 | nokia5110_spi_writeByte(PCD8544_MODE_Data, c); 193 | } 194 | 195 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /StdPeriph_Driver/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 | -------------------------------------------------------------------------------- /src/magnetometer/lis3mdl/lis3mdl_i2c.c: -------------------------------------------------------------------------------- 1 | #include "magnetometer/lis3mdl/lis3mdl_i2c.h" 2 | #include 3 | #include 4 | #include "i2c1/i2c1.h" 5 | 6 | #define LIS3MDL_REG_WHO_AM_I 0x0F 7 | #define LIS3MDL_REG_CTL_1 0x20 8 | #define LIS3MDL_REG_CTL_2 0x21 9 | #define LIS3MDL_REG_CTL_3 0x22 10 | #define LIS3MDL_REG_CTL_4 0x23 11 | #define LIS3MDL_REG_STATUS 0x27 12 | #define LIS3MDL_REG_OUT_X_L 0x28 13 | #define LIS3MDL_REG_OUT_X_H 0x29 14 | #define LIS3MDL_REG_OUT_Y_L 0x2A 15 | #define LIS3MDL_REG_OUT_Y_H 0x2B 16 | #define LIS3MDL_REG_OUT_Z_L 0x2C 17 | #define LIS3MDL_REG_OUT_Z_H 0x2D 18 | #define LIS3MDL_REG_OUT_TEMP_L 0x2E 19 | #define LIS3MDL_REG_OUT_TEMP_H 0x2F 20 | 21 | #define LIS3MDL_REG_CTL_1_TEMP_EN 0x80 22 | #define LIS3MDL_REG_CTL_1_ODR_EN 0x02 23 | #define LIS3MDL_REG_CTL_2_RESET 0x04 24 | 25 | static const uint16_t LIS3MDLGAUSS_TO_SCALE[] = { 4, 8, 12, 16 }; 26 | 27 | static lis3mdl_err_t _LIS3MDL_init(lis3mdl_t* lis3mdl); 28 | static lis3mdl_err_t _LIS3MDL_readRegister(lis3mdl_t* lis3mdl, uint8_t reg, uint8_t* value); 29 | static lis3mdl_err_t _LIS3MDL_writeRegister(lis3mdl_t* lis3mdl, uint8_t reg, uint8_t data, uint8_t mask); 30 | static void _LIS3MDL_readRegister_int16(lis3mdl_t* lis3mdl, uint8_t lowAddr, uint8_t highAddr, int16_t* value); 31 | 32 | lis3mdl_err_t LIS3MDL_setup(lis3mdl_t* lis3mdl, 33 | uint8_t address) { 34 | lis3mdl->address = address; 35 | LIS3MDL_clearMinMax(lis3mdl); 36 | return _LIS3MDL_init(lis3mdl); 37 | } 38 | /////////////////////////////////////////////////////// 39 | 40 | void LIS3MDL_clearMinMax(lis3mdl_t* lis3mdl) { 41 | for (int axis = 0; axis < 3; axis++) { 42 | lis3mdl->min[axis] = INT16_MAX; 43 | lis3mdl->max[axis] = INT16_MIN; 44 | } 45 | } 46 | /////////////////////////////////////////////////////// 47 | 48 | void LIS3MDL_setMinMax(lis3mdl_t* lis3mdl, 49 | uint8_t axis, 50 | int16_t min, 51 | int16_t max) { 52 | lis3mdl->min[axis] = min; 53 | lis3mdl->max[axis] = -max; 54 | } 55 | /////////////////////////////////////////////////////// 56 | 57 | lis3mdl_err_t LIS3MDL_reset(lis3mdl_t* lis3mdl) { 58 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_2, LIS3MDL_REG_CTL_2_RESET, LIS3MDL_REG_CTL_2_RESET); 59 | return _LIS3MDL_init(lis3mdl); 60 | } 61 | /////////////////////////////////////////////////////// 62 | 63 | void LIS3MDL_enableTemperature(lis3mdl_t* lis3mdl, 64 | bool enable) { 65 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_1, enable, LIS3MDL_REG_CTL_1_TEMP_EN); 66 | } 67 | /////////////////////////////////////////////////////// 68 | 69 | void LIS3MDL_setPerformance(lis3mdl_t* lis3mdl, 70 | uint8_t performance) { 71 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_1, (uint8_t)(performance << 5), 0x60); // 0b01100000 72 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_4, (uint8_t)(performance << 2), 0x0c); // 0b00001100 73 | } 74 | /////////////////////////////////////////////////////// 75 | 76 | void LIS3MDL_setDataRate(lis3mdl_t* lis3mdl, 77 | uint8_t dataRate) { 78 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_1, (uint8_t)(dataRate << 2), 0x1c); // 0b00011100 79 | } 80 | /////////////////////////////////////////////////////// 81 | 82 | void LIS3MDL_setMode(lis3mdl_t* lis3mdl, 83 | uint8_t mode) { 84 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_3, (uint8_t)(mode << 0), 0x03); // 0b00000011 85 | } 86 | /////////////////////////////////////////////////////// 87 | 88 | void LIS3MDL_setScale(lis3mdl_t* lis3mdl, 89 | uint8_t scale) { 90 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_2, (uint8_t)(scale << 5), 0x60); // 0b01100000 91 | lis3mdl->scale = LIS3MDLGAUSS_TO_SCALE[scale]; 92 | } 93 | /////////////////////////////////////////////////////// 94 | 95 | void LIS3MDL_readAxis(lis3mdl_t* lis3mdl, 96 | uint8_t axis, 97 | int16_t* value) { 98 | uint8_t lowAddr, highAddr; 99 | lowAddr = highAddr = 0; 100 | switch (axis) { 101 | case LIS3MDL_AXIS_X: 102 | lowAddr = LIS3MDL_REG_OUT_X_L; 103 | highAddr = LIS3MDL_REG_OUT_X_H; 104 | break; 105 | case LIS3MDL_AXIS_Y: 106 | lowAddr = LIS3MDL_REG_OUT_Y_L; 107 | highAddr = LIS3MDL_REG_OUT_Y_H; 108 | break; 109 | case LIS3MDL_AXIS_Z: 110 | lowAddr = LIS3MDL_REG_OUT_Z_L; 111 | highAddr = LIS3MDL_REG_OUT_Z_H; 112 | break; 113 | } 114 | _LIS3MDL_readRegister_int16(lis3mdl, lowAddr, highAddr, value); 115 | } 116 | /////////////////////////////////////////////////////// 117 | 118 | void LIS3MDL_readTemperature(lis3mdl_t* lis3mdl, 119 | int16_t* value) { 120 | _LIS3MDL_readRegister_int16(lis3mdl, LIS3MDL_REG_OUT_TEMP_L, LIS3MDL_REG_OUT_TEMP_H, value); 121 | } 122 | /////////////////////////////////////////////////////// 123 | 124 | lis3mdl_err_t LIS3MDL_readDeviceId(lis3mdl_t* lis3mdl, 125 | uint8_t* deviceId) { 126 | return _LIS3MDL_readRegister(lis3mdl, LIS3MDL_REG_WHO_AM_I, deviceId); 127 | } 128 | /////////////////////////////////////////////////////// 129 | 130 | void LIS3MDL_readStatus(lis3mdl_t* lis3mdl, 131 | uint8_t* status) { 132 | _LIS3MDL_readRegister(lis3mdl, LIS3MDL_REG_STATUS, status); 133 | } 134 | /////////////////////////////////////////////////////// 135 | 136 | lis3mdl_err_t _LIS3MDL_init(lis3mdl_t* lis3mdl) { 137 | uint8_t deviceId = 0; 138 | uint8_t ctl2 = 0; 139 | lis3mdl_err_t err = LIS3MDL_readDeviceId(lis3mdl, &deviceId); 140 | 141 | if (err) 142 | return err; 143 | 144 | if (deviceId != LIS3MDL_DEVICE_ID) 145 | return 1; 146 | 147 | _LIS3MDL_readRegister(lis3mdl, LIS3MDL_REG_CTL_2, &ctl2); 148 | lis3mdl->scale = LIS3MDLGAUSS_TO_SCALE[(ctl2 >> 5) & 0x03]; 149 | return 0; 150 | } 151 | /////////////////////////////////////////////////////// 152 | 153 | void _LIS3MDL_readRegister_int16(lis3mdl_t* lis3mdl, 154 | uint8_t lowAddr, 155 | uint8_t highAddr, 156 | int16_t* value) { 157 | uint8_t low, high; 158 | _LIS3MDL_readRegister(lis3mdl, lowAddr, &low); 159 | _LIS3MDL_readRegister(lis3mdl, highAddr, &high); 160 | *value = (int16_t)(((uint16_t)high) << 8) | (uint16_t)low; //here parses two's complement because of ARM arch 161 | } 162 | /////////////////////////////////////////////////////// 163 | 164 | lis3mdl_err_t _LIS3MDL_readRegister(lis3mdl_t* lis3mdl, 165 | uint8_t reg, 166 | uint8_t* value) { 167 | I2C1FinishCode fc = i2c1_read_buff_sync(lis3mdl->address, reg, value, 1); 168 | return (lis3mdl_err_t)fc; 169 | } 170 | /////////////////////////////////////////////////////// 171 | 172 | lis3mdl_err_t _LIS3MDL_writeRegister(lis3mdl_t* lis3mdl, 173 | uint8_t reg, 174 | uint8_t data, 175 | uint8_t mask) { 176 | uint8_t valueToWrite, currentValue; 177 | _LIS3MDL_readRegister(lis3mdl, reg, ¤tValue); 178 | valueToWrite = (currentValue & ~mask) | (data & mask); 179 | I2C1FinishCode fc = i2c1_write_buff_sync(lis3mdl->address, reg, &valueToWrite, 1); 180 | return (lis3mdl_err_t) fc; 181 | } 182 | /////////////////////////////////////////////////////// 183 | 184 | void LIS3MDL_read3AxisSync(lis3mdl_t *lis3mdl, 185 | int16_t value[3]) { 186 | LIS3MDL_readAxis(lis3mdl, LIS3MDL_AXIS_X, &value[0]); 187 | LIS3MDL_readAxis(lis3mdl, LIS3MDL_AXIS_Y, &value[1]); 188 | LIS3MDL_readAxis(lis3mdl, LIS3MDL_AXIS_Z, &value[2]); 189 | } 190 | /////////////////////////////////////////////////////// 191 | 192 | void LIS3MDL_enableODR(lis3mdl_t *lis3mdl, bool enable) { 193 | _LIS3MDL_writeRegister(lis3mdl, LIS3MDL_REG_CTL_1, enable, LIS3MDL_REG_CTL_1_ODR_EN); 194 | } 195 | /////////////////////////////////////////////////////// 196 | 197 | float LIS3MDL_currentGain(const lis3mdl_t *lis3mdl) { 198 | switch (lis3mdl->scale) { 199 | case 4: return 6842.0f; 200 | case 8: return 3421.0f; 201 | case 12: return 2281.0f; 202 | case 16: return 1711.0f; 203 | } 204 | return 1.0f; 205 | } 206 | -------------------------------------------------------------------------------- /StdPeriph_Driver/inc/stm32f10x_bkp.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f10x_bkp.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 BKP 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_BKP_H 25 | #define __STM32F10x_BKP_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 BKP 39 | * @{ 40 | */ 41 | 42 | /** @defgroup BKP_Exported_Types 43 | * @{ 44 | */ 45 | 46 | /** 47 | * @} 48 | */ 49 | 50 | /** @defgroup BKP_Exported_Constants 51 | * @{ 52 | */ 53 | 54 | /** @defgroup Tamper_Pin_active_level 55 | * @{ 56 | */ 57 | 58 | #define BKP_TamperPinLevel_High ((uint16_t)0x0000) 59 | #define BKP_TamperPinLevel_Low ((uint16_t)0x0001) 60 | #define IS_BKP_TAMPER_PIN_LEVEL(LEVEL) (((LEVEL) == BKP_TamperPinLevel_High) || \ 61 | ((LEVEL) == BKP_TamperPinLevel_Low)) 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @defgroup RTC_output_source_to_output_on_the_Tamper_pin 67 | * @{ 68 | */ 69 | 70 | #define BKP_RTCOutputSource_None ((uint16_t)0x0000) 71 | #define BKP_RTCOutputSource_CalibClock ((uint16_t)0x0080) 72 | #define BKP_RTCOutputSource_Alarm ((uint16_t)0x0100) 73 | #define BKP_RTCOutputSource_Second ((uint16_t)0x0300) 74 | #define IS_BKP_RTC_OUTPUT_SOURCE(SOURCE) (((SOURCE) == BKP_RTCOutputSource_None) || \ 75 | ((SOURCE) == BKP_RTCOutputSource_CalibClock) || \ 76 | ((SOURCE) == BKP_RTCOutputSource_Alarm) || \ 77 | ((SOURCE) == BKP_RTCOutputSource_Second)) 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @defgroup Data_Backup_Register 83 | * @{ 84 | */ 85 | 86 | #define BKP_DR1 ((uint16_t)0x0004) 87 | #define BKP_DR2 ((uint16_t)0x0008) 88 | #define BKP_DR3 ((uint16_t)0x000C) 89 | #define BKP_DR4 ((uint16_t)0x0010) 90 | #define BKP_DR5 ((uint16_t)0x0014) 91 | #define BKP_DR6 ((uint16_t)0x0018) 92 | #define BKP_DR7 ((uint16_t)0x001C) 93 | #define BKP_DR8 ((uint16_t)0x0020) 94 | #define BKP_DR9 ((uint16_t)0x0024) 95 | #define BKP_DR10 ((uint16_t)0x0028) 96 | #define BKP_DR11 ((uint16_t)0x0040) 97 | #define BKP_DR12 ((uint16_t)0x0044) 98 | #define BKP_DR13 ((uint16_t)0x0048) 99 | #define BKP_DR14 ((uint16_t)0x004C) 100 | #define BKP_DR15 ((uint16_t)0x0050) 101 | #define BKP_DR16 ((uint16_t)0x0054) 102 | #define BKP_DR17 ((uint16_t)0x0058) 103 | #define BKP_DR18 ((uint16_t)0x005C) 104 | #define BKP_DR19 ((uint16_t)0x0060) 105 | #define BKP_DR20 ((uint16_t)0x0064) 106 | #define BKP_DR21 ((uint16_t)0x0068) 107 | #define BKP_DR22 ((uint16_t)0x006C) 108 | #define BKP_DR23 ((uint16_t)0x0070) 109 | #define BKP_DR24 ((uint16_t)0x0074) 110 | #define BKP_DR25 ((uint16_t)0x0078) 111 | #define BKP_DR26 ((uint16_t)0x007C) 112 | #define BKP_DR27 ((uint16_t)0x0080) 113 | #define BKP_DR28 ((uint16_t)0x0084) 114 | #define BKP_DR29 ((uint16_t)0x0088) 115 | #define BKP_DR30 ((uint16_t)0x008C) 116 | #define BKP_DR31 ((uint16_t)0x0090) 117 | #define BKP_DR32 ((uint16_t)0x0094) 118 | #define BKP_DR33 ((uint16_t)0x0098) 119 | #define BKP_DR34 ((uint16_t)0x009C) 120 | #define BKP_DR35 ((uint16_t)0x00A0) 121 | #define BKP_DR36 ((uint16_t)0x00A4) 122 | #define BKP_DR37 ((uint16_t)0x00A8) 123 | #define BKP_DR38 ((uint16_t)0x00AC) 124 | #define BKP_DR39 ((uint16_t)0x00B0) 125 | #define BKP_DR40 ((uint16_t)0x00B4) 126 | #define BKP_DR41 ((uint16_t)0x00B8) 127 | #define BKP_DR42 ((uint16_t)0x00BC) 128 | 129 | #define IS_BKP_DR(DR) (((DR) == BKP_DR1) || ((DR) == BKP_DR2) || ((DR) == BKP_DR3) || \ 130 | ((DR) == BKP_DR4) || ((DR) == BKP_DR5) || ((DR) == BKP_DR6) || \ 131 | ((DR) == BKP_DR7) || ((DR) == BKP_DR8) || ((DR) == BKP_DR9) || \ 132 | ((DR) == BKP_DR10) || ((DR) == BKP_DR11) || ((DR) == BKP_DR12) || \ 133 | ((DR) == BKP_DR13) || ((DR) == BKP_DR14) || ((DR) == BKP_DR15) || \ 134 | ((DR) == BKP_DR16) || ((DR) == BKP_DR17) || ((DR) == BKP_DR18) || \ 135 | ((DR) == BKP_DR19) || ((DR) == BKP_DR20) || ((DR) == BKP_DR21) || \ 136 | ((DR) == BKP_DR22) || ((DR) == BKP_DR23) || ((DR) == BKP_DR24) || \ 137 | ((DR) == BKP_DR25) || ((DR) == BKP_DR26) || ((DR) == BKP_DR27) || \ 138 | ((DR) == BKP_DR28) || ((DR) == BKP_DR29) || ((DR) == BKP_DR30) || \ 139 | ((DR) == BKP_DR31) || ((DR) == BKP_DR32) || ((DR) == BKP_DR33) || \ 140 | ((DR) == BKP_DR34) || ((DR) == BKP_DR35) || ((DR) == BKP_DR36) || \ 141 | ((DR) == BKP_DR37) || ((DR) == BKP_DR38) || ((DR) == BKP_DR39) || \ 142 | ((DR) == BKP_DR40) || ((DR) == BKP_DR41) || ((DR) == BKP_DR42)) 143 | 144 | #define IS_BKP_CALIBRATION_VALUE(VALUE) ((VALUE) <= 0x7F) 145 | /** 146 | * @} 147 | */ 148 | 149 | /** 150 | * @} 151 | */ 152 | 153 | /** @defgroup BKP_Exported_Macros 154 | * @{ 155 | */ 156 | 157 | /** 158 | * @} 159 | */ 160 | 161 | /** @defgroup BKP_Exported_Functions 162 | * @{ 163 | */ 164 | 165 | void BKP_DeInit(void); 166 | void BKP_TamperPinLevelConfig(uint16_t BKP_TamperPinLevel); 167 | void BKP_TamperPinCmd(FunctionalState NewState); 168 | void BKP_ITConfig(FunctionalState NewState); 169 | void BKP_RTCOutputConfig(uint16_t BKP_RTCOutputSource); 170 | void BKP_SetRTCCalibrationValue(uint8_t CalibrationValue); 171 | void BKP_WriteBackupRegister(uint16_t BKP_DR, uint16_t Data); 172 | uint16_t BKP_ReadBackupRegister(uint16_t BKP_DR); 173 | FlagStatus BKP_GetFlagStatus(void); 174 | void BKP_ClearFlag(void); 175 | ITStatus BKP_GetITStatus(void); 176 | void BKP_ClearITPendingBit(void); 177 | 178 | #ifdef __cplusplus 179 | } 180 | #endif 181 | 182 | #endif /* __STM32F10x_BKP_H */ 183 | /** 184 | * @} 185 | */ 186 | 187 | /** 188 | * @} 189 | */ 190 | 191 | /** 192 | * @} 193 | */ 194 | 195 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 196 | --------------------------------------------------------------------------------