From 0f390b743634ef4cdda03da8cb3f175524d59bd0 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Sun, 3 Jul 2011 15:34:29 +0200 Subject: Removed old stuff. --- .gitmodules | 3 - IMU.cpp | 92 ----------- IMU.h | 26 --- SConstruct | 14 +- ak8975.h | 24 --- bma150.h | 24 --- board.c | 18 -- board.h | 96 ----------- ch.ld | 114 ------------- chconf.h | 508 --------------------------------------------------------- chibios | 1 - foo.h | 84 ---------- halconf.h | 283 -------------------------------- i2csensor.cpp | 77 --------- i2csensor.h | 26 --- itg3200.h | 24 --- main.cpp | 256 ----------------------------- mcuconf.h | 170 ------------------- motormixer.cpp | 86 ---------- motormixer.h | 12 -- ppmsum.cpp | 39 ----- ppmsum.h | 13 -- thread.h | 23 --- usbserial.cpp | 281 ------------------------------- usbserial.h | 38 ----- 25 files changed, 2 insertions(+), 2330 deletions(-) delete mode 100644 .gitmodules delete mode 100755 IMU.cpp delete mode 100755 IMU.h delete mode 100644 ak8975.h delete mode 100644 bma150.h delete mode 100644 board.c delete mode 100644 board.h delete mode 100644 ch.ld delete mode 100644 chconf.h delete mode 160000 chibios delete mode 100644 foo.h delete mode 100644 halconf.h delete mode 100644 i2csensor.cpp delete mode 100644 i2csensor.h delete mode 100644 itg3200.h delete mode 100644 mcuconf.h delete mode 100644 motormixer.cpp delete mode 100644 motormixer.h delete mode 100644 ppmsum.cpp delete mode 100644 ppmsum.h delete mode 100644 thread.h delete mode 100644 usbserial.cpp delete mode 100644 usbserial.h diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index de8b785..0000000 --- a/.gitmodules +++ /dev/null @@ -1,3 +0,0 @@ -[submodule "chibios"] - path = chibios - url = git://git.jvnv.net/chibios diff --git a/IMU.cpp b/IMU.cpp deleted file mode 100755 index bcff537..0000000 --- a/IMU.cpp +++ /dev/null @@ -1,92 +0,0 @@ -//===================================================================================================== -// IMU.c -// S.O.H. Madgwick -// 25th September 2010 -//===================================================================================================== -// Description: -// -// Quaternion implementation of the 'DCM filter' [Mayhony et al]. -// -// User must define 'halfT' as the (sample period / 2), and the filter gains 'Kp' and 'Ki'. -// -// Global variables 'q0', 'q1', 'q2', 'q3' are the quaternion elements representing the estimated -// orientation. See my report for an overview of the use of quaternions in this application. -// -// User must call 'IMUupdate()' every sample period and parse calibrated gyroscope ('gx', 'gy', 'gz') -// and accelerometer ('ax', 'ay', 'ay') data. Gyroscope units are radians/second, accelerometer -// units are irrelevant as the vector is normalised. -// -//===================================================================================================== - -//---------------------------------------------------------------------------------------------------- -// Header files - -#include "IMU.h" -#include -#include - -//---------------------------------------------------------------------------------------------------- -// Definitions - -#define Kp 2.0f // proportional gain governs rate of convergence to accelerometer/magnetometer -#define Ki 0.005f // integral gain governs rate of convergence of gyroscope biases -#define halfT (0.5f / 100) // half the sample period - -//--------------------------------------------------------------------------------------------------- -// Variable definitions - -float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation -float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error - -//==================================================================================================== -// Function -//==================================================================================================== - -void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az) { - float norm; - float vx, vy, vz; - float ex, ey, ez; - - // normalise the measurements - norm = sqrt(ax*ax + ay*ay + az*az); - ax = ax / norm; - ay = ay / norm; - az = az / norm; - - // estimated direction of gravity - vx = 2*(q1*q3 - q0*q2); - vy = 2*(q0*q1 + q2*q3); - vz = q0*q0 - q1*q1 - q2*q2 + q3*q3; - - // error is sum of cross product between reference direction of field and direction measured by sensor - ex = (ay*vz - az*vy); - ey = (az*vx - ax*vz); - ez = (ax*vy - ay*vx); - - // integral error scaled integral gain - exInt = exInt + ex*Ki; - eyInt = eyInt + ey*Ki; - ezInt = ezInt + ez*Ki; - - // adjusted gyroscope measurements - gx = gx + Kp*ex + exInt; - gy = gy + Kp*ey + eyInt; - gz = gz + Kp*ez + ezInt; - - // integrate quaternion rate and normalise - q0 = q0 + (-q1*gx - q2*gy - q3*gz)*halfT; - q1 = q1 + (q0*gx + q2*gz - q3*gy)*halfT; - q2 = q2 + (q0*gy - q1*gz + q3*gx)*halfT; - q3 = q3 + (q0*gz + q1*gy - q2*gx)*halfT; - - // normalise quaternion - norm = sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3); - q0 = q0 / norm; - q1 = q1 / norm; - q2 = q2 / norm; - q3 = q3 / norm; -} - -//==================================================================================================== -// END OF CODE -//==================================================================================================== diff --git a/IMU.h b/IMU.h deleted file mode 100755 index 003488a..0000000 --- a/IMU.h +++ /dev/null @@ -1,26 +0,0 @@ -//===================================================================================================== -// IMU.h -// S.O.H. Madgwick -// 25th September 2010 -//===================================================================================================== -// -// See IMU.c file for description. -// -//===================================================================================================== -#ifndef IMU_h -#define IMU_h - -//---------------------------------------------------------------------------------------------------- -// Variable declaration - -extern float q0, q1, q2, q3; // quaternion elements representing the estimated orientation - -//--------------------------------------------------------------------------------------------------- -// Function declaration - -void IMUupdate(float gx, float gy, float gz, float ax, float ay, float az); - -#endif -//===================================================================================================== -// End of file -//===================================================================================================== \ No newline at end of file diff --git a/SConstruct b/SConstruct index 6d3566a..08229f4 100644 --- a/SConstruct +++ b/SConstruct @@ -18,23 +18,13 @@ env = Environment( AR = 'arm-none-eabi-ar', RANLIB = 'arm-none-eabi-ranlib', - CPPPATH = ['.', - 'chibios/os/kernel/include', 'chibios/os/ports/GCC/ARMCMx', 'chibios/os/ports/GCC/ARMCMx/STM32', - 'chibios/os/hal/include', 'chibios/os/hal/platforms/STM32', - ], + #CPPPATH = [], #LIBPATH = [], LIBS = ['m'], ) -sources = \ - Glob('chibios/os/kernel/src/*.c') + \ - ['chibios/os/ports/GCC/ARMCMx/' + i for i in ('STM32/vectors.c', 'chcore.c', 'chcore_v7m.c', 'nvic.c', 'crt0_v7m.s')] + \ - Glob('chibios/os/hal/src/*.c') + \ - Glob('chibios/os/hal/platforms/STM32/*.c') - -#firmware = env.Program('suzumebachi.elf', Glob('*.cpp') + Glob('*.c') + sources) -firmware = env.Program('suzumebachi.elf', ['main.cpp', 'entry.cpp', 'interrupt.cpp', 'rcc.cpp']) +firmware = env.Program('suzumebachi.elf', Glob('*.cpp')) env.Depends(firmware, 'suzumebachi.ld') env.Command('prog', ['suzumebachi.elf'], 'openocd -f openocd.cfg -c flash_chip') diff --git a/ak8975.h b/ak8975.h deleted file mode 100644 index beb9940..0000000 --- a/ak8975.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef AK8975_H -#define AK8975_H - -#include "i2csensor.h" - -class AK8975 : public I2CSensor { - public: - int16_t x, y, z; - - void init() { - i2c_address = 0x0c; - write(0x0a, 0x01); // Start first measurement. - } - - void update() { - read(0x03, 6); - x = (rxdata[0] | rxdata[1] << 8); - y = (rxdata[2] | rxdata[3] << 8); - z = (rxdata[4] | rxdata[5] << 8); - write(0x0a, 0x01); // Start a new measurement. - } -}; - -#endif diff --git a/bma150.h b/bma150.h deleted file mode 100644 index 0cd6b27..0000000 --- a/bma150.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef BMA150_H -#define BMA150_H - -#include "i2csensor.h" - -class BMA150 : public I2CSensor { - public: - int16_t x, y, z; - - void init() { - i2c_address = 0x38; - read(0x14, 1); - write(0x14, (rxdata[0] & 0xe0) | 0x00 | 0x00); // 2g range, 25 Hz bandwidth - } - - void update() { - read(0x02, 6); - x = ((rxdata[0] & 0xc0) | rxdata[1] << 8); - y = ((rxdata[2] & 0xc0) | rxdata[3] << 8); - z = ((rxdata[4] & 0xc0) | rxdata[5] << 8); - } -}; - -#endif diff --git a/board.c b/board.c deleted file mode 100644 index f4b836c..0000000 --- a/board.c +++ /dev/null @@ -1,18 +0,0 @@ -#include -#include - -const PALConfig pal_default_config = { - {VAL_GPIOAODR, VAL_GPIOACRL, VAL_GPIOACRH}, - {VAL_GPIOBODR, VAL_GPIOBCRL, VAL_GPIOBCRH}, - {VAL_GPIOCODR, VAL_GPIOCCRL, VAL_GPIOCCRH}, - {VAL_GPIODODR, VAL_GPIODCRL, VAL_GPIODCRH}, - {VAL_GPIOEODR, VAL_GPIOECRL, VAL_GPIOECRH}, -}; - -void __early_init(void) { - stm32_clock_init(); -} - -void boardInit(void) { - -} diff --git a/board.h b/board.h deleted file mode 100644 index f273db2..0000000 --- a/board.h +++ /dev/null @@ -1,96 +0,0 @@ -#ifndef BOARD_H -#define BOARD_H - -#define STM32_HSECLK 8000000 - -#define STM32F10X_MD - -/* - * I/O ports initial setup, this configuration is established soon after reset - * in the initialization code. - * - * The digits have the following meaning: - * 0 - Analog input. - * 1 - Push Pull output 10MHz. - * 2 - Push Pull output 2MHz. - * 3 - Push Pull output 50MHz. - * 4 - Digital input. - * 5 - Open Drain output 10MHz. - * 6 - Open Drain output 2MHz. - * 7 - Open Drain output 50MHz. - * 8 - Digital input with PullUp or PullDown resistor depending on ODR. - * 9 - Alternate Push Pull output 10MHz. - * A - Alternate Push Pull output 2MHz. - * B - Alternate Push Pull output 50MHz. - * C - Reserved. - * D - Alternate Open Drain output 10MHz. - * E - Alternate Open Drain output 2MHz. - * F - Alternate Open Drain output 50MHz. - * Please refer to the STM32 Reference Manual for details. - */ - -/* - * Port A setup. - * Everything input with pull-up except: - * PA0 - Normal input (BUTTON). - * PA2 - Alternate output (USART2 TX). - * PA3 - Normal input (USART2 RX). - */ -#define VAL_GPIOACRL 0x88384B88 /* PA7...PA0 */ -#define VAL_GPIOACRH 0x888884B8 /* PA15...PA8 */ -#define VAL_GPIOAODR 0xFFFFFFFF - -/* - * Port B setup. - * Everything input with pull-up except: - * PB13 - Alternate output (MMC SPI2 SCK). - * PB14 - Normal input (MMC SPI2 MISO). - * PB15 - Alternate output (MMC SPI2 MOSI). - */ -#define VAL_GPIOBCRL 0x84888888 /* PB7...PB0 */ -#define VAL_GPIOBCRH 0xB4B84488 /* PB15...PB8 */ -#define VAL_GPIOBODR 0xFFFFFFFF - -/* - * Port C setup. - * Everything input with pull-up except: - * PC4 - Normal input because there is an external resistor. - * PC6 - Normal input because there is an external resistor. - * PC7 - Normal input because there is an external resistor. - * PC10 - Push Pull output (CAN CNTRL). - * PC11 - Push Pull output (USB DISC). - * PC12 - Push Pull output (LED). - */ -#define VAL_GPIOCCRL 0x44008888 /* PC7...PC0 */ -#define VAL_GPIOCCRH 0x88833388 /* PC15...PC8 */ -#define VAL_GPIOCODR 0xFFFFFFFF - -/* - * Port D setup. - * Everything input with pull-up except: - * PD0 - Normal input (XTAL). - * PD1 - Normal input (XTAL). - */ -#define VAL_GPIODCRL 0x88888844 /* PD7...PD0 */ -#define VAL_GPIODCRH 0x88888888 /* PD15...PD8 */ -#define VAL_GPIODODR 0xFFFFFFFF - -/* - * Port E setup. - * Everything input with pull-up except: - */ -#define VAL_GPIOECRL 0x88888888 /* PE7...PE0 */ -#define VAL_GPIOECRH 0x88888888 /* PE15...PE8 */ -#define VAL_GPIOEODR 0xFFFFFFFF - -#if !defined(_FROM_ASM_) -#ifdef __cplusplus -extern "C" { -#endif - void boardInit(void); -#ifdef __cplusplus -} -#endif -#endif - -#endif diff --git a/ch.ld b/ch.ld deleted file mode 100644 index ae79ddd..0000000 --- a/ch.ld +++ /dev/null @@ -1,114 +0,0 @@ -/* - ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, - 2011 Giovanni Di Sirio. - - This file is part of ChibiOS/RT. - - ChibiOS/RT is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 3 of the License, or - (at your option) any later version. - - ChibiOS/RT is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/* - * ST32F103 memory setup. - */ -__main_stack_size__ = 0x0400; -__process_stack_size__ = 0x0400; -__stacks_total_size__ = __main_stack_size__ + __process_stack_size__; - -MEMORY -{ - flash : org = 0x08000000, len = 128k - ram : org = 0x20000000, len = 20k -} - -__ram_start__ = ORIGIN(ram); -__ram_size__ = LENGTH(ram); -__ram_end__ = __ram_start__ + __ram_size__; - -SECTIONS -{ - . = 0; - - .text : ALIGN(16) SUBALIGN(16) - { - _text = .; - KEEP(*(vectors)) - *(.text) - *(.text.*) - *(.rodata) - *(.rodata.*) - *(.glue_7t) - *(.glue_7) - *(.gcc*) - } > flash - - .ctors : - { - PROVIDE(_ctors_start_ = .); - KEEP(*(SORT(.ctors.*))) - KEEP(*(.ctors)) - PROVIDE(_ctors_end_ = .); - } > flash - - .dtors : - { - PROVIDE(_dtors_start_ = .); - KEEP(*(SORT(.dtors.*))) - KEEP(*(.dtors)) - PROVIDE(_dtors_end_ = .); - } > flash - - .ARM.extab : {*(.ARM.extab* .gnu.linkonce.armextab.*)} - - __exidx_start = .; - .ARM.exidx : {*(.ARM.exidx* .gnu.linkonce.armexidx.*)} > flash - __exidx_end = .; - - .eh_frame_hdr : {*(.eh_frame_hdr)} - - .eh_frame : ONLY_IF_RO {*(.eh_frame)} - - . = ALIGN(4); - _etext = .; - _textdata = _etext; - - .data : - { - _data = .; - *(.data) - . = ALIGN(4); - *(.data.*) - . = ALIGN(4); - *(.ramtext) - . = ALIGN(4); - _edata = .; - } > ram AT > flash - - .bss : - { - _bss_start = .; - *(.bss) - . = ALIGN(4); - *(.bss.*) - . = ALIGN(4); - *(COMMON) - . = ALIGN(4); - _bss_end = .; - } > ram -} - -PROVIDE(end = .); -_end = .; - -__heap_base__ = _end; -__heap_end__ = __ram_end__ - __stacks_total_size__; diff --git a/chconf.h b/chconf.h deleted file mode 100644 index dcb2493..0000000 --- a/chconf.h +++ /dev/null @@ -1,508 +0,0 @@ -/* - ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, - 2011 Giovanni Di Sirio. - - This file is part of ChibiOS/RT. - - ChibiOS/RT is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 3 of the License, or - (at your option) any later version. - - ChibiOS/RT is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/** - * @file templates/chconf.h - * @brief Configuration file template. - * @details A copy of this file must be placed in each project directory, it - * contains the application specific kernel settings. - * - * @addtogroup config - * @details Kernel related settings and hooks. - * @{ - */ - -#ifndef _CHCONF_H_ -#define _CHCONF_H_ - -/*===========================================================================*/ -/* Kernel parameters. */ -/*===========================================================================*/ - -/** - * @brief System tick frequency. - * @details Frequency of the system timer that drives the system ticks. This - * setting also defines the system tick time unit. - */ -#if !defined(CH_FREQUENCY) || defined(__DOXYGEN__) -#define CH_FREQUENCY 1000 -#endif - -/** - * @brief Round robin interval. - * @details This constant is the number of system ticks allowed for the - * threads before preemption occurs. Setting this value to zero - * disables the preemption for threads with equal priority and the - * round robin becomes cooperative. Note that higher priority - * threads can still preempt, the kernel is always preemptive. - * - * @note Disabling the round robin preemption makes the kernel more compact - * and generally faster. - */ -#if !defined(CH_TIME_QUANTUM) || defined(__DOXYGEN__) -#define CH_TIME_QUANTUM 20 -#endif - -/** - * @brief Nested locks. - * @details If enabled then the use of nested @p chSysLock() / @p chSysUnlock() - * operations is allowed.
- * For performance and code size reasons the recommended setting - * is to leave this option disabled.
- * You may use this option if you need to merge ChibiOS/RT with - * external libraries that require nested lock/unlock operations. - * - * @note The default is @p FALSE. - */ -#if !defined(CH_USE_NESTED_LOCKS) || defined(__DOXYGEN__) -#define CH_USE_NESTED_LOCKS FALSE -#endif - -/** - * @brief Managed RAM size. - * @details Size of the RAM area to be managed by the OS. If set to zero - * then the whole available RAM is used. The core memory is made - * available to the heap allocator and/or can be used directly through - * the simplified core memory allocator. - * - * @note In order to let the OS manage the whole RAM the linker script must - * provide the @p __heap_base__ and @p __heap_end__ symbols. - * @note Requires @p CH_USE_COREMEM. - */ -#if !defined(CH_MEMCORE_SIZE) || defined(__DOXYGEN__) -#define CH_MEMCORE_SIZE 0 -#endif - -/*===========================================================================*/ -/* Performance options. */ -/*===========================================================================*/ - -/** - * @brief OS optimization. - * @details If enabled then time efficient rather than space efficient code - * is used when two possible implementations exist. - * - * @note This is not related to the compiler optimization options. - * @note The default is @p TRUE. - */ -#if !defined(CH_OPTIMIZE_SPEED) || defined(__DOXYGEN__) -#define CH_OPTIMIZE_SPEED TRUE -#endif - -/** - * @brief Exotic optimization. - * @details If defined then a CPU register is used as storage for the global - * @p currp variable. Caching this variable in a register greatly - * improves both space and time OS efficiency. A side effect is that - * one less register has to be saved during the context switch - * resulting in lower RAM usage and faster context switch. - * - * @note This option is only usable with the GCC compiler and is only useful - * on processors with many registers like ARM cores. - * @note If this option is enabled then ALL the libraries linked to the - * ChibiOS/RT code must be recompiled with the GCC option @p - * -ffixed-@. - * @note This option must be enabled in the Makefile, it is listed here for - * documentation only. - */ -#if defined(__DOXYGEN__) -#define CH_CURRP_REGISTER_CACHE "reg" -#endif - -/*===========================================================================*/ -/* Subsystem options. */ -/*===========================================================================*/ - -/** - * @brief Threads registry APIs. - * @details If enabled then the registry APIs are included in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_REGISTRY) || defined(__DOXYGEN__) -#define CH_USE_REGISTRY TRUE -#endif - -/** - * @brief Threads synchronization APIs. - * @details If enabled then the @p chThdWait() function is included in - * the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_WAITEXIT) || defined(__DOXYGEN__) -#define CH_USE_WAITEXIT TRUE -#endif - -/** - * @brief Semaphores APIs. - * @details If enabled then the Semaphores APIs are included in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_SEMAPHORES) || defined(__DOXYGEN__) -#define CH_USE_SEMAPHORES TRUE -#endif - -/** - * @brief Semaphores queuing mode. - * @details If enabled then the threads are enqueued on semaphores by - * priority rather than in FIFO order. - * - * @note The default is @p FALSE. Enable this if you have special requirements. - * @note Requires @p CH_USE_SEMAPHORES. - */ -#if !defined(CH_USE_SEMAPHORES_PRIORITY) || defined(__DOXYGEN__) -#define CH_USE_SEMAPHORES_PRIORITY FALSE -#endif - -/** - * @brief Atomic semaphore API. - * @details If enabled then the semaphores the @p chSemSignalWait() API - * is included in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_SEMAPHORES. - */ -#if !defined(CH_USE_SEMSW) || defined(__DOXYGEN__) -#define CH_USE_SEMSW TRUE -#endif - -/** - * @brief Mutexes APIs. - * @details If enabled then the mutexes APIs are included in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_MUTEXES) || defined(__DOXYGEN__) -#define CH_USE_MUTEXES TRUE -#endif - -/** - * @brief Conditional Variables APIs. - * @details If enabled then the conditional variables APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_MUTEXES. - */ -#if !defined(CH_USE_CONDVARS) || defined(__DOXYGEN__) -#define CH_USE_CONDVARS TRUE -#endif - -/** - * @brief Conditional Variables APIs with timeout. - * @details If enabled then the conditional variables APIs with timeout - * specification are included in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_CONDVARS. - */ -#if !defined(CH_USE_CONDVARS_TIMEOUT) || defined(__DOXYGEN__) -#define CH_USE_CONDVARS_TIMEOUT TRUE -#endif - -/** - * @brief Events Flags APIs. - * @details If enabled then the event flags APIs are included in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_EVENTS) || defined(__DOXYGEN__) -#define CH_USE_EVENTS TRUE -#endif - -/** - * @brief Events Flags APIs with timeout. - * @details If enabled then the events APIs with timeout specification - * are included in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_EVENTS. - */ -#if !defined(CH_USE_EVENTS_TIMEOUT) || defined(__DOXYGEN__) -#define CH_USE_EVENTS_TIMEOUT TRUE -#endif - -/** - * @brief Synchronous Messages APIs. - * @details If enabled then the synchronous messages APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_MESSAGES) || defined(__DOXYGEN__) -#define CH_USE_MESSAGES TRUE -#endif - -/** - * @brief Synchronous Messages queuing mode. - * @details If enabled then messages are served by priority rather than in - * FIFO order. - * - * @note The default is @p FALSE. Enable this if you have special requirements. - * @note Requires @p CH_USE_MESSAGES. - */ -#if !defined(CH_USE_MESSAGES_PRIORITY) || defined(__DOXYGEN__) -#define CH_USE_MESSAGES_PRIORITY FALSE -#endif - -/** - * @brief Mailboxes APIs. - * @details If enabled then the asynchronous messages (mailboxes) APIs are - * included in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_SEMAPHORES. - */ -#if !defined(CH_USE_MAILBOXES) || defined(__DOXYGEN__) -#define CH_USE_MAILBOXES TRUE -#endif - -/** - * @brief I/O Queues APIs. - * @details If enabled then the I/O queues APIs are included in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_SEMAPHORES. - */ -#if !defined(CH_USE_QUEUES) || defined(__DOXYGEN__) -#define CH_USE_QUEUES TRUE -#endif - -/** - * @brief Core Memory Manager APIs. - * @details If enabled then the core memory manager APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_MEMCORE) || defined(__DOXYGEN__) -#define CH_USE_MEMCORE TRUE -#endif - -/** - * @brief Heap Allocator APIs. - * @details If enabled then the memory heap allocator APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_COREMEM and either @p CH_USE_MUTEXES or - * @p CH_USE_SEMAPHORES. - * @note Mutexes are recommended. - */ -#if !defined(CH_USE_HEAP) || defined(__DOXYGEN__) -#define CH_USE_HEAP TRUE -#endif - -/** - * @brief C-runtime allocator. - * @details If enabled the the heap allocator APIs just wrap the C-runtime - * @p malloc() and @p free() functions. - * - * @note The default is @p FALSE. - * @note Requires @p CH_USE_HEAP. - * @note The C-runtime may or may not require @p CH_USE_COREMEM, see the - * appropriate documentation. - */ -#if !defined(CH_USE_MALLOC_HEAP) || defined(__DOXYGEN__) -#define CH_USE_MALLOC_HEAP FALSE -#endif - -/** - * @brief Memory Pools Allocator APIs. - * @details If enabled then the memory pools allocator APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - */ -#if !defined(CH_USE_MEMPOOLS) || defined(__DOXYGEN__) -#define CH_USE_MEMPOOLS TRUE -#endif - -/** - * @brief Dynamic Threads APIs. - * @details If enabled then the dynamic threads creation APIs are included - * in the kernel. - * - * @note The default is @p TRUE. - * @note Requires @p CH_USE_WAITEXIT. - * @note Requires @p CH_USE_HEAP and/or @p CH_USE_MEMPOOLS. - */ -#if !defined(CH_USE_DYNAMIC) || defined(__DOXYGEN__) -#define CH_USE_DYNAMIC TRUE -#endif - -/*===========================================================================*/ -/* Debug options. */ -/*===========================================================================*/ - -/** - * @brief Debug option, parameters checks. - * @details If enabled then the checks on the API functions input - * parameters are activated. - * - * @note The default is @p FALSE. - */ -#if !defined(CH_DBG_ENABLE_CHECKS) || defined(__DOXYGEN__) -#define CH_DBG_ENABLE_CHECKS FALSE -#endif - -/** - * @brief Debug option, consistency checks. - * @details If enabled then all the assertions in the kernel code are - * activated. This includes consistency checks inside the kernel, - * runtime anomalies and port-defined checks. - * - * @note The default is @p FALSE. - */ -#if !defined(CH_DBG_ENABLE_ASSERTS) || defined(__DOXYGEN__) -#define CH_DBG_ENABLE_ASSERTS FALSE -#endif - -/** - * @brief Debug option, trace buffer. - * @details If enabled then the context switch circular trace buffer is - * activated. - * - * @note The default is @p FALSE. - */ -#if !defined(CH_DBG_ENABLE_TRACE) || defined(__DOXYGEN__) -#define CH_DBG_ENABLE_TRACE FALSE -#endif - -/** - * @brief Debug option, stack checks. - * @details If enabled then a runtime stack check is performed. - * - * @note The default is @p FALSE. - * @note The stack check is performed in a architecture/port dependent way. - * It may not be implemented or some ports. - * @note The default failure mode is to halt the system with the global - * @p panic_msg variable set to @p NULL. - */ -#if !defined(CH_DBG_ENABLE_STACK_CHECK) || defined(__DOXYGEN__) -#define CH_DBG_ENABLE_STACK_CHECK FALSE -#endif - -/** - * @brief Debug option, stacks initialization. - * @details If enabled then the threads working area is filled with a byte - * value when a thread is created. This can be useful for the - * runtime measurement of the used stack. - * - * @note The default is @p FALSE. - */ -#if !defined(CH_DBG_FILL_THREADS) || defined(__DOXYGEN__) -#define CH_DBG_FILL_THREADS FALSE -#endif - -/** - * @brief Debug option, threads profiling. - * @details If enabled then a field is added to the @p Thread structure that - * counts the system ticks occurred while executing the thread. - * - * @note The default is @p TRUE. - * @note This debug option is defaulted to TRUE because it is required by - * some test cases into the test suite. - */ -#if !defined(CH_DBG_THREADS_PROFILING) || defined(__DOXYGEN__) -#define CH_DBG_THREADS_PROFILING TRUE -#endif - -/*===========================================================================*/ -/* Kernel hooks. */ -/*===========================================================================*/ - -/** - * @brief Threads descriptor structure extension. - * @details User fields added to the end of the @p Thread structure. - */ -#if !defined(THREAD_EXT_FIELDS) || defined(__DOXYGEN__) -#define THREAD_EXT_FIELDS \ - /* Add threads custom fields here.*/ -#endif - -/** - * @brief Threads initialization hook. - * @details User initialization code added to the @p chThdInit() API. - * - * @note It is invoked from within @p chThdInit() and implicitily from all - * the threads creation APIs. - */ -#if !defined(THREAD_EXT_INIT_HOOK) || defined(__DOXYGEN__) -#define THREAD_EXT_INIT_HOOK(tp) { \ - /* Add threads initialization code here.*/ \ -} -#endif - -/** - * @brief Threads finalization hook. - * @details User finalization code added to the @p chThdExit() API. - * - * @note It is inserted into lock zone. - * @note It is also invoked when the threads simply return in order to - * terminate. - */ -#if !defined(THREAD_EXT_EXIT_HOOK) || defined(__DOXYGEN__) -#define THREAD_EXT_EXIT_HOOK(tp) { \ - /* Add threads finalization code here.*/ \ -} -#endif - -/** - * @brief Idle Loop hook. - * @details This hook is continuously invoked by the idle thread loop. - */ -#if !defined(IDLE_LOOP_HOOK) || defined(__DOXYGEN__) -#define IDLE_LOOP_HOOK() { \ - /* Idle loop code here.*/ \ -} -#endif - -/** - * @brief System tick event hook. - * @details This hook is invoked in the system tick handler immediately - * after processing the virtual timers queue. - */ -#if !defined(SYSTEM_TICK_EVENT_HOOK) || defined(__DOXYGEN__) -#define SYSTEM_TICK_EVENT_HOOK() { \ - /* System tick event code here.*/ \ -} -#endif - -/** - * @brief System halt hook. - * @details This hook is invoked in case to a system halting error before - * the system is halted. - */ -#if !defined(SYSTEM_HALT_HOOK) || defined(__DOXYGEN__) -#define SYSTEM_HALT_HOOK() { \ - /* System halt code here.*/ \ -} -#endif - -/*===========================================================================*/ -/* Port-specific settings (override port settings defaulted in chcore.h). */ -/*===========================================================================*/ - -#endif /* _CHCONF_H_ */ - -/** @} */ diff --git a/chibios b/chibios deleted file mode 160000 index fb2b302..0000000 --- a/chibios +++ /dev/null @@ -1 +0,0 @@ -Subproject commit fb2b302ea5e9529bde203aa1a7057369d6f52170 diff --git a/foo.h b/foo.h deleted file mode 100644 index 312d839..0000000 --- a/foo.h +++ /dev/null @@ -1,84 +0,0 @@ -int usbprintf(USBSerial& usbs, const char* format, ...) { - int32_t* argp = (int32_t*)&format; - int num = 0; - - while(*format) { - if(*format != '%') { - usbs.putc(*format++); - num++; - continue; - } - format++; - - bool is_signed = false; - bool zero_pad = false; - int radix = 16; - int min_len = 0; - - if(*format == '0') { - zero_pad = true; - } - - while(*format >= '0' && *format <= '9') { - min_len = min_len * 10 + *format++ - '0'; - } - - switch(*format++) { - case '%': - usbs.putc('%'); - num++; - break; - - case 'c': - usbs.putc((char)*++argp); - num++; - break; - - case 's': { - char* str = (char*)*++argp; - while(*str) { - usbs.putc(*str++); - num++; - } - } break; - - case 'd': - is_signed = true; - case 'u': - radix = 10; - case 'x': - ; - uint32_t x = (uint32_t)*(++argp); - if(is_signed && (int32_t)x < 0) { - x = -x; - usbs.putc('-'); - min_len--; - } - - char buf[11]; - char* bufp = &buf[sizeof(buf)]; - *--bufp = 0; - - do { - int d = x % radix; - *--bufp = d < 10 ? '0' + d : 'a' - 10 + d; - x /= radix; - min_len--; - } while(x); - - while(min_len > 0) { - usbs.putc(zero_pad ? '0' : ' '); - num++; - min_len--; - } - - while(*bufp) { - usbs.putc(*bufp++); - num++; - } - break; - } - } - - return num; -} diff --git a/halconf.h b/halconf.h deleted file mode 100644 index f8b44ec..0000000 --- a/halconf.h +++ /dev/null @@ -1,283 +0,0 @@ -/* - ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, - 2011 Giovanni Di Sirio. - - This file is part of ChibiOS/RT. - - ChibiOS/RT is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 3 of the License, or - (at your option) any later version. - - ChibiOS/RT is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/** - * @file templates/halconf.h - * @brief HAL configuration header. - * @details HAL configuration file, this file allows to enable or disable the - * various device drivers from your application. You may also use - * this file in order to override the device drivers default settings. - * - * @addtogroup HAL_CONF - * @{ - */ - -#ifndef _HALCONF_H_ -#define _HALCONF_H_ - -#include "mcuconf.h" - -/** - * @brief Enables the PAL subsystem. - */ -#if !defined(HAL_USE_PAL) || defined(__DOXYGEN__) -#define HAL_USE_PAL TRUE -#endif - -/** - * @brief Enables the ADC subsystem. - */ -#if !defined(HAL_USE_ADC) || defined(__DOXYGEN__) -#define HAL_USE_ADC TRUE -#endif - -/** - * @brief Enables the CAN subsystem. - */ -#if !defined(HAL_USE_CAN) || defined(__DOXYGEN__) -#define HAL_USE_CAN FALSE -#endif - -/** - * @brief Enables the GPT subsystem. - */ -#if !defined(HAL_USE_GPT) || defined(__DOXYGEN__) -#define HAL_USE_GPT FALSE -#endif - -/** - * @brief Enables the I2C subsystem. - */ -#if !defined(HAL_USE_I2C) || defined(__DOXYGEN__) -#define HAL_USE_I2C TRUE -#endif - -#define HAL_USE_ICU TRUE - -/** - * @brief Enables the MAC subsystem. - */ -#if !defined(HAL_USE_MAC) || defined(__DOXYGEN__) -#define HAL_USE_MAC FALSE -#endif - -/** - * @brief Enables the MMC_SPI subsystem. - */ -#if !defined(HAL_USE_MMC_SPI) || defined(__DOXYGEN__) -#define HAL_USE_MMC_SPI FALSE -#endif - -/** - * @brief Enables the PWM subsystem. - */ -#if !defined(HAL_USE_PWM) || defined(__DOXYGEN__) -#define HAL_USE_PWM TRUE -#endif - -/** - * @brief Enables the SERIAL subsystem. - */ -#if !defined(HAL_USE_SERIAL) || defined(__DOXYGEN__) -#define HAL_USE_SERIAL TRUE -#endif - -/** - * @brief Enables the SERIAL over USB subsystem. - */ -#if !defined(HAL_USE_SERIAL_USB) || defined(__DOXYGEN__) -#define HAL_USE_SERIAL_USB TRUE -#endif - -/** - * @brief Enables the SPI subsystem. - */ -#if !defined(HAL_USE_SPI) || defined(__DOXYGEN__) -#define HAL_USE_SPI FALSE -#endif - -/** - * @brief Enables the UART subsystem. - */ -#if !defined(HAL_USE_UART) || defined(__DOXYGEN__) -#define HAL_USE_UART FALSE -#endif - -/** - * @brief Enables the USB subsystem. - */ -#if !defined(HAL_USE_USB) || defined(__DOXYGEN__) -#define HAL_USE_USB TRUE -#endif - -/*===========================================================================*/ -/* ADC driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Enables synchronous APIs. - * @note Disabling this option saves both code and data space. - */ -#if !defined(ADC_USE_WAIT) || defined(__DOXYGEN__) -#define ADC_USE_WAIT TRUE -#endif - -/** - * @brief Enables the @p adcAcquireBus() and @p adcReleaseBus() APIs. - * @note Disabling this option saves both code and data space. - */ -#if !defined(ADC_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) -#define ADC_USE_MUTUAL_EXCLUSION TRUE -#endif - -/*===========================================================================*/ -/* CAN driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Sleep mode related APIs inclusion switch. - */ -#if !defined(CAN_USE_SLEEP_MODE) || defined(__DOXYGEN__) -#define CAN_USE_SLEEP_MODE TRUE -#endif - -/*===========================================================================*/ -/* I2C driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Enables the mutual exclusion APIs on the I2C bus. - */ -#if !defined(I2C_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) -#define I2C_USE_MUTUAL_EXCLUSION TRUE -#endif - -/*===========================================================================*/ -/* MAC driver related settings. */ -/*===========================================================================*/ - -/*===========================================================================*/ -/* MMC_SPI driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Block size for MMC transfers. - */ -#if !defined(MMC_SECTOR_SIZE) || defined(__DOXYGEN__) -#define MMC_SECTOR_SIZE 512 -#endif - -/** - * @brief Delays insertions. - * @details If enabled this options inserts delays into the MMC waiting - * routines releasing some extra CPU time for the threads with - * lower priority, this may slow down the driver a bit however. - * This option is recommended also if the SPI driver does not - * use a DMA channel and heavily loads the CPU. - */ -#if !defined(MMC_NICE_WAITING) || defined(__DOXYGEN__) -#define MMC_NICE_WAITING TRUE -#endif - -/** - * @brief Number of positive insertion queries before generating the - * insertion event. - */ -#if !defined(MMC_POLLING_INTERVAL) || defined(__DOXYGEN__) -#define MMC_POLLING_INTERVAL 10 -#endif - -/** - * @brief Interval, in milliseconds, between insertion queries. - */ -#if !defined(MMC_POLLING_DELAY) || defined(__DOXYGEN__) -#define MMC_POLLING_DELAY 10 -#endif - -/** - * @brief Uses the SPI polled API for small data transfers. - * @details Polled transfers usually improve performance because it - * saves two context switches and interrupt servicing. Note - * that this option has no effect on large transfers which - * are always performed using DMAs/IRQs. - */ -#if !defined(MMC_USE_SPI_POLLING) || defined(__DOXYGEN__) -#define MMC_USE_SPI_POLLING TRUE -#endif - -/*===========================================================================*/ -/* PAL driver related settings. */ -/*===========================================================================*/ - -/*===========================================================================*/ -/* PWM driver related settings. */ -/*===========================================================================*/ - -/*===========================================================================*/ -/* SERIAL driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Default bit rate. - * @details Configuration parameter, this is the baud rate selected for the - * default configuration. - */ -#if !defined(SERIAL_DEFAULT_BITRATE) || defined(__DOXYGEN__) -#define SERIAL_DEFAULT_BITRATE 38400 -#endif - -/** - * @brief Serial buffers size. - * @details Configuration parameter, you can change the depth of the queue - * buffers depending on the requirements of your application. - * @note The default is 64 bytes for both the transmission and receive - * buffers. - */ -#if !defined(SERIAL_BUFFERS_SIZE) || defined(__DOXYGEN__) -#define SERIAL_BUFFERS_SIZE 16 -#endif - -/*===========================================================================*/ -/* SPI driver related settings. */ -/*===========================================================================*/ - -/** - * @brief Enables synchronous APIs. - * @note Disabling this option saves both code and data space. - */ -#if !defined(SPI_USE_WAIT) || defined(__DOXYGEN__) -#define SPI_USE_WAIT TRUE -#endif - -/** - * @brief Enables the @p spiAcquireBus() and @p spiReleaseBus() APIs. - * @note Disabling this option saves both code and data space. - */ -#if !defined(SPI_USE_MUTUAL_EXCLUSION) || defined(__DOXYGEN__) -#define SPI_USE_MUTUAL_EXCLUSION TRUE -#endif - -/*===========================================================================*/ -/* UART driver related settings. */ -/*===========================================================================*/ - -#endif /* _HALCONF_H_ */ - -/** @} */ diff --git a/i2csensor.cpp b/i2csensor.cpp deleted file mode 100644 index f9d8d11..0000000 --- a/i2csensor.cpp +++ /dev/null @@ -1,77 +0,0 @@ -#include "i2csensor.h" - -namespace { - void callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) { - if(i2cp->id_slave_config->restart) { - i2cp->id_slave_config->restart = FALSE; - i2cMasterReceive(i2cp, i2cscfg); - } else { - i2cMasterStop(i2cp); - } - } - - void err_callback(I2CDriver* i2cp, I2CSlaveConfig* i2cscfg) { - - } - - I2CConfig i2c_conf = { - opmodeI2C, - 100000, - stdDutyCycle, - 0, - 0 - }; -}; - -uint8_t I2CSensor::rxdata[]; -uint8_t I2CSensor::txdata[]; - -I2CSlaveConfig I2CSensor::i2c_slave_conf = { - callback, err_callback, - - rxdata, sizeof(rxdata), 0, 0, - txdata, sizeof(txdata), 0, 0, - - 0, - FALSE, -}; - -void I2CSensor::enable_bus() { - palSetPadMode(GPIOB, 10, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); - palSetPadMode(GPIOB, 11, PAL_MODE_STM32_ALTERNATE_OPENDRAIN); - - i2cStart(&I2CD2, &i2c_conf); - - while(I2CD2.id_state != I2C_READY) { - chThdSleepMilliseconds(1); - } -} - -#include "usbserial.h" -extern USBSerial usbs; -int usbprintf(USBSerial& usbs, const char* format, ...); - -void I2CSensor::write(uint8_t reg, uint8_t value) { - I2CD2.id_state = I2C_READY; - i2c_slave_conf.address = i2c_address; - i2c_slave_conf.txbytes = 2; - i2c_slave_conf.txbuf[0] = reg; - i2c_slave_conf.txbuf[1] = value; - - i2cMasterTransmit(&I2CD2, &i2c_slave_conf); - chThdSleepMilliseconds(2); -} - -void I2CSensor::read(uint8_t reg, size_t len) { - I2CD2.id_state = I2C_READY; - i2c_slave_conf.address = i2c_address; - i2c_slave_conf.txbufhead = 0; - i2c_slave_conf.txbuf[0] = reg; - i2c_slave_conf.txbytes = 1; - i2c_slave_conf.rxbufhead = 0; - i2c_slave_conf.rxbytes = len; - i2c_slave_conf.restart = TRUE; - - i2cMasterTransmit(&I2CD2, &i2c_slave_conf); - chThdSleepMilliseconds(2); -} diff --git a/i2csensor.h b/i2csensor.h deleted file mode 100644 index 1878e16..0000000 --- a/i2csensor.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef I2CSENSOR_H -#define I2CSENSOR_H - -#include - -#include -#include - -class I2CSensor { - private: - static I2CSlaveConfig i2c_slave_conf; - - public: - static void enable_bus(); - - protected: - uint8_t i2c_address; - - void write(uint8_t reg, uint8_t value); - void read(uint8_t reg, size_t len); - - static uint8_t rxdata[8]; - static uint8_t txdata[2]; -}; - -#endif diff --git a/itg3200.h b/itg3200.h deleted file mode 100644 index 90cc4b3..0000000 --- a/itg3200.h +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef ITG3200_H -#define ITG3200_H - -#include "i2csensor.h" - -class ITG3200 : public I2CSensor { - public: - int16_t x, y, z; - - void init() { - i2c_address = 0x68; - write(0x3e, 0x03); // Select clock reference. - write(0x16, 0x18 | 0x02); // 2000 deg/sec range, 98 Hz bandwidth. - } - - void update() { - read(0x1d, 6); - x = (rxdata[0] << 8 | rxdata[1]) - -50; - y = (rxdata[2] << 8 | rxdata[3]) - 36; - z = (rxdata[4] << 8 | rxdata[5]) - 2; - } -}; - -#endif diff --git a/main.cpp b/main.cpp index 6eb1a27..7af1122 100644 --- a/main.cpp +++ b/main.cpp @@ -230,259 +230,3 @@ int main() { } } } - -/* -#include "thread.h" -#include "usbserial.h" -#include "itg3200.h" -#include "bma150.h" -#include "ak8975.h" -#include "ppmsum.h" -#include "motormixer.h" - -#include -#include - -#include "IMU.h" - -class LEDThread : public BaseThread { - public: - noreturn_t thread_main() { - systime_t time = chTimeNow(); // T0 - while (TRUE) { - time += MS2ST(1000); // Next deadline - palClearPad(GPIOA, 5); - chThdSleepUntil(time); - time += MS2ST(1000); // Next deadline - palSetPad(GPIOA, 5); - chThdSleepUntil(time); - } - } -}; - -LEDThread led_thread; - -PPMSum ppmsum; -MotorMixer motors; - -class USBThread : public BaseThread { - private: - typedef enum {W_S, W_N, W_V} w_s_t; - - public: - USBSerial* usbs; - - uint8_t data[9]; - - noreturn_t thread_main() { - for(int i = 0; i < 9; i++) { - data[i] = 0; - } - - w_s_t w_s = W_S; - uint8_t w_n = 0; - - while(1) { - size_t buffer = usbs->getc(); - if(buffer >= 0 && buffer < 256) { - if(w_s == W_S && buffer == 'S') { - w_s = W_N; - } else if(w_s == W_N && buffer >= '1' && buffer <= '9') { - w_s = W_V; - w_n = buffer - '1'; - } else if(w_s == W_V) { - w_s = W_S; - data[w_n] = buffer; - } else { - w_s = W_S; - } - } - } - } -}; - -USBThread usb_thread; -USBSerial usbs; - -#include "foo.h" -#include - -uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55}; -uint8_t buf[64]; -int16_t* sensordata = (int16_t*)buf; - -template -inline void saturate(T& var, int absmax) { - if(var > absmax) { - var = absmax; - } else if(var < -absmax) { - var = -absmax; - } -} - -class I2CThread : public BaseThread { - public: - ITG3200 gyro; - BMA150 acc; - AK8975 magn; - - int16_t x, y, z; - - noreturn_t thread_main() { - I2CSensor::enable_bus(); - - gyro.init(); - acc.init(); - magn.init(); - - systime_t time = chTimeNow(); - - int32_t pitch_angle_accum = 0; - int32_t roll_angle_accum = 0; - - while (1) { - gyro.update(); - acc.update(); - magn.update(); - x = gyro.x; - y = gyro.y; - z = gyro.z; - - IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z); - - //float pitch = asinf(2*(q0*q2 - q3*q1)); - //int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - //int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767; - //int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767; - - float norm_x = 2*(q0*q2 - q1*q3); - float norm_y = 2*(q0*q1 + q2*q3); - float norm_z = (1 - 2*(q1*q1 + q2*q2)); - - float elev = acosf(norm_z); - float azim = atan2f(norm_y, norm_x); - - int16_t pitch = elev * sinf(azim) / M_PI * 32767; - int16_t roll = elev * cosf(azim) / M_PI * 32767; - int16_t yaw = 0; - - sensordata[0] = gyro.x; - sensordata[1] = gyro.y; - sensordata[2] = gyro.z; - sensordata[3] = acc.x; - sensordata[4] = acc.y; - sensordata[5] = acc.z; - sensordata[6] = magn.x; - sensordata[7] = magn.y; - sensordata[8] = magn.z; - sensordata[9] = pitch; - sensordata[10] = roll; - sensordata[11] = yaw; - - usbs.write(syncword, sizeof(syncword)); - usbs.write(buf, sizeof(buf)); - - /*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n", - gyro.x, gyro.y, gyro.z, - acc.x, acc.y, acc.z, - magn.x, magn.y, magn.z, - int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000), - int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*//* - - int16_t pitch_angle_target = (ppmsum.data[1] - 500) * 8; - int16_t roll_angle_target = (ppmsum.data[0] - 500) * 8; - - int16_t pitch_angle_error = pitch_angle_target - pitch; - int16_t roll_angle_error = roll_angle_target - roll; - - // 25 deg max error. - saturate(pitch_angle_error, 4551); - saturate(roll_angle_error, 4551); - - pitch_angle_accum += pitch_angle_error; - roll_angle_accum += roll_angle_error; - - // 20 deg s max error. - saturate(pitch_angle_accum, 364088); - saturate(roll_angle_accum, 364088); - - int32_t pitch_rate_target = (pitch_angle_error * 2 * 65536 + pitch_angle_accum * 98) >> 16; - int32_t roll_rate_target = (roll_angle_error * 2 * 65536 + roll_angle_accum * 98) >> 16; - - int16_t pitch_rate_comp = ((pitch_rate_target - (gyro.x * 4000 / 360)) * 6 * 36) >> 16; - int16_t roll_rate_comp = ((roll_rate_target - (gyro.y * 4000 / 360)) * 6 * 36) >> 16; - - saturate(pitch_rate_comp, 250); - saturate(roll_rate_comp, 250); - - motors.update(ppmsum.data[2], pitch_rate_comp, roll_rate_comp, 0); - - time += MS2ST(10); - if(time > chTimeNow()) { - chThdSleepUntil(time); - } - } - } -}; - -I2CThread i2c_thread; - -static const ADCConversionGroup adcgrpcfg = { - FALSE, - 2, - 0, - 0, - 0, - 0, - 0, - ADC_SQR1_NUM_CH(2), - 0, - ADC_SQR3_SQ2_N(ADC_CHANNEL_IN14) | ADC_SQR3_SQ1_N(ADC_CHANNEL_IN15) -}; - -class ADCThread : public BaseThread { - private: - adcsample_t adc_samples[2]; - - public: - noreturn_t thread_main() { - adcStart(&ADCD1, NULL); - - systime_t time = chTimeNow(); - while (TRUE) { - adcStartConversion(&ADCD1, &adcgrpcfg, adc_samples, 1); - sensordata[12] = adc_samples[0] * 1265 / 1000; - sensordata[13] = adc_samples[1] * 2201 / 1000; - - time += MS2ST(1000); - chThdSleepUntil(time); - } - } -}; - -ADCThread adc_thread; - -int main(void) { - halInit(); - chSysInit(); - - led_thread.start(); - - ppmsum.start(); - - usbs.init(); - - i2c_thread.start(); - - adc_thread.start(); - - motors.start(); - - usb_thread.usbs = &usbs; - usb_thread.start(); - - while (1) { - chThdSleepMilliseconds(1000); - } -} -*/ \ No newline at end of file diff --git a/mcuconf.h b/mcuconf.h deleted file mode 100644 index 5ecad24..0000000 --- a/mcuconf.h +++ /dev/null @@ -1,170 +0,0 @@ -/* - ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010, - 2011 Giovanni Di Sirio. - - This file is part of ChibiOS/RT. - - ChibiOS/RT is free software; you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation; either version 3 of the License, or - (at your option) any later version. - - ChibiOS/RT is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . -*/ - -/* - * STM32 drivers configuration. - * The following settings override the default settings present in - * the various device driver implementation headers. - * Note that the settings for each driver only have effect if the whole - * driver is enabled in halconf.h. - * - * IRQ priorities: - * 15...0 Lowest...Highest. - * - * DMA priorities: - * 0...3 Lowest...Highest. - */ - -/* - * HAL driver system settings. - */ -#define STM32_SW STM32_SW_PLL -#define STM32_PLLSRC STM32_PLLSRC_HSE -#define STM32_PLLXTPRE STM32_PLLXTPRE_DIV1 -#define STM32_PLLMUL_VALUE 9 -#define STM32_HPRE STM32_HPRE_DIV1 -#define STM32_PPRE1 STM32_PPRE1_DIV2 -#define STM32_PPRE2 STM32_PPRE2_DIV2 -#define STM32_ADCPRE STM32_ADCPRE_DIV4 -#define STM32_USBPRE STM32_USBPRE_DIV1P5 -#define STM32_MCO STM32_MCO_NOCLOCK - -/* - * ADC driver system settings. - */ -#define STM32_ADC_USE_ADC1 TRUE -#define STM32_ADC_ADC1_DMA_PRIORITY 3 -#define STM32_ADC_ADC1_IRQ_PRIORITY 5 -#define STM32_ADC_ADC1_DMA_ERROR_HOOK() chSysHalt() - -/* - * CAN driver system settings. - */ -#define STM32_CAN_USE_CAN1 TRUE -#define STM32_CAN_CAN1_IRQ_PRIORITY 11 - -/* - * GPT driver system settings. - */ -#define STM32_GPT_USE_TIM1 FALSE -#define STM32_GPT_USE_TIM2 FALSE -#define STM32_GPT_USE_TIM3 FALSE -#define STM32_GPT_USE_TIM4 FALSE -#define STM32_GPT_USE_TIM5 FALSE -#define STM32_GPT_TIM1_IRQ_PRIORITY 7 -#define STM32_GPT_TIM2_IRQ_PRIORITY 7 -#define STM32_GPT_TIM3_IRQ_PRIORITY 7 -#define STM32_GPT_TIM4_IRQ_PRIORITY 7 -#define STM32_GPT_TIM5_IRQ_PRIORITY 7 - -/* - * ICU driver system settings. - */ -#define STM32_ICU_USE_TIM1 FALSE -#define STM32_ICU_USE_TIM2 FALSE -#define STM32_ICU_USE_TIM3 FALSE -#define STM32_ICU_USE_TIM4 TRUE -#define STM32_ICU_USE_TIM5 FALSE -#define STM32_ICU_TIM1_IRQ_PRIORITY 7 -#define STM32_ICU_TIM2_IRQ_PRIORITY 7 -#define STM32_ICU_TIM3_IRQ_PRIORITY 7 -#define STM32_ICU_TIM4_IRQ_PRIORITY 7 -#define STM32_ICU_TIM5_IRQ_PRIORITY 7 - -/* - * PWM driver system settings. - */ -#define STM32_PWM_USE_ADVANCED FALSE -#define STM32_PWM_USE_TIM1 FALSE -#define STM32_PWM_USE_TIM2 TRUE -#define STM32_PWM_USE_TIM3 FALSE -#define STM32_PWM_USE_TIM4 FALSE -#define STM32_PWM_USE_TIM5 FALSE -#define STM32_PWM_TIM1_IRQ_PRIORITY 7 -#define STM32_PWM_TIM2_IRQ_PRIORITY 7 -#define STM32_PWM_TIM3_IRQ_PRIORITY 7 -#define STM32_PWM_TIM4_IRQ_PRIORITY 7 -#define STM32_PWM_TIM5_IRQ_PRIORITY 7 - -/* - * SERIAL driver system settings. - */ -#define STM32_SERIAL_USE_USART1 FALSE -#define STM32_SERIAL_USE_USART2 TRUE -#define STM32_SERIAL_USE_USART3 FALSE -#define STM32_SERIAL_USE_UART4 FALSE -#define STM32_SERIAL_USE_UART5 FALSE -#define STM32_SERIAL_USART1_PRIORITY 12 -#define STM32_SERIAL_USART2_PRIORITY 12 -#define STM32_SERIAL_USART3_PRIORITY 12 -#define STM32_SERIAL_UART4_PRIORITY 12 -#define STM32_SERIAL_UART5_PRIORITY 12 - -/* - * SPI driver system settings. - */ -#define STM32_SPI_USE_SPI1 TRUE -#define STM32_SPI_USE_SPI2 TRUE -#define STM32_SPI_USE_SPI3 FALSE -#define STM32_SPI_SPI1_DMA_PRIORITY 2 -#define STM32_SPI_SPI2_DMA_PRIORITY 2 -#define STM32_SPI_SPI3_DMA_PRIORITY 2 -#define STM32_SPI_SPI1_IRQ_PRIORITY 10 -#define STM32_SPI_SPI2_IRQ_PRIORITY 10 -#define STM32_SPI_SPI3_IRQ_PRIORITY 10 -#define STM32_SPI_SPI1_DMA_ERROR_HOOK() chSysHalt() -#define STM32_SPI_SPI2_DMA_ERROR_HOOK() chSysHalt() -#define STM32_SPI_SPI3_DMA_ERROR_HOOK() chSysHalt() - -/* - * UART driver system settings. - */ -#define STM32_UART_USE_USART1 FALSE -#define STM32_UART_USE_USART2 TRUE -#define STM32_UART_USE_USART3 FALSE -#define STM32_UART_USART1_IRQ_PRIORITY 12 -#define STM32_UART_USART2_IRQ_PRIORITY 12 -#define STM32_UART_USART3_IRQ_PRIORITY 12 -#define STM32_UART_USART1_DMA_PRIORITY 0 -#define STM32_UART_USART2_DMA_PRIORITY 0 -#define STM32_UART_USART3_DMA_PRIORITY 0 -#define STM32_UART_USART1_DMA_ERROR_HOOK() chSysHalt() -#define STM32_UART_USART2_DMA_ERROR_HOOK() chSysHalt() -#define STM32_UART_USART3_DMA_ERROR_HOOK() chSysHalt() - -/* - * USB driver system settings. - */ -#define STM32_USB_USE_USB1 TRUE -#define STM32_USB_LOW_POWER_ON_SUSPEND FALSE -#define STM32_USB_USB1_HP_IRQ_PRIORITY 6 -#define STM32_USB_USB1_LP_IRQ_PRIORITY 14 - -/* - * I2C driver system settings. - */ -#define STM32_I2C_USE_I2C1 TRUE -#define STM32_I2C_USE_I2C2 TRUE -#define STM32_I2C_I2C1_IRQ_PRIORITY 11 -#define STM32_I2C_I2C2_IRQ_PRIORITY 11 -#define STM32_I2C_I2C1_DMA_PRIORITY 4 -#define STM32_I2C_I2C2_DMA_PRIORITY 4 -#define STM32_I2C_I2C1_DMA_ERROR_HOOK() chSysHalt() -#define STM32_I2C_I2C2_DMA_ERROR_HOOK() chSysHalt() diff --git a/motormixer.cpp b/motormixer.cpp deleted file mode 100644 index 5718526..0000000 --- a/motormixer.cpp +++ /dev/null @@ -1,86 +0,0 @@ -#include "motormixer.h" - -#include -#include - -template -inline T abs(T val) { - if(val > 0) { - return val; - } else { - return -val; - } -} - -namespace { - uint16_t motors[4]; - - void foo(PWMDriver*) { - pwmEnableChannel(&PWMD2, 0, 1000 + motors[0]); - pwmEnableChannel(&PWMD2, 1, 1000 + motors[1]); - pwmEnableChannel(&PWMD2, 2, 1000 + motors[2]); - pwmEnableChannel(&PWMD2, 3, 1000 + motors[3]); - } - - static PWMConfig pwmcfg = { - 1000000, - 1000000 / 50, - foo, - { - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL}, - {PWM_OUTPUT_ACTIVE_HIGH, NULL} - }, - 0 - }; -}; - -void MotorMixer::start() { - pwmStart(&PWMD2, &pwmcfg); - palSetPadMode(GPIOA, 0, PAL_MODE_STM32_ALTERNATE_PUSHPULL); - palSetPadMode(GPIOA, 1, PAL_MODE_STM32_ALTERNATE_PUSHPULL); - palSetPadMode(GPIOA, 2, PAL_MODE_STM32_ALTERNATE_PUSHPULL); - palSetPadMode(GPIOA, 3, PAL_MODE_STM32_ALTERNATE_PUSHPULL); - - pwmEnableChannel(&PWMD2, 0, 1000); - pwmEnableChannel(&PWMD2, 1, 1000); - pwmEnableChannel(&PWMD2, 2, 1000); - pwmEnableChannel(&PWMD2, 3, 1000); -} - -void MotorMixer::update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw) { - //thrust = ppmsum.data[2]; - if(thrust < 0) { - thrust = 0; - } - if(thrust > 1000) { - thrust = 1000; - } - - // Check that total thrust will not be exceeded. - if(abs(pitch) + abs(roll) > thrust) { - // If saturated by pitch/roll, drop yaw component. - yaw = 0; - - // Scale pitch and roll. - int32_t df = ((abs(pitch) + abs(roll)) << 16) / thrust; - - pitch = (pitch << 16) / df; - roll = (roll << 16) / df; - - } else if(abs(pitch) + abs(roll) + abs(yaw) > thrust) { - // Scale yaw value. - yaw = (yaw > 0 ? 1 : -1) * (thrust - abs(pitch) - abs(roll)); - } - - motors[0] = thrust + pitch + roll - yaw; - motors[1] = thrust + pitch - roll + yaw; - motors[2] = thrust - pitch + roll + yaw; - motors[3] = thrust - pitch - roll - yaw; - - //sensordata[20] = motor_1; - //sensordata[21] = motor_2; - //sensordata[22] = motor_3; - //sensordata[23] = motor_4; -} diff --git a/motormixer.h b/motormixer.h deleted file mode 100644 index 5ec1869..0000000 --- a/motormixer.h +++ /dev/null @@ -1,12 +0,0 @@ -#ifndef MOTORMIXER_H -#define MOTORMIXER_H - -#include - -class MotorMixer { - public: - void start(); - void update(int16_t thrust, int16_t pitch, int16_t roll, int16_t yaw); -}; - -#endif diff --git a/ppmsum.cpp b/ppmsum.cpp deleted file mode 100644 index ce601e5..0000000 --- a/ppmsum.cpp +++ /dev/null @@ -1,39 +0,0 @@ -#include "ppmsum.h" - -#include -#include - -namespace { - icucnt_t last_width, last_period; - - PPMSum* ppmsum; - int ppm_n = 0; - - static void icuwidthcb(ICUDriver *icup) { - last_width = icuGetWidthI(icup); - ppmsum->data[ppm_n] = last_width - 1050; - } - - static void icuperiodcb(ICUDriver *icup) { - last_period = icuGetPeriodI(icup); - if(last_period > 5000) { - ppm_n = 0; - } else { - ppm_n = (ppm_n + 1) % 4; - } - } - - static ICUConfig icucfg = { - ICU_INPUT_ACTIVE_HIGH, - 1000000, - icuwidthcb, - icuperiodcb - }; -}; - -void PPMSum::start() { - ppmsum = this; - - icuStart(&ICUD4, &icucfg); - icuEnable(&ICUD4); -} diff --git a/ppmsum.h b/ppmsum.h deleted file mode 100644 index 82b62df..0000000 --- a/ppmsum.h +++ /dev/null @@ -1,13 +0,0 @@ -#ifndef PPMSUM_H -#define PPMSUM_H - -#include - -class PPMSum { - public: - int16_t data[4]; - - void start(); -}; - -#endif diff --git a/thread.h b/thread.h deleted file mode 100644 index b69801a..0000000 --- a/thread.h +++ /dev/null @@ -1,23 +0,0 @@ -#ifndef THREAD_H -#define THREAD_H - -#include - -#define noreturn_t __attribute__((noreturn)) void - -template -class BaseThread { - private: - WORKING_AREA(stack_space, stack_size); - - static inline noreturn_t thread_main_wrap(void* arg) { - ((Child*)arg)->thread_main(); - } - - public: - void start(tprio_t priority = NORMALPRIO) { - chThdCreateStatic(stack_space, sizeof(stack_space), priority, (tfunc_t)thread_main_wrap, this); - } -}; - -#endif diff --git a/usbserial.cpp b/usbserial.cpp deleted file mode 100644 index 421f938..0000000 --- a/usbserial.cpp +++ /dev/null @@ -1,281 +0,0 @@ -#include "usbserial.h" - -/* - * USB Device Descriptor. - */ -static const uint8_t vcom_device_descriptor_data[18] = { - USB_DESC_DEVICE (0x0110, /* bcdUSB (1.1). */ - 0x02, /* bDeviceClass (CDC). */