Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found

Target

Select target project
  • authierj/dfall-system
1 result
Show changes
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 4725 deletions
crazyflie-firmware-src/src/hal/interface/uart.h deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* uart.h - uart CRTP link and raw access functions
*/
#ifndef UART_H_
#define UART_H_
#include <stdbool.h>
#include "crtp.h"
#include "eprintf.h"
#define UART_TYPE USART3
#define UART_PERIF RCC_APB1Periph_USART3
#define UART_DMA_IRQ DMA1_Channel2_IRQn
#define UART_DMA_IT_TC DMA1_IT_TC2
#define UART_DMA_CH DMA1_Channel2
#define UART_GPIO_PERIF RCC_APB2Periph_GPIOB
#define UART_GPIO_PORT GPIOB
#define UART_GPIO_TX GPIO_Pin_10
#define UART_GPIO_RX GPIO_Pin_11
/**
* Initialize the UART.
*
* @note Initialize CRTP link only if USE_CRTP_UART is defined
*/
void uartInit(void);
/**
* Test the UART status.
*
* @return true if the UART is initialized
*/
bool uartTest(void);
/**
* Get CRTP link data structure
*
* @return Address of the crtp link operations structure.
*/
struct crtpLinkOperations * uartGetLink();
/**
* Sends raw data using a lock. Should be used from
* exception functions and for debugging when a lot of data
* should be transfered.
* @param[in] size Number of bytes to send
* @param[in] data Pointer to data
*
* @note If UART Crtp link is activated this function does nothing
*/
void uartSendData(uint32_t size, uint8_t* data);
/**
* Send a single character to the serial port using the uartSendData function.
* @param[in] ch Character to print. Only the 8 LSB are used.
* @return Character printed
*
* @note If UART Crtp link is activated this function does nothing
*/
int uartPutchar(int ch);
/**
* Uart printf macro that uses eprintf
* @param[in] FMT String format
* @param[in] ... Parameters to print
*
* @note If UART Crtp link is activated this function does nothing
*/
#define uartPrintf(FMT, ...) eprintf(uartPutchar, FMT, ## __VA_ARGS__)
/**
* Sends raw data using DMA transfer. Should be used from
* exception functions and for debugging when a lot of data
* should be transfered.
* @param[in] size Number of bytes to send
* @param[in] data Pointer to data
*
* @note If UART Crtp link is activated this function does nothing
*/
void uartSendDataDma(uint32_t size, uint8_t* data);
/**
* Interrupt service routine handling UART interrupts.
*/
void uartIsr(void);
/**
* Interrupt service routine handling UART DMA interrupts.
*/
void uartDmaIsr(void);
#endif /* UART_H_ */
crazyflie-firmware-src/src/hal/interface/usb.h deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* uart.h - uart CRTP link and raw access functions
*/
#ifndef USB_H_
#define USB_H_
#include <stdbool.h>
#include <stdint.h>
#include "usbd_conf.h"
#define USB_RX_TX_PACKET_SIZE (64)
/* Structure used for in/out data via USB */
typedef struct
{
uint8_t size;
uint8_t data[USB_RX_TX_PACKET_SIZE];
} USBPacket;
/**
* Initialize the UART.
*
* @note Initialize CRTP link only if USE_CRTP_UART is defined
*/
void usbInit(void);
/**
* Test the UART status.
*
* @return true if the UART is initialized
*/
bool usbTest(void);
/**
* Get CRTP link data structure
*
* @return Address of the crtp link operations structure.
*/
struct crtpLinkOperations * usbGetLink();
/**
* Get data from rx queue with timeout.
* @param[out] c Byte of data
*
* @return true if byte received, false if timout reached.
*/
bool usbGetDataBlocking(USBPacket *in);
/**
* Sends raw data using a lock. Should be used from
* exception functions and for debugging when a lot of data
* should be transfered.
* @param[in] size Number of bytes to send
* @param[in] data Pointer to data
*
* @note If UART Crtp link is activated this function does nothing
*/
bool usbSendData(uint32_t size, uint8_t* data);
#endif /* UART_H_ */
crazyflie-firmware-src/src/hal/interface/usb_conf.h deleted 100755 → 0
View file @ 6291d058
/**
******************************************************************************
* @file usb_conf.h
* @author MCD Application Team
* @version V1.1.0
* @date 19-March-2012
* @brief General low level driver configuration
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2012 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USB_CONF__H__
#define __USB_CONF__H__
#include <sys/cdefs.h>
/* Includes ------------------------------------------------------------------*/
/*#if defined (USE_STM322xG_EVAL)
#include "stm322xg_eval.h"
#include "stm322xg_eval_lcd.h"
#include "stm322xg_eval_ioe.h"
#include "stm322xg_eval_sdio_sd.h"
#elif defined(USE_STM324xG_EVAL)
#include "stm32f4xx.h"
#include "stm324xg_eval.h"
#include "stm324xg_eval_lcd.h"
#include "stm324xg_eval_ioe.h"
#include "stm324xg_eval_sdio_sd.h"
#elif defined (USE_STM3210C_EVAL)
#include "stm32f10x.h"
#include "stm3210c_eval.h"
#include "stm3210c_eval_lcd.h"
#include "stm3210c_eval_ioe.h"
#include "stm3210c_eval_spi_sd.h"
#else
#error "Missing define: Evaluation board (ie. USE_STM322xG_EVAL)"
#endif*/
/** @addtogroup USB_OTG_DRIVER
* @{
*/
/** @defgroup USB_CONF
* @brief USB low level driver configuration file
* @{
*/
/** @defgroup USB_CONF_Exported_Defines
* @{
*/
/* USB Core and PHY interface configuration.
Tip: To avoid modifying these defines each time you need to change the USB
configuration, you can declare the needed define in your toolchain
compiler preprocessor.
*/
/****************** USB OTG FS PHY CONFIGURATION *******************************
* The USB OTG FS Core supports one on-chip Full Speed PHY.
*
* The USE_EMBEDDED_PHY symbol is defined in the project compiler preprocessor
* when FS core is used.
*******************************************************************************/
#ifndef USE_USB_OTG_FS
#define USE_USB_OTG_FS
#endif /* USE_USB_OTG_FS */
#ifdef USE_USB_OTG_FS
#define USB_OTG_FS_CORE
#endif
/****************** USB OTG HS PHY CONFIGURATION *******************************
* The USB OTG HS Core supports two PHY interfaces:
* (i) An ULPI interface for the external High Speed PHY: the USB HS Core will
* operate in High speed mode
* (ii) An on-chip Full Speed PHY: the USB HS Core will operate in Full speed mode
*
* You can select the PHY to be used using one of these two defines:
* (i) USE_ULPI_PHY: if the USB OTG HS Core is to be used in High speed mode
* (ii) USE_EMBEDDED_PHY: if the USB OTG HS Core is to be used in Full speed mode
*
* Notes:
* - The USE_ULPI_PHY symbol is defined in the project compiler preprocessor as
* default PHY when HS core is used.
* - On STM322xG-EVAL and STM324xG-EVAL boards, only configuration(i) is available.
* Configuration (ii) need a different hardware, for more details refer to your
* STM32 device datasheet.
*******************************************************************************/
#ifndef USE_USB_OTG_HS
//#define USE_USB_OTG_HS
#endif /* USE_USB_OTG_HS */
#ifndef USE_ULPI_PHY
//#define USE_ULPI_PHY
#endif /* USE_ULPI_PHY */
#ifndef USE_EMBEDDED_PHY
//#define USE_EMBEDDED_PHY
#endif /* USE_EMBEDDED_PHY */
#ifdef USE_USB_OTG_HS
#define USB_OTG_HS_CORE
#endif
/*******************************************************************************
* FIFO Size Configuration in Device mode
*
* (i) Receive data FIFO size = RAM for setup packets +
* OUT endpoint control information +
* data OUT packets + miscellaneous
* Space = ONE 32-bits words
* --> RAM for setup packets = 10 spaces
* (n is the nbr of CTRL EPs the device core supports)
* --> OUT EP CTRL info = 1 space
* (one space for status information written to the FIFO along with each
* received packet)
* --> data OUT packets = (Largest Packet Size / 4) + 1 spaces
* (MINIMUM to receive packets)
* --> OR data OUT packets = at least 2*(Largest Packet Size / 4) + 1 spaces
* (if high-bandwidth EP is enabled or multiple isochronous EPs)
* --> miscellaneous = 1 space per OUT EP
* (one space for transfer complete status information also pushed to the
* FIFO with each endpoint's last packet)
*
* (ii)MINIMUM RAM space required for each IN EP Tx FIFO = MAX packet size for
* that particular IN EP. More space allocated in the IN EP Tx FIFO results
* in a better performance on the USB and can hide latencies on the AHB.
*
* (iii) TXn min size = 16 words. (n : Transmit FIFO index)
* (iv) When a TxFIFO is not used, the Configuration should be as follows:
* case 1 : n > m and Txn is not used (n,m : Transmit FIFO indexes)
* --> Txm can use the space allocated for Txn.
* case2 : n < m and Txn is not used (n,m : Transmit FIFO indexes)
* --> Txn should be configured with the minimum space of 16 words
* (v) The FIFO is used optimally when used TxFIFOs are allocated in the top
* of the FIFO.Ex: use EP1 and EP2 as IN instead of EP1 and EP3 as IN ones.
* (vi) In HS case 12 FIFO locations should be reserved for internal DMA registers
* so total FIFO size should be 1012 Only instead of 1024
*******************************************************************************/
/****************** USB OTG HS CONFIGURATION **********************************/
#ifdef USB_OTG_HS_CORE
#define RX_FIFO_HS_SIZE 512
#define TX0_FIFO_HS_SIZE 64
#define TX1_FIFO_HS_SIZE 372
#define TX2_FIFO_HS_SIZE 64
#define TX3_FIFO_HS_SIZE 0
#define TX4_FIFO_HS_SIZE 0
#define TX5_FIFO_HS_SIZE 0
// #define USB_OTG_HS_SOF_OUTPUT_ENABLED
#ifdef USE_ULPI_PHY
#define USB_OTG_ULPI_PHY_ENABLED
#endif
#ifdef USE_EMBEDDED_PHY
#define USB_OTG_EMBEDDED_PHY_ENABLED
/* wakeup is working only when HS core is configured in FS mode */
#define USB_OTG_HS_LOW_PWR_MGMT_SUPPORT
#endif
/* #define USB_OTG_HS_INTERNAL_DMA_ENABLED */ /* Be aware that enabling DMA mode will result in data being sent only by
multiple of 4 packet sizes. This is due to the fact that USB DMA does
not allow sending data from non word-aligned addresses.
For this specific application, it is advised to not enable this option
unless required. */
#define USB_OTG_HS_DEDICATED_EP1_ENABLED
#endif
/****************** USB OTG FS CONFIGURATION **********************************/
#ifdef USB_OTG_FS_CORE
#define RX_FIFO_FS_SIZE 128
#define TX0_FIFO_FS_SIZE 32
#define TX1_FIFO_FS_SIZE 128
#define TX2_FIFO_FS_SIZE 32
#define TX3_FIFO_FS_SIZE 0
// #define USB_OTG_FS_LOW_PWR_MGMT_SUPPORT
// #define USB_OTG_FS_SOF_OUTPUT_ENABLED
#endif
/****************** USB OTG MISC CONFIGURATION ********************************/
//#define VBUS_SENSING_ENABLED
/****************** USB OTG MODE CONFIGURATION ********************************/
//#define USE_HOST_MODE
#define USE_DEVICE_MODE
//#define USE_OTG_MODE
#ifndef USB_OTG_FS_CORE
#ifndef USB_OTG_HS_CORE
#error "USB_OTG_HS_CORE or USB_OTG_FS_CORE should be defined"
#endif
#endif
#ifndef USE_DEVICE_MODE
#ifndef USE_HOST_MODE
#error "USE_DEVICE_MODE or USE_HOST_MODE should be defined"
#endif
#endif
#ifndef USE_USB_OTG_HS
#ifndef USE_USB_OTG_FS
#error "USE_USB_OTG_HS or USE_USB_OTG_FS should be defined"
#endif
#else //USE_USB_OTG_HS
#ifndef USE_ULPI_PHY
#ifndef USE_EMBEDDED_PHY
#error "USE_ULPI_PHY or USE_EMBEDDED_PHY should be defined"
#endif
#endif
#endif
/****************** C Compilers dependant keywords ****************************/
/* In HS mode and when the DMA is used, all variables and data structures dealing
with the DMA during the transaction process should be 4-bytes aligned */
#ifdef USB_OTG_HS_INTERNAL_DMA_ENABLED
#if defined (__GNUC__) /* GNU Compiler */
#define __ALIGN_END __attribute__ ((aligned (4)))
#define __ALIGN_BEGIN
#else
#define __ALIGN_END
#if defined (__CC_ARM) /* ARM Compiler */
#define __ALIGN_BEGIN __align(4)
#elif defined (__ICCARM__) /* IAR Compiler */
#define __ALIGN_BEGIN
#elif defined (__TASKING__) /* TASKING Compiler */
#define __ALIGN_BEGIN __align(4)
#endif /* __CC_ARM */
#endif /* __GNUC__ */
#else
#define __ALIGN_BEGIN
#define __ALIGN_END
#endif /* USB_OTG_HS_INTERNAL_DMA_ENABLED */
/* __packed keyword used to decrease the data type alignment to 1-byte */
#if defined (__CC_ARM) /* ARM Compiler */
#define __packed __packed
#elif defined (__ICCARM__) /* IAR Compiler */
#define __packed __packed
#elif defined ( __GNUC__ ) /* GNU Compiler */
// #define __packed __attribute__ ((__packed__))
#ifndef __packed
#define __packed __attribute__ ((__packed__))
#endif
#elif defined (__TASKING__) /* TASKING Compiler */
#define __packed __unaligned
#endif /* __CC_ARM */
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Types
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_FunctionsPrototype
* @{
*/
/**
* @}
*/
#endif //__USB_CONF__H__
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
crazyflie-firmware-src/src/hal/interface/usbd_conf.h deleted 100755 → 0
View file @ 6291d058
/**
******************************************************************************
* @file usbd_conf.h
* @author MCD Application Team
* @version V1.1.0
* @date 19-March-2012
* @brief USB Device configuration file
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2012 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USBD_CONF__H__
#define __USBD_CONF__H__
/* Includes ------------------------------------------------------------------*/
#include "usb_conf.h"
/** @defgroup USB_CONF_Exported_Defines
* @{
*/
#define USBD_CFG_MAX_NUM 1
#define USBD_ITF_MAX_NUM 1
#define USBD_SELF_POWERED
#define USB_MAX_STR_DESC_SIZ 255
/** @defgroup USB_VCP_Class_Layer_Parameter
* @{
*/
#define CDC_IN_EP 0x81 /* EP1 for data IN */
#define CDC_OUT_EP 0x01 /* EP1 for data OUT */
#define CDC_CMD_EP 0x82 /* EP2 for CDC commands */
/* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */
#ifdef USE_USB_OTG_HS
#define CDC_DATA_MAX_PACKET_SIZE 512 /* Endpoint IN & OUT Packet size */
#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */
#define CDC_IN_FRAME_INTERVAL 40 /* Number of micro-frames between IN transfers */
#define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer:
APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL*8 */
#else
#define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */
#define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */
#define CDC_IN_FRAME_INTERVAL 1 /* Number of frames between IN transfers */
#define APP_RX_DATA_SIZE 2048 /* Total size of IN buffer:
APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */
#endif /* USE_USB_OTG_HS */
#define APP_FOPS CRTP_fops
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Types
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_Variables
* @{
*/
/**
* @}
*/
/** @defgroup USB_CONF_Exported_FunctionsPrototype
* @{
*/
/**
* @}
*/
#endif //__USBD_CONF__H__
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
crazyflie-firmware-src/src/hal/interface/usbd_desc.h deleted 100755 → 0
View file @ 6291d058
/**
******************************************************************************
* @file usbd_desc.h
* @author MCD Application Team
* @version V1.1.0
* @date 19-March-2012
* @brief header file for the usbd_desc.c file
******************************************************************************
* @attention
*
* <h2><center>&copy; COPYRIGHT 2012 STMicroelectronics</center></h2>
*
* Licensed under MCD-ST Liberty SW License Agreement V2, (the "License");
* You may not use this file except in compliance with the License.
* You may obtain a copy of the License at:
*
* http://www.st.com/software_license_agreement_liberty_v2
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
******************************************************************************
*/
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __USB_DESC_H
#define __USB_DESC_H
/* Includes ------------------------------------------------------------------*/
#include "usbd_def.h"
/** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY
* @{
*/
/** @defgroup USB_DESC
* @brief general defines for the usb device library file
* @{
*/
/** @defgroup USB_DESC_Exported_Defines
* @{
*/
#define USB_DEVICE_DESCRIPTOR_TYPE 0x01
#define USB_CONFIGURATION_DESCRIPTOR_TYPE 0x02
#define USB_STRING_DESCRIPTOR_TYPE 0x03
#define USB_INTERFACE_DESCRIPTOR_TYPE 0x04
#define USB_ENDPOINT_DESCRIPTOR_TYPE 0x05
#define USB_SIZ_DEVICE_DESC 18
#define USB_SIZ_STRING_LANGID 4
/**
* @}
*/
/** @defgroup USBD_DESC_Exported_TypesDefinitions
* @{
*/
/**
* @}
*/
/** @defgroup USBD_DESC_Exported_Macros
* @{
*/
/**
* @}
*/
/** @defgroup USBD_DESC_Exported_Variables
* @{
*/
extern uint8_t USBD_DeviceDesc [USB_SIZ_DEVICE_DESC];
extern uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ];
extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC];
extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC];
extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID];
extern USBD_DEVICE USR_desc;
/**
* @}
*/
/** @defgroup USBD_DESC_Exported_FunctionsPrototype
* @{
*/
uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_ManufacturerStrDescriptor ( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_ProductStrDescriptor ( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length);
uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length);
#ifdef USB_SUPPORT_USER_STRING_DESC
uint8_t * USBD_USR_USRStringDesc (uint8_t speed, uint8_t idx , uint16_t *length);
#endif /* USB_SUPPORT_USER_STRING_DESC */
/**
* @}
*/
#endif /* __USBD_DESC_H */
/**
* @}
*/
/**
* @}
*/
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
crazyflie-firmware-src/src/hal/interface/usblink.h deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* syslink.h: Implementation of the link between MCU
*/
#ifndef __USBLINK_H__
#define __USBLINK_H__
#include <stdbool.h>
#include "crtp.h"
//#define SYSLINK_MTU 32
#define CRTP_START_BYTE 0xAA
#define SYSLINK_START_BYTE1 0xBC
#define SYSLINK_START_BYTE2 0xCF
// Defined packet types
//#define SYSLINK_RADIO_RAW 0x00
//#define SYSLINK_RADIO_CHANNEL 0x01
//#define SYSLINK_RADIO_DATARATE 0x02
//#define SYSLINK_PM_SOURCE 0x10
//#define SYSLINK_PM_ONOFF_SWITCHOFF 0x11
//#define SYSLINK_PM_BATTERY_VOLTAGE 0x12
//#define SYSLINK_PM_BATTERY_STATE 0x13
//#define SYSLINK_PM_BATTERY_AUTOUPDATE 0x14
//#define SYSLINK_OW_SCAN 0x20
//#define SYSLINK_OW_READ 0x21
/*
typedef struct _SyslinkPacket
{
uint8_t type;
uint8_t length;
char data[SYSLINK_MTU];
} __attribute__((packed)) SyslinkPacket;
typedef enum
{
waitForFirstStart,
waitForSecondStart,
waitForType,
waitForLengt,
waitForData,
waitForChksum1,
waitForChksum2
} SyslinkRxState;
*/
void usblinkInit();
bool usblinkTest();
struct crtpLinkOperations * usblinkGetLink();
#endif
crazyflie-firmware-src/src/hal/interface/usec_time.h deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2013 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* usec_time.h - Microsecond-resolution timer.
*/
#ifndef USEC_TIME_H_
#define USEC_TIME_H_
#include <stdint.h>
/**
* Initialize microsecond-resolution timer (TIM1).
*/
void initUsecTimer(void);
/**
* Get microsecond-resolution timestamp.
*/
uint64_t usecTimestamp(void);
#endif /* USEC_TIME_H_ */
crazyflie-firmware-src/src/hal/src/buzzer.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* buzzer.c - Functions for interfacing with decks with buzzers
*/
#define DEBUG_MODULE "BUZZER"
#include "buzzer.h"
static struct buzzerControl * ctrl;
void buzzerInit()
{
}
bool buzzerTest()
{
return true;
}
void buzzerOff()
{
if (ctrl)
ctrl->off();
}
void buzzerOn(uint32_t freq)
{
if (ctrl)
ctrl->on(freq);
}
void buzzerSetControl(struct buzzerControl * bc)
{
ctrl = bc;
}
crazyflie-firmware-src/src/hal/src/eskylink.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* eskylink.c: esky 2.4GHz-compatible link driver
*/
/*
* Experimental code!
* This link implements the ESky remote protocol using the nRF24L01p chip and
* sends CRTP packets to the commander.
*
* Thanks to 'dvdouden' for documenting the protocol!
* -> http://www.deviationtx.com/forum/protocol-development/1059-esky-protocol?q=/forum/protocol-development/1059-esky-protocol
* -> http://sourceforge.net/p/arduinorclib/wiki/Esky%20Radio/
*/
#include <stdbool.h>
#include <string.h>
#include <errno.h>
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
#include "system.h"
#include "config.h"
#include "nrf24l01.h"
#include "crtp.h"
#include "configblock.h"
#include "ledseq.h"
/* FIXME: This might be a bit tight range? */
#define PPM_ZERO 1500
#define PPM_RANGE 500
#define PPM_MIN 1000
#define PPM_MAX 2000
static bool isInit;
static char address[4] = {0x00, 0x00, 0x00, 0xBB};
static char packet[32];
/* Synchronisation */
xSemaphoreHandle dataRdy;
/* Data queue */
xQueueHandle rxQueue;
static struct {
bool enabled;
bool paired;
uint8_t band;
} state;
static void interruptCallback()
{
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
//To unlock RadioTask
xSemaphoreGiveFromISR(dataRdy, &xHigherPriorityTaskWoken);
if(xHigherPriorityTaskWoken)
portYIELD();
}
// 'Class' functions, called from callbacks
static int setEnable(bool enable)
{
nrfSetEnable(enable);
state.enabled = enable;
return 0;
}
static int sendPacket(CRTPPacket * pk)
{
if (!state.enabled)
return ENETDOWN;
// NOP!
return 0;
}
static int receivePacket(CRTPPacket * pk)
{
if (!state.enabled)
return ENETDOWN;
xQueueReceive( rxQueue, pk, portMAX_DELAY);
return 0;
}
static struct crtpLinkOperations eskyOp =
{
.setEnable = setEnable,
.sendPacket = sendPacket,
.receivePacket = receivePacket,
};
static int eskylinkFetchData(char * packet, int dataLen)
{
nrfSetEnable(false);
//Fetch the data
nrfReadRX(packet, dataLen);
//clear the interruptions flags
nrfWrite1Reg(REG_STATUS, 0x70);
nrfSetEnable(true);
return dataLen;
}
static void eskylinkInitPairing(void)
{
int i;
//Power the radio, Enable the DR interruption, set the radio in PRX mode with 2bytes CRC
nrfWrite1Reg(REG_CONFIG, 0x3F);
vTaskDelay(M2T(2)); //Wait for the chip to be ready
//Set the radio channel, pairing channel is 50
nrfSetChannel(50);
//Set the radio data rate
nrfSetDatarate(RADIO_RATE_1M);
nrfWrite1Reg(REG_SETUP_AW, VAL_SETUP_AW_3B); // 3 bytes address
address[0] = address[1] = address[2] = 0;
nrfWriteReg(REG_RX_ADDR_P0, address, 3); // Pipe address == 0
nrfWrite1Reg(REG_EN_RXADDR, 0x01);
nrfWrite1Reg(REG_FEATURE, 0x00); // No dynamic size payload
nrfWrite1Reg(REG_DYNPD, 0x00);
nrfWrite1Reg(REG_RX_PW_P0, 13); //13 bytes payload
nrfWrite1Reg(REG_EN_AA, 0); //Disable shockburst
//Flush RX
for(i=0;i<3;i++)
nrfFlushRx();
//Flush TX
for(i=0;i<3;i++)
nrfFlushTx();
}
static void eskylinkInitPaired(int channel)
{
nrfSetChannel(channel);
nrfSetDatarate(RADIO_RATE_1M);
nrfWrite1Reg(REG_SETUP_AW, VAL_SETUP_AW_4B); // 4 bytes address
nrfWriteReg(REG_RX_ADDR_P0, address, 4); // Pipe address == from pairing packet
nrfWrite1Reg(REG_EN_RXADDR, 0x01);
nrfWrite1Reg(REG_FEATURE, 0x00); // No dynamic size payload
nrfWrite1Reg(REG_DYNPD, 0x00);
nrfWrite1Reg(REG_RX_PW_P0, 13); //13 bytes payload
nrfWrite1Reg(REG_EN_AA, 0); //Disable shockburst
}
//FIXME: A lot of parameters shall be configurable
static void eskylinkDecode(char* packet)
{
static CRTPPacket crtpPacket;
float pitch, roll, yaw;
uint16_t thrust;
pitch = ((packet[2]<<8) | packet[3])-PPM_ZERO;
if (roll<(-PPM_RANGE)) roll = -PPM_RANGE;
if (roll>PPM_RANGE) roll = PPM_RANGE;
pitch *= 20.0/PPM_RANGE;
roll = ((packet[0]<<8) | packet[1])-PPM_ZERO;
if (roll<(-PPM_RANGE)) roll = -PPM_RANGE;
if (roll>PPM_RANGE) roll = PPM_RANGE;
roll *= 20.0/PPM_RANGE;
yaw = ((packet[6]<<8) | packet[7])-PPM_ZERO;
if (yaw<(-PPM_RANGE)) yaw = -PPM_RANGE;
if (yaw>PPM_RANGE) yaw = PPM_RANGE;
yaw *= 200.0/PPM_RANGE;
thrust = ((packet[4]<<8) | packet[5])-PPM_MIN;
if (thrust<0) thrust = 0;
if (thrust>(2*PPM_RANGE)) thrust = 2*PPM_RANGE;
thrust *= 55000/(2*PPM_RANGE);
crtpPacket.port = CRTP_PORT_SETPOINT;
memcpy(&crtpPacket.data[0], (char*)&roll, 4);
memcpy(&crtpPacket.data[4], (char*)&pitch, 4);
memcpy(&crtpPacket.data[8], (char*)&yaw, 4);
memcpy(&crtpPacket.data[12], (char*)&thrust, 2);
xQueueSend(rxQueue, &crtpPacket, 0);
}
static void eskylinkTask(void * arg)
{
int channel = 7;
int channel1 = -1; //As long as channel1<0 the copter is in scann mode
int channel2 = 0;
//Waiting for pairing packet
while (!state.paired)
{
xSemaphoreTake(dataRdy, portMAX_DELAY);
ledseqRun(LED_GREEN, seq_linkup);
eskylinkFetchData(packet, 13);
if (packet[4]==0x18 && packet[5]==0x29)
{
address[2]=packet[0];
address[1]=packet[1];
address[0]=packet[2];
state.band = packet[3];
state.paired = true;
}
}
ledseqRun(LED_GREEN, seq_testPassed);
nrfSetEnable(false);
eskylinkInitPaired(channel);
nrfSetEnable(true);
//Paired! handling packets.
while(1)
{
if (xSemaphoreTake(dataRdy, M2T(10))==pdTRUE)
{
ledseqRun(LED_GREEN, seq_linkup);
eskylinkFetchData(packet, 13);
eskylinkDecode(packet);
if (channel1<0) //Channels found!
{
channel1 = channel;
channel2 = channel1+37;
if (channel2>83) channel2 = channel1 - 37;
}
}
else
{
if (channel1<0)
{
channel++;
if(channel>83) channel=7;
nrfSetEnable(false);
nrfSetChannel(channel);
nrfSetEnable(true);
}
else
{
if (channel == channel1)
channel = channel2;
else
channel = channel1;
nrfSetEnable(false);
nrfSetChannel(channel);
nrfSetEnable(true);
}
}
}
}
/*
* Public functions
*/
void eskylinkInit()
{
if(isInit)
return;
nrfInit();
nrfSetInterruptCallback(interruptCallback);
//vTaskSetApplicationTaskTag(0, (void*)TASK_RADIO_ID_NBR);
/* Initialise the semaphores */
vSemaphoreCreateBinary(dataRdy);
/* Queue init */
rxQueue = xQueueCreate(3, sizeof(CRTPPacket));
eskylinkInitPairing();
/* Launch the Radio link task */
xTaskCreate(eskylinkTask, ESKYLINK_TASK_NAME,
ESKYLINK_TASK_STACKSIZE, NULL, ESKYLINK_TASK_PRI, NULL);
isInit = true;
}
bool eskylinkTest()
{
return nrfTest();
}
struct crtpLinkOperations * eskylinkGetLink()
{
return &eskyOp;
}
//FIXME: To implement!
void eskylinkReInit(void)
{
;
}
crazyflie-firmware-src/src/hal/src/freeRTOSdebug.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* debug.c - Various debug functions
*/
#include <stdint.h>
#include "FreeRTOSConfig.h"
#include "FreeRTOS.h"
#include "task.h"
#include "config.h"
#include "debug.h"
#include "nvicconf.h"
#include "led.h"
#ifdef UART_OUTPUT_TRACE_DATA
#include "uart.h"
#endif
uint32_t traceTickCount;
void vApplicationMallocFailedHook( void )
{
portDISABLE_INTERRUPTS();
DEBUG_PRINT("\nMalloc failed!\n");
ledSet(ERR_LED1, 1);
ledSet(ERR_LED2, 1);
while(1);
}
#if (configCHECK_FOR_STACK_OVERFLOW == 1)
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName)
{
portDISABLE_INTERRUPTS();
DEBUG_PRINT("\nStack overflow!\n");
ledSet(ERR_LED1, 1);
ledSet(ERR_LED2, 1);
while(1);
}
#endif
#ifdef UART_OUTPUT_TRACE_DATA
void debugSendTraceInfo(unsigned int taskNbr)
{
uint32_t traceData;
traceData = (taskNbr << 29) | (((traceTickCount << 16) + TIM1->CNT) & 0x1FFFFFF);
uartSendDataDma(sizeof(traceData), (uint8_t*)&traceData);
}
void debugInitTrace(void)
{
/*TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
NVIC_InitTypeDef NVIC_InitStructure;
//Enable the Timer
RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
//Timer configuration
TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
TIM_TimeBaseStructure.TIM_Prescaler = 72;
TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
TIM_TimeBaseInit(TIM1, &TIM_TimeBaseStructure);
NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = NVIC_TRACE_TIM_PRI;
NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure);
DBGMCU_Config(DBGMCU_TIM1_STOP, ENABLE);
TIM_ITConfig(TIM1, TIM_IT_Update, ENABLE);
TIM_Cmd(TIM1, ENABLE);
traceTickCount = 0;*/
}
#else
void debugSendTraceInfo(unsigned int taskNbr)
{
}
#endif
crazyflie-firmware-src/src/hal/src/imu_cf1.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
*
* imu.c - inertial measurement unit
*/
#define DEBUG_MODULE "IMU"
#include <math.h>
#include "stm32f10x_conf.h"
#include "FreeRTOS.h"
#include "task.h"
#include "debug.h"
#include "configblock.h"
#include "cfassert.h"
#include "imu.h"
#include "i2cdev.h"
#include "mpu6050.h"
#include "hmc5883l.h"
#include "ms5611.h"
#include "ledseq.h"
#include "uart.h"
#include "param.h"
#define IMU_ENABLE_MAG_HMC5883
#define IMU_ENABLE_PRESSURE_MS5611
//#define IMU_MPU6050_DLPF_256HZ
#define IMU_GYRO_FS_CFG MPU6050_GYRO_FS_2000
#define IMU_DEG_PER_LSB_CFG MPU6050_DEG_PER_LSB_2000
#define IMU_ACCEL_FS_CFG MPU6050_ACCEL_FS_8
#define IMU_G_PER_LSB_CFG MPU6050_G_PER_LSB_8
#define IMU_1G_RAW (int16_t)(1.0 / MPU6050_G_PER_LSB_8)
#define IMU_STARTUP_TIME_MS 1000
#define GYRO_NBR_OF_AXES 3
#define GYRO_X_SIGN (-1)
#define GYRO_Y_SIGN (-1)
#define GYRO_Z_SIGN (-1)
#define GYRO_NBR_OF_AXES 3
#define GYRO_MIN_BIAS_TIMEOUT_MS M2T(1*1000)
#define IMU_NBR_OF_BIAS_SAMPLES 128
#define GYRO_VARIANCE_BASE 4000
#define GYRO_VARIANCE_THRESHOLD_X (GYRO_VARIANCE_BASE)
#define GYRO_VARIANCE_THRESHOLD_Y (GYRO_VARIANCE_BASE)
#define GYRO_VARIANCE_THRESHOLD_Z (GYRO_VARIANCE_BASE)
#define MAG_GAUSS_PER_LSB_CFG HMC5883L_GAIN_660
#define MAG_GAUSS_PER_LSB 660.0
typedef struct
{
Axis3i16 bias;
bool isBiasValueFound;
bool isBufferFilled;
Axis3i16* bufHead;
Axis3i16 buffer[IMU_NBR_OF_BIAS_SAMPLES];
} BiasObj;
static BiasObj gyroBias;
static BiasObj accelBias;
static int32_t varianceSampleTime;
static Axis3i16 gyroMpu;
static Axis3i16 accelMpu;
static Axis3i16 accelLPF;
static Axis3i16 accelLPFAligned;
static Axis3i16 mag;
static Axis3i32 accelStoredFilterValues;
static uint8_t imuAccLpfAttFactor;
static bool isHmc5883lPresent;
static bool isMs5611Present;
static bool isMpu6050TestPassed;
static bool isHmc5883lTestPassed;
static bool isMs5611TestPassed;
// Pre-calculated values for accelerometer alignment
static float cosPitch;
static float sinPitch;
static float cosRoll;
static float sinRoll;
/**
* MPU6050 selt test function. If the chip is moved to much during the self test
* it will cause the test to fail.
*/
static void imuBiasInit(BiasObj* bias);
static void imuCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut);
static void imuCalculateVarianceAndMean(BiasObj* bias, Axis3i32* varOut, Axis3i32* meanOut);
static bool imuFindBiasValue(BiasObj* bias);
static void imuAddBiasValue(BiasObj* bias, Axis3i16* dVal);
static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out,
Axis3i32* storedValues, int32_t attenuation);
static void imuAccAlignToGravity(Axis3i16* in, Axis3i16* out);
static bool isInit;
void imu6Init(void)
{
if(isInit)
return;
isHmc5883lPresent = false;
isMs5611Present = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C1);
mpu6050Init(I2C1);
if (mpu6050TestConnection() == true)
{
DEBUG_PRINT("MPU6050 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU6050 I2C connection [FAIL].\n");
}
mpu6050Reset();
vTaskDelay(M2T(50));
// Activate MPU6050
mpu6050SetSleepEnabled(false);
// Enable temp sensor
mpu6050SetTempSensorEnabled(true);
// Disable interrupts
mpu6050SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6050SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6050SetClockSource(MPU6050_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6050SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6050SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6050_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6050SetRate(15);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6050SetRate(1);
// Set digital low-pass bandwidth
mpu6050SetDLPFMode(MPU6050_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_HMC5883
hmc5883lInit(I2C1);
if (hmc5883lTestConnection() == true)
{
isHmc5883lPresent = true;
DEBUG_PRINT("HMC5883 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("HMC5883L I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_MS5611
if (ms5611Init(I2C1) == true)
{
isMs5611Present = true;
DEBUG_PRINT("MS5611 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MS5611 I2C connection [FAIL].\n");
}
#endif
imuBiasInit(&gyroBias);
imuBiasInit(&accelBias);
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cos(configblockGetCalibPitch() * M_PI/180);
sinPitch = sin(configblockGetCalibPitch() * M_PI/180);
cosRoll = cos(configblockGetCalibRoll() * M_PI/180);
sinRoll = sin(configblockGetCalibRoll() * M_PI/180);
isInit = true;
}
bool imu6Test(void)
{
bool testStatus = true;
if (!isInit)
{
DEBUG_PRINT("Uninitialized\n");
testStatus = false;
}
// Test for CF 10-DOF variant with none responding sensor
if((isHmc5883lPresent && !isMs5611Present) ||
(!isHmc5883lPresent && isMs5611Present))
{
DEBUG_PRINT("HMC5883L or MS5611 is not responding\n");
testStatus = false;
}
if (testStatus)
{
isMpu6050TestPassed = mpu6050SelfTest();
testStatus = isMpu6050TestPassed ;
}
if (testStatus && isHmc5883lPresent)
{
isHmc5883lTestPassed = hmc5883lSelfTest();
testStatus = isHmc5883lTestPassed;
}
if (testStatus && isMs5611Present)
{
isMs5611TestPassed = ms5611SelfTest();
testStatus = isMs5611TestPassed;
}
return testStatus;
}
void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);
imuAddBiasValue(&gyroBias, &gyroMpu);
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
ledseqRun(LED_RED, seq_calibrated);
// uartPrintf("Gyro bias: %i, %i, %i\n",
// gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z);
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - IMU_1G_RAW;
accelBias.isBiasValueFound = true;
//uartPrintf("Accel bias: %i, %i, %i\n",
// accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
(int32_t)imuAccLpfAttFactor);
imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
// Re-map outputs
gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
}
void imu9Read(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut)
{
imu6Read(gyroOut, accOut);
if (isHmc5883lPresent)
{
hmc5883lGetHeading(&mag.x, &mag.y, &mag.z);
magOut->x = (float)mag.x / MAG_GAUSS_PER_LSB;
magOut->y = (float)mag.y / MAG_GAUSS_PER_LSB;
magOut->z = (float)mag.z / MAG_GAUSS_PER_LSB;
}
else
{
magOut->x = 0.0;
magOut->y = 0.0;
magOut->z = 0.0;
}
}
bool imu6IsCalibrated(void)
{
bool status;
status = gyroBias.isBiasValueFound;
#ifdef IMU_TAKE_ACCEL_BIAS
status &= accelBias.isBiasValueFound;
#endif
return status;
}
bool imuHasBarometer(void)
{
return isMs5611Present;
}
bool imuHasMangnetometer(void)
{
return isHmc5883lPresent;
}
static void imuBiasInit(BiasObj* bias)
{
bias->isBufferFilled = false;
bias->bufHead = bias->buffer;
}
/**
* Calculates the variance and mean for the bias buffer.
*/
static void imuCalculateVarianceAndMean(BiasObj* bias, Axis3i32* varOut, Axis3i32* meanOut)
{
uint32_t i;
int32_t sum[GYRO_NBR_OF_AXES] = {0};
int64_t sumSq[GYRO_NBR_OF_AXES] = {0};
for (i = 0; i < IMU_NBR_OF_BIAS_SAMPLES; i++)
{
sum[0] += bias->buffer[i].x;
sum[1] += bias->buffer[i].y;
sum[2] += bias->buffer[i].z;
sumSq[0] += bias->buffer[i].x * bias->buffer[i].x;
sumSq[1] += bias->buffer[i].y * bias->buffer[i].y;
sumSq[2] += bias->buffer[i].z * bias->buffer[i].z;
}
varOut->x = (sumSq[0] - ((int64_t)sum[0] * sum[0]) / IMU_NBR_OF_BIAS_SAMPLES);
varOut->y = (sumSq[1] - ((int64_t)sum[1] * sum[1]) / IMU_NBR_OF_BIAS_SAMPLES);
varOut->z = (sumSq[2] - ((int64_t)sum[2] * sum[2]) / IMU_NBR_OF_BIAS_SAMPLES);
meanOut->x = sum[0] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->y = sum[1] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->z = sum[2] / IMU_NBR_OF_BIAS_SAMPLES;
isInit = true;
}
/**
* Calculates the mean for the bias buffer.
*/
static void __attribute__((used)) imuCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut)
{
uint32_t i;
int32_t sum[GYRO_NBR_OF_AXES] = {0};
for (i = 0; i < IMU_NBR_OF_BIAS_SAMPLES; i++)
{
sum[0] += bias->buffer[i].x;
sum[1] += bias->buffer[i].y;
sum[2] += bias->buffer[i].z;
}
meanOut->x = sum[0] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->y = sum[1] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->z = sum[2] / IMU_NBR_OF_BIAS_SAMPLES;
}
/**
* Adds a new value to the variance buffer and if it is full
* replaces the oldest one. Thus a circular buffer.
*/
static void imuAddBiasValue(BiasObj* bias, Axis3i16* dVal)
{
bias->bufHead->x = dVal->x;
bias->bufHead->y = dVal->y;
bias->bufHead->z = dVal->z;
bias->bufHead++;
if (bias->bufHead >= &bias->buffer[IMU_NBR_OF_BIAS_SAMPLES])
{
bias->bufHead = bias->buffer;
bias->isBufferFilled = true;
}
}
/**
* Checks if the variances is below the predefined thresholds.
* The bias value should have been added before calling this.
* @param bias The bias object
*/
static bool imuFindBiasValue(BiasObj* bias)
{
bool foundBias = false;
if (bias->isBufferFilled)
{
Axis3i32 variance;
Axis3i32 mean;
imuCalculateVarianceAndMean(bias, &variance, &mean);
//uartSendData(sizeof(variance), (uint8_t*)&variance);
//uartSendData(sizeof(mean), (uint8_t*)&mean);
//uartPrintf("%i, %i, %i", variance.x, variance.y, variance.z);
//uartPrintf(" %i, %i, %i\n", mean.x, mean.y, mean.z);
if (variance.x < GYRO_VARIANCE_THRESHOLD_X &&
variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
{
varianceSampleTime = xTaskGetTickCount();
bias->bias.x = mean.x;
bias->bias.y = mean.y;
bias->bias.z = mean.z;
foundBias = true;
bias->isBiasValueFound = true;
}
}
return foundBias;
}
static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out, Axis3i32* storedValues, int32_t attenuation)
{
out->x = iirLPFilterSingle(in->x, attenuation, &storedValues->x);
out->y = iirLPFilterSingle(in->y, attenuation, &storedValues->y);
out->z = iirLPFilterSingle(in->z, attenuation, &storedValues->z);
}
/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
* rotate the accelerometer to be aligned with gravity.
*/
static void imuAccAlignToGravity(Axis3i16* in, Axis3i16* out)
{
Axis3i16 rx;
Axis3i16 ry;
// Rotate around x-axis
rx.x = in->x;
rx.y = in->y * cosRoll - in->z * sinRoll;
rx.z = in->y * sinRoll + in->z * cosRoll;
// Rotate around y-axis
ry.x = rx.x * cosPitch - rx.z * sinPitch;
ry.y = rx.y;
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
out->x = ry.x;
out->y = ry.y;
out->z = ry.z;
}
PARAM_GROUP_START(imu_acc_lpf)
PARAM_ADD(PARAM_UINT8, factor, &imuAccLpfAttFactor)
PARAM_GROUP_STOP(imu_acc_lpf)
PARAM_GROUP_START(imu_sensors)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isHmc5883lPresent)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isMs5611Present)
PARAM_GROUP_STOP(imu_sensors)
PARAM_GROUP_START(imu_tests)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MPU6050, &isMpu6050TestPassed)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isHmc5883lTestPassed)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isMs5611TestPassed)
PARAM_GROUP_STOP(imu_tests)
crazyflie-firmware-src/src/hal/src/imu_cf2.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
*
* imu.c - inertial measurement unit
*/
#define DEBUG_MODULE "IMU"
#include <math.h>
#include "stm32fxxx.h"
#include "FreeRTOS.h"
#include "task.h"
#include "debug.h"
#include "configblock.h"
#include "cfassert.h"
#include "imu.h"
#include "i2cdev.h"
#include "mpu6500.h"
#include "hmc5883l.h"
#include "ms5611.h"
#include "ak8963.h"
#include "lps25h.h"
#include "ledseq.h"
#include "param.h"
#include "log.h"
#include "sound.h"
#define IMU_ENABLE_PRESSURE_LPS25H
#define IMU_ENABLE_MAG_AK8963
//#define IMU_MPU6500_DLPF_256HZ
#define IMU_GYRO_FS_CFG MPU6500_GYRO_FS_2000
#define IMU_DEG_PER_LSB_CFG MPU6500_DEG_PER_LSB_2000
#define IMU_ACCEL_FS_CFG MPU6500_ACCEL_FS_8
#define IMU_G_PER_LSB_CFG MPU6500_G_PER_LSB_8
#define IMU_1G_RAW (int16_t)(1.0f / MPU6500_G_PER_LSB_8)
#define IMU_VARIANCE_MAN_TEST_TIMEOUT M2T(1000) // Timeout in ms
#define IMU_MAN_TEST_LEVEL_MAX 5.0f // Max degrees off
#define MAG_GAUSS_PER_LSB 666.7f
#define IMU_STARTUP_TIME_MS 1000
#define GYRO_NBR_OF_AXES 3
#define GYRO_X_SIGN (-1)
#define GYRO_Y_SIGN (-1)
#define GYRO_Z_SIGN (-1)
#define GYRO_NBR_OF_AXES 3
#define GYRO_MIN_BIAS_TIMEOUT_MS M2T(1*1000)
// Number of samples used in variance calculation. Changing this effects the threshold
#define IMU_NBR_OF_BIAS_SAMPLES 1024
// Variance threshold to take zero bias for gyro
#define GYRO_VARIANCE_BASE 5000
#define GYRO_VARIANCE_THRESHOLD_X (GYRO_VARIANCE_BASE)
#define GYRO_VARIANCE_THRESHOLD_Y (GYRO_VARIANCE_BASE)
#define GYRO_VARIANCE_THRESHOLD_Z (GYRO_VARIANCE_BASE)
typedef struct
{
Axis3f bias;
bool isBiasValueFound;
bool isBufferFilled;
Axis3i16* bufHead;
Axis3i16 buffer[IMU_NBR_OF_BIAS_SAMPLES];
} BiasObj;
BiasObj gyroBias;
#ifdef IMU_TAKE_ACCEL_BIAS
BiasObj accelBias;
#endif
static int32_t varianceSampleTime;
static Axis3i16 gyroMpu;
static Axis3i16 accelMpu;
static Axis3i16 accelLPF;
static Axis3i16 accelLPFAligned;
static Axis3i16 mag;
static Axis3i32 accelStoredFilterValues;
static uint8_t imuAccLpfAttFactor;
static bool isMagPresent;
static bool isBaroPresent;
static bool isMpu6500TestPassed = true;
static bool isAK8963TestPassed = true;
static bool isLPS25HTestPassed = true;
// Pre-calculated values for accelerometer alignment
float cosPitch;
float sinPitch;
float cosRoll;
float sinRoll;
LOG_GROUP_START(mag_raw)
LOG_ADD(LOG_INT16, x, &mag.x)
LOG_ADD(LOG_INT16, y, &mag.y)
LOG_ADD(LOG_INT16, z, &mag.z)
LOG_GROUP_STOP(mag_raw)
/**
* MPU6500 selt test function. If the chip is moved to much during the self test
* it will cause the test to fail.
*/
static void imuBiasInit(BiasObj* bias);
static void imuCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut);
static void imuCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3f* meanOut);
static bool imuFindBiasValue(BiasObj* bias);
static void imuAddBiasValue(BiasObj* bias, Axis3i16* dVal);
static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out,
Axis3i32* storedValues, int32_t attenuation);
static void imuAccAlignToGravity(Axis3i16* in, Axis3i16* out);
static bool isInit;
void imu6Init(void)
{
if(isInit)
return;
isMagPresent = false;
isBaroPresent = false;
// Wait for sensors to startup
while (xTaskGetTickCount() < M2T(IMU_STARTUP_TIME_MS));
i2cdevInit(I2C3_DEV);
mpu6500Init(I2C3_DEV);
if (mpu6500TestConnection() == true)
{
DEBUG_PRINT("MPU9250 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n");
}
mpu6500Reset();
vTaskDelay(M2T(50));
// Activate MPU6500
mpu6500SetSleepEnabled(false);
// Enable temp sensor
mpu6500SetTempSensorEnabled(true);
// Disable interrupts
mpu6500SetIntEnabled(false);
// Connect the HMC5883L to the main I2C bus
mpu6500SetI2CBypassEnabled(true);
// Set x-axis gyro as clock source
mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO);
// Set gyro full scale range
mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG);
// Set accelerometer full scale range
mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG);
#ifdef IMU_MPU6500_DLPF_256HZ
// 256Hz digital low-pass filter only works with little vibrations
// Set output rate (15): 8000 / (1 + 15) = 500Hz
mpu6500SetRate(15);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_256);
#else
// To low DLPF bandwidth might cause instability and decrease agility
// but it works well for handling vibrations and unbalanced propellers
// Set output rate (1): 1000 / (1 + 1) = 500Hz
mpu6500SetRate(1);
// Set digital low-pass bandwidth
mpu6500SetDLPFMode(MPU6500_DLPF_BW_98);
#endif
#ifdef IMU_ENABLE_MAG_AK8963
ak8963Init(I2C3_DEV);
if (ak8963TestConnection() == true)
{
isMagPresent = true;
ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz
DEBUG_PRINT("AK8963 I2C connection [OK].\n");
}
else
{
DEBUG_PRINT("AK8963 I2C connection [FAIL].\n");
}
#endif
#ifdef IMU_ENABLE_PRESSURE_LPS25H
lps25hInit(I2C3_DEV);
if (lps25hTestConnection() == true)
{
lps25hSetEnabled(true);
isBaroPresent = true;
DEBUG_PRINT("LPS25H I2C connection [OK].\n");
}
else
{
//TODO: Should sensor test fail hard if no connection
DEBUG_PRINT("LPS25H I2C connection [FAIL].\n");
}
#endif
imuBiasInit(&gyroBias);
#ifdef IMU_TAKE_ACCEL_BIAS
imuBiasInit(&accelBias);
#endif
varianceSampleTime = -GYRO_MIN_BIAS_TIMEOUT_MS + 1;
imuAccLpfAttFactor = IMU_ACC_IIR_LPF_ATT_FACTOR;
cosPitch = cosf(configblockGetCalibPitch() * (float) M_PI/180);
sinPitch = sinf(configblockGetCalibPitch() * (float) M_PI/180);
cosRoll = cosf(configblockGetCalibRoll() * (float) M_PI/180);
sinRoll = sinf(configblockGetCalibRoll() * (float) M_PI/180);
isInit = true;
}
bool imu6Test(void)
{
bool testStatus = true;
if (!isInit)
{
DEBUG_PRINT("Uninitialized\n");
testStatus = false;
}
#ifdef IMU_ENABLE_MAG_AK8963
testStatus &= isMagPresent;
if (testStatus)
{
isAK8963TestPassed = ak8963SelfTest();
testStatus = isAK8963TestPassed;
}
#endif
#ifdef IMU_ENABLE_PRESSURE_LPS25H
testStatus &= isBaroPresent;
if (testStatus)
{
isLPS25HTestPassed = lps25hSelfTest();
testStatus = isLPS25HTestPassed;
}
#endif
return testStatus;
}
bool imu6ManufacturingTest(void)
{
bool testStatus = false;
Axis3f gyro; // Gyro axis data in deg/s
Axis3f acc; // Accelerometer axis data in mG
float pitch, roll;
uint32_t startTick = xTaskGetTickCount();
testStatus = mpu6500SelfTest();
if (testStatus)
{
while (xTaskGetTickCount() - startTick < IMU_VARIANCE_MAN_TEST_TIMEOUT)
{
imu6Read(&gyro, &acc);
if (gyroBias.isBiasValueFound)
{
DEBUG_PRINT("Gyro variance test [OK]\n");
break;
}
}
if (gyroBias.isBiasValueFound)
{
// Calculate pitch and roll based on accelerometer. Board must be level
pitch = tanf(-acc.x/(sqrtf(acc.y*acc.y + acc.z*acc.z))) * 180/(float) M_PI;
roll = tanf(acc.y/acc.z) * 180/(float) M_PI;
if ((fabsf(roll) < IMU_MAN_TEST_LEVEL_MAX) && (fabsf(pitch) < IMU_MAN_TEST_LEVEL_MAX))
{
DEBUG_PRINT("Acc level test [OK]\n");
testStatus = true;
}
else
{
DEBUG_PRINT("Acc level test Roll:%0.2f, Pitch:%0.2f [FAIL]\n", roll, pitch);
testStatus = false;
}
}
else
{
DEBUG_PRINT("Gyro variance test [FAIL]\n");
testStatus = false;
}
}
return testStatus;
}
void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
mpu6500GetMotion6(&accelMpu.y, &accelMpu.x, &accelMpu.z, &gyroMpu.y, &gyroMpu.x, &gyroMpu.z);
imuAddBiasValue(&gyroBias, &gyroMpu);
#ifdef IMU_TAKE_ACCEL_BIAS
if (!accelBias.isBiasValueFound)
{
imuAddBiasValue(&accelBias, &accelMpu);
}
#endif
if (!gyroBias.isBiasValueFound)
{
imuFindBiasValue(&gyroBias);
if (gyroBias.isBiasValueFound)
{
soundSetEffect(SND_CALIB);
ledseqRun(SYS_LED, seq_calibrated);
}
}
#ifdef IMU_TAKE_ACCEL_BIAS
if (gyroBias.isBiasValueFound &&
!accelBias.isBiasValueFound)
{
Axis3i32 mean;
imuCalculateBiasMean(&accelBias, &mean);
accelBias.bias.x = mean.x;
accelBias.bias.y = mean.y;
accelBias.bias.z = mean.z - IMU_1G_RAW;
accelBias.isBiasValueFound = true;
}
#endif
imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
(int32_t)imuAccLpfAttFactor);
imuAccAlignToGravity(&accelLPF, &accelLPFAligned);
// Re-map outputs
gyroOut->x = -(gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
#ifdef IMU_TAKE_ACCEL_BIAS
accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
#else
accOut->x = -(accelLPFAligned.x) * IMU_G_PER_LSB_CFG;
accOut->y = (accelLPFAligned.y) * IMU_G_PER_LSB_CFG;
accOut->z = (accelLPFAligned.z) * IMU_G_PER_LSB_CFG;
#endif
}
bool imu6IsCalibrated(void)
{
bool status;
status = gyroBias.isBiasValueFound;
#ifdef IMU_TAKE_ACCEL_BIAS
status &= accelBias.isBiasValueFound;
#endif
return status;
}
void imu9Read(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut)
{
imu6Read(gyroOut, accOut);
if (isMagPresent)
{
if (ak8963GetDataReady() == true) {
ak8963GetHeading(&mag.x, &mag.y, &mag.z);
ak8963GetOverflowStatus();
magOut->x = (float)mag.x / MAG_GAUSS_PER_LSB;
magOut->y = (float)mag.y / MAG_GAUSS_PER_LSB;
magOut->z = (float)mag.z / MAG_GAUSS_PER_LSB;
}
}
else
{
magOut->x = 0.0;
magOut->y = 0.0;
magOut->z = 0.0;
}
}
bool imuHasBarometer(void)
{
return isBaroPresent;
}
bool imuHasMangnetometer(void)
{
return isMagPresent;
}
static void imuBiasInit(BiasObj* bias)
{
bias->isBufferFilled = false;
bias->bufHead = bias->buffer;
}
/**
* Calculates the variance and mean for the bias buffer.
*/
static void imuCalculateVarianceAndMean(BiasObj* bias, Axis3f* varOut, Axis3f* meanOut)
{
uint32_t i;
int64_t sum[GYRO_NBR_OF_AXES] = {0};
int64_t sumSq[GYRO_NBR_OF_AXES] = {0};
for (i = 0; i < IMU_NBR_OF_BIAS_SAMPLES; i++)
{
sum[0] += bias->buffer[i].x;
sum[1] += bias->buffer[i].y;
sum[2] += bias->buffer[i].z;
sumSq[0] += bias->buffer[i].x * bias->buffer[i].x;
sumSq[1] += bias->buffer[i].y * bias->buffer[i].y;
sumSq[2] += bias->buffer[i].z * bias->buffer[i].z;
}
varOut->x = (sumSq[0] - ((int64_t)sum[0] * sum[0]) / IMU_NBR_OF_BIAS_SAMPLES);
varOut->y = (sumSq[1] - ((int64_t)sum[1] * sum[1]) / IMU_NBR_OF_BIAS_SAMPLES);
varOut->z = (sumSq[2] - ((int64_t)sum[2] * sum[2]) / IMU_NBR_OF_BIAS_SAMPLES);
meanOut->x = (float)sum[0] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->y = (float)sum[1] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->z = (float)sum[2] / IMU_NBR_OF_BIAS_SAMPLES;
isInit = true;
}
/**
* Calculates the mean for the bias buffer.
*/
static void __attribute__((used)) imuCalculateBiasMean(BiasObj* bias, Axis3i32* meanOut)
{
uint32_t i;
int32_t sum[GYRO_NBR_OF_AXES] = {0};
for (i = 0; i < IMU_NBR_OF_BIAS_SAMPLES; i++)
{
sum[0] += bias->buffer[i].x;
sum[1] += bias->buffer[i].y;
sum[2] += bias->buffer[i].z;
}
meanOut->x = sum[0] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->y = sum[1] / IMU_NBR_OF_BIAS_SAMPLES;
meanOut->z = sum[2] / IMU_NBR_OF_BIAS_SAMPLES;
}
/**
* Adds a new value to the variance buffer and if it is full
* replaces the oldest one. Thus a circular buffer.
*/
static void imuAddBiasValue(BiasObj* bias, Axis3i16* dVal)
{
bias->bufHead->x = dVal->x;
bias->bufHead->y = dVal->y;
bias->bufHead->z = dVal->z;
bias->bufHead++;
if (bias->bufHead >= &bias->buffer[IMU_NBR_OF_BIAS_SAMPLES])
{
bias->bufHead = bias->buffer;
bias->isBufferFilled = true;
}
}
/**
* Checks if the variances is below the predefined thresholds.
* The bias value should have been added before calling this.
* @param bias The bias object
*/
static bool imuFindBiasValue(BiasObj* bias)
{
bool foundBias = false;
if (bias->isBufferFilled)
{
Axis3f variance;
Axis3f mean;
imuCalculateVarianceAndMean(bias, &variance, &mean);
if (variance.x < GYRO_VARIANCE_THRESHOLD_X &&
variance.y < GYRO_VARIANCE_THRESHOLD_Y &&
variance.z < GYRO_VARIANCE_THRESHOLD_Z &&
(varianceSampleTime + GYRO_MIN_BIAS_TIMEOUT_MS < xTaskGetTickCount()))
{
varianceSampleTime = xTaskGetTickCount();
bias->bias.x = mean.x;
bias->bias.y = mean.y;
bias->bias.z = mean.z;
foundBias = true;
bias->isBiasValueFound = true;
}
}
return foundBias;
}
static void imuAccIIRLPFilter(Axis3i16* in, Axis3i16* out, Axis3i32* storedValues, int32_t attenuation)
{
out->x = iirLPFilterSingle(in->x, attenuation, &storedValues->x);
out->y = iirLPFilterSingle(in->y, attenuation, &storedValues->y);
out->z = iirLPFilterSingle(in->z, attenuation, &storedValues->z);
}
/**
* Compensate for a miss-aligned accelerometer. It uses the trim
* data gathered from the UI and written in the config-block to
* rotate the accelerometer to be aligned with gravity.
*/
static void imuAccAlignToGravity(Axis3i16* in, Axis3i16* out)
{
Axis3i16 rx;
Axis3i16 ry;
// Rotate around x-axis
rx.x = in->x;
rx.y = in->y * cosRoll - in->z * sinRoll;
rx.z = in->y * sinRoll + in->z * cosRoll;
// Rotate around y-axis
ry.x = rx.x * cosPitch - rx.z * sinPitch;
ry.y = rx.y;
ry.z = -rx.x * sinPitch + rx.z * cosPitch;
out->x = ry.x;
out->y = ry.y;
out->z = ry.z;
}
PARAM_GROUP_START(imu_acc_lpf)
PARAM_ADD(PARAM_UINT8, factor, &imuAccLpfAttFactor)
PARAM_GROUP_STOP(imu_acc_lpf)
PARAM_GROUP_START(imu_sensors)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isMagPresent)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isBaroPresent) // TODO: Rename MS5611 to LPS25H. Client needs to be updated at the same time.
PARAM_GROUP_STOP(imu_sensors)
PARAM_GROUP_START(imu_tests)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MPU6500, &isMpu6500TestPassed)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, HMC5883L, &isAK8963TestPassed)
PARAM_ADD(PARAM_UINT8 | PARAM_RONLY, MS5611, &isLPS25HTestPassed) // TODO: Rename MS5611 to LPS25H. Client needs to be updated at the same time.
PARAM_GROUP_STOP(imu_tests)
crazyflie-firmware-src/src/hal/src/ledseq.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*/
/*
* ledseq.c - LED sequence handler
*/
#include <stdbool.h>
#include "ledseq.h"
#include "FreeRTOS.h"
#include "timers.h"
#include "semphr.h"
#include "led.h"
#ifdef CALIBRATED_LED_MORSE
#define DOT 100
#define DASH (3 * DOT)
#define GAP DOT
#define LETTER_GAP (3 * DOT)
#define WORD_GAP (7 * DOT)
#endif // #ifdef CALIBRATED_LED_MORSE
/* Led sequence priority */
static ledseq_t const * sequences[] = {
seq_testPassed,
seq_lowbat,
seq_charged,
seq_charging,
seq_chargingMax,
seq_bootloader,
seq_armed,
seq_calibrated,
seq_alive,
seq_linkup,
};
/* Led sequences */
const ledseq_t seq_lowbat[] = {
{ true, LEDSEQ_WAITMS(1000)},
{ 0, LEDSEQ_LOOP},
};
const ledseq_t seq_armed[] = {
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(250)},
{ 0, LEDSEQ_LOOP},
};
const ledseq_t seq_calibrated[] = {
#ifndef CALIBRATED_LED_MORSE
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(450)},
{ 0, LEDSEQ_LOOP},
#else
{ true, LEDSEQ_WAITMS(DASH)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DOT)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DASH)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DOT)},
{false, LEDSEQ_WAITMS(LETTER_GAP)},
{ true, LEDSEQ_WAITMS(DOT)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DOT)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DASH)},
{false, LEDSEQ_WAITMS(GAP)},
{ true, LEDSEQ_WAITMS(DOT)},
{false, LEDSEQ_WAITMS(WORD_GAP)},
{ 0, LEDSEQ_LOOP},
#endif // ifndef CALIBRATED_LED_MORSE
};
const ledseq_t seq_alive[] = {
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(1950)},
{ 0, LEDSEQ_LOOP},
};
//TODO: Change, right now is called so fast it looks like seq_lowbat
const ledseq_t seq_altHold[] = {
{ true, LEDSEQ_WAITMS(1)},
{false, LEDSEQ_WAITMS(50)},
{ 0, LEDSEQ_STOP},
};
const ledseq_t seq_linkup[] = {
{ true, LEDSEQ_WAITMS(1)},
{false, LEDSEQ_WAITMS(0)},
{ 0, LEDSEQ_STOP},
};
const ledseq_t seq_charged[] = {
{ true, LEDSEQ_WAITMS(1000)},
{ 0, LEDSEQ_LOOP},
};
ledseq_t seq_charging[] = {
{ true, LEDSEQ_WAITMS(200)},
{false, LEDSEQ_WAITMS(800)},
{ 0, LEDSEQ_LOOP},
};
ledseq_t seq_chargingMax[] = {
{ true, LEDSEQ_WAITMS(100)},
{false, LEDSEQ_WAITMS(400)},
{ 0, LEDSEQ_LOOP},
};
const ledseq_t seq_bootloader[] = {
{ true, LEDSEQ_WAITMS(500)},
{false, LEDSEQ_WAITMS(500)},
{ 0, LEDSEQ_LOOP},
};
const ledseq_t seq_testPassed[] = {
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{ true, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_WAITMS(50)},
{false, LEDSEQ_STOP},
};
/* Led sequence handling machine implementation */
#define SEQ_NUM (sizeof(sequences)/sizeof(sequences[0]))
static void runLedseq(xTimerHandle xTimer);
static int getPrio(const ledseq_t *seq);
static void updateActive(led_t led);
//State of every sequence for every led: LEDSEQ_STOP if stopped or the current
//step
static int state[LED_NUM][SEQ_NUM];
//Active sequence for each led
static int activeSeq[LED_NUM];
static xTimerHandle timer[LED_NUM];
static xSemaphoreHandle ledseqSem;
static bool isInit = false;
static bool ledseqEnabled = false;
void ledseqInit()
{
int i,j;
if(isInit)
return;
ledInit();
//Initialise the sequences state
for(i=0; i<LED_NUM; i++) {
activeSeq[i] = LEDSEQ_STOP;
for(j=0; j<SEQ_NUM; j++)
state[i][j] = LEDSEQ_STOP;
}
//Init the soft timers that runs the led sequences for each leds
for(i=0; i<LED_NUM; i++)
timer[i] = xTimerCreate("ledseqTimer", M2T(1000), pdFALSE, (void*)i, runLedseq);
vSemaphoreCreateBinary(ledseqSem);
isInit = true;
}
bool ledseqTest(void)
{
bool status;
status = isInit & ledTest();
ledseqEnable(true);
return status;
}
void ledseqEnable(bool enable)
{
ledseqEnabled = enable;
}
void ledseqRun(led_t led, const ledseq_t *sequence)
{
int prio = getPrio(sequence);
if(prio<0) return;
xSemaphoreTake(ledseqSem, portMAX_DELAY);
state[led][prio] = 0; //Reset the seq. to its first step
updateActive(led);
xSemaphoreGive(ledseqSem);
//Run the first step if the new seq is the active sequence
if(activeSeq[led] == prio)
runLedseq(timer[led]);
}
void ledseqSetTimes(ledseq_t *sequence, int32_t onTime, int32_t offTime)
{
sequence[0].action = onTime;
sequence[1].action = offTime;
}
void ledseqStop(led_t led, const ledseq_t *sequence)
{
int prio = getPrio(sequence);
if(prio<0) return;
xSemaphoreTake(ledseqSem, portMAX_DELAY);
state[led][prio] = LEDSEQ_STOP; //Stop the seq.
updateActive(led);
xSemaphoreGive(ledseqSem);
//Run the next active sequence (if any...)
runLedseq(timer[led]);
}
/* Center of the led sequence machine. This function is executed by the FreeRTOS
* timer and runs the sequences
*/
static void runLedseq( xTimerHandle xTimer )
{
led_t led = (led_t)pvTimerGetTimerID(xTimer);
const ledseq_t *step;
bool leave=false;
if (!ledseqEnabled)
return;
while(!leave) {
int prio = activeSeq[led];
if (prio == LEDSEQ_STOP)
return;
step = &sequences[prio][state[led][prio]];
state[led][prio]++;
xSemaphoreTake(ledseqSem, portMAX_DELAY);
switch(step->action)
{
case LEDSEQ_LOOP:
state[led][prio] = 0;
break;
case LEDSEQ_STOP:
state[led][prio] = LEDSEQ_STOP;
updateActive(led);
break;
default: //The step is a LED action and a time
ledSet(led, step->value);
if (step->action == 0)
break;
xTimerChangePeriod(xTimer, M2T(step->action), 0);
xTimerStart(xTimer, 0);
leave=true;
break;
}
xSemaphoreGive(ledseqSem);
}
}
//Utility functions
static int getPrio(const ledseq_t *seq)
{
int prio;
//Find the priority of the sequence
for(prio=0; prio<SEQ_NUM; prio++)
if(sequences[prio]==seq) return prio;
return -1; //Invalid sequence
}
static void updateActive(led_t led)
{
int prio;
activeSeq[led]=LEDSEQ_STOP;
ledSet(led, false);
for(prio=0;prio<SEQ_NUM;prio++)
{
if (state[led][prio] != LEDSEQ_STOP)
{
activeSeq[led]=prio;
break;
}
}
}
crazyflie-firmware-src/src/hal/src/nrf24link.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2012 BitCraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* nrf24link.c: nRF24L01 implementation of the CRTP link
*/
#include <stdbool.h>
#include <errno.h>
#include "config.h"
#include "nrf24l01.h"
#include "crtp.h"
#include "configblock.h"
#include "ledseq.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
static bool isInit;
#define RADIO_CONNECTED_TIMEOUT M2T(2000)
/* Synchronisation */
xSemaphoreHandle dataRdy;
/* Data queue */
xQueueHandle txQueue;
xQueueHandle rxQueue;
static uint32_t lastPacketTick;
//Union used to efficiently handle the packets (Private type)
typedef union
{
CRTPPacket crtp;
struct {
uint8_t size;
uint8_t data[32];
} __attribute__((packed)) raw;
} RadioPacket;
static struct {
bool enabled;
} state;
static void interruptCallback()
{
portBASE_TYPE xHigherPriorityTaskWoken = pdFALSE;
//To unlock RadioTask
xSemaphoreGiveFromISR(dataRdy, &xHigherPriorityTaskWoken);
if(xHigherPriorityTaskWoken)
portYIELD();
}
// 'Class' functions, called from callbacks
static int setEnable(bool enable)
{
nrfSetEnable(enable);
state.enabled = enable;
return 0;
}
static int sendPacket(CRTPPacket * pk)
{
if (!state.enabled)
return ENETDOWN;
if (xQueueSend(txQueue, pk, M2T(100)) == pdTRUE)
{
return true;
}
return false;
}
static int receivePacket(CRTPPacket * pk)
{
if (!state.enabled)
return ENETDOWN;
xQueueReceive( rxQueue, pk, portMAX_DELAY);
return 0;
}
static int reset(void)
{
xQueueReset(txQueue);
nrfFlushTx();
return 0;
}
static bool isConnected(void)
{
if ((xTaskGetTickCount() - lastPacketTick) > RADIO_CONNECTED_TIMEOUT)
return false;
return true;
}
static struct crtpLinkOperations radioOp =
{
.setEnable = setEnable,
.sendPacket = sendPacket,
.receivePacket = receivePacket,
.isConnected = isConnected,
.reset = reset,
};
/* Radio task handles the CRTP packet transfers as well as the radio link
* specific communications (eg. Scann and ID ports, communication error handling
* and so much other cool things that I don't have time for it ...)
*/
static void nrf24linkTask(void * arg)
{
unsigned char dataLen;
static RadioPacket pk;
//Packets handling loop
while(1)
{
ledseqRun(LED_GREEN, seq_linkup);
xSemaphoreTake(dataRdy, portMAX_DELAY);
lastPacketTick = xTaskGetTickCount();
nrfSetEnable(false);
//Fetch all the data (Loop until the RX Fifo is NOT empty)
while( !(nrfRead1Reg(REG_FIFO_STATUS)&0x01) )
{
dataLen = nrfRxLength(0);
if (dataLen>32) //If a packet has a wrong size it is dropped
nrfFlushRx();
else //Else, it is processed
{
//Fetch the data
pk.raw.size = dataLen-1;
nrfReadRX((char *)pk.raw.data, dataLen);
//Push it in the queue (If overflow, the packet is dropped)
if (!CRTP_IS_NULL_PACKET(pk.crtp)) //Don't follow the NULL packets
xQueueSend( rxQueue, &pk, 0);
}
}
//Push the data to send (Loop until the TX Fifo is full or there is no more data to send)
while( (uxQueueMessagesWaiting((xQueueHandle)txQueue) > 0) && !(nrfRead1Reg(REG_FIFO_STATUS)&0x20) )
{
xQueueReceive(txQueue, &pk, 0);
pk.raw.size++;
nrfWriteAck(0, (char*) pk.raw.data, pk.raw.size);
}
//clear the interruptions flags
nrfWrite1Reg(REG_STATUS, 0x70);
//Re-enable the radio
nrfSetEnable(true);
}
}
static void nrf24linkInitNRF24L01P(void)
{
int i;
char radioAddress[5] = {0xE7, 0xE7, 0xE7, 0xE7, 0xE7};
//Set the radio channel
nrfSetChannel(configblockGetRadioChannel());
//Set the radio data rate
nrfSetDatarate(configblockGetRadioSpeed());
//Set radio address
nrfSetAddress(0, radioAddress);
//Power the radio, Enable the DS interruption, set the radio in PRX mode
nrfWrite1Reg(REG_CONFIG, 0x3F);
vTaskDelay(M2T(2)); //Wait for the chip to be ready
// Enable the dynamic payload size and the ack payload for the pipe 0
nrfWrite1Reg(REG_FEATURE, 0x06);
nrfWrite1Reg(REG_DYNPD, 0x01);
//Flush RX
for(i=0;i<3;i++)
nrfFlushRx();
//Flush TX
for(i=0;i<3;i++)
nrfFlushTx();
}
/*
* Public functions
*/
void nrf24linkInit()
{
if(isInit)
return;
nrfInit();
nrfSetInterruptCallback(interruptCallback);
vTaskSetApplicationTaskTag(0, (void*)TASK_RADIO_ID_NBR);
/* Initialise the semaphores */
vSemaphoreCreateBinary(dataRdy);
/* Queue init */
rxQueue = xQueueCreate(3, sizeof(RadioPacket));
txQueue = xQueueCreate(3, sizeof(RadioPacket));
nrf24linkInitNRF24L01P();
/* Launch the Radio link task */
xTaskCreate(nrf24linkTask, NRF24LINK_TASK_NAME,
NRF24LINK_TASK_STACKSIZE, NULL, NRF24LINK_TASK_PRI, NULL);
isInit = true;
}
bool nrf24linkTest()
{
return nrfTest();
}
struct crtpLinkOperations * nrf24linkGetLink()
{
return &radioOp;
}
void nrf24linkReInit(void)
{
if (!isInit)
return;
nrf24linkInitNRF24L01P();
}
crazyflie-firmware-src/src/hal/src/ow_none.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* ow.c - One-wire functions
*/
#define DEBUG_MODULE "OW"
#include <string.h>
#include "FreeRTOS.h"
#include "semphr.h"
#include "ow.h"
void owInit()
{
}
bool owTest()
{
return true;
}
void owSyslinkRecieve(SyslinkPacket *slp)
{
}
bool owScan(uint8_t *nMem)
{
return true;
}
bool owGetinfo(uint8_t selectMem, OwSerialNum *serialNum)
{
return false;
}
bool owRead(uint8_t selectMem, uint16_t address, uint8_t length, uint8_t *data)
{
return false;
}
bool owWrite(uint8_t selectMem, uint16_t address, uint8_t length, uint8_t *data)
{
return false;
}
crazyflie-firmware-src/src/hal/src/ow_syslink.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* ow.c - One-wire functions
*/
#define DEBUG_MODULE "OW"
#include <string.h>
#include "FreeRTOS.h"
#include "semphr.h"
#include "ow.h"
#include "assert.h"
#include "debug.h"
static xSemaphoreHandle waitForReply;
static xSemaphoreHandle lockCmdBuf;
static OwCommand owCmdBuf;
static bool owDataIsValid;
static bool owSyslinkTransfer(uint8_t type, uint8_t length);
#ifdef OW_WRITE_TEST
static uint8_t bqtestData[] =
{
0xEB, 0x00, 0x00, 0x00, 0x00, 0xBC, 0xFE, 0x3C, 0x00, 0x0D, 0x01, 0x08,
0x62, 0x63, 0x42, 0x51, 0x74, 0x65, 0x73, 0x74, 0x02, 0x01, 0x61, 0x70
};
#endif
void owInit()
{
syslinkInit();
vSemaphoreCreateBinary(waitForReply);
lockCmdBuf = xSemaphoreCreateMutex();
// Put reply semaphore in right state.
xSemaphoreTake(waitForReply, portMAX_DELAY);
}
bool owTest()
{
uint8_t nOwMem = 0;
uint8_t nOwIter = 0;
OwSerialNum sn;
if (owScan(&nOwMem))
{
DEBUG_PRINT("Found %d.\n", nOwMem);
}
else
{
DEBUG_PRINT("Scan [FAILED].\n");
}
for (nOwIter = 0; nOwIter < nOwMem; nOwIter++)
{
if (owGetinfo(nOwIter, &sn))
{
DEBUG_PRINT("Serial 0x%X %X %X %X %X %X %X %X.\n",
sn.type, sn.id[0], sn.id[1], sn.id[2],
sn.id[3], sn.id[4], sn.id[5], sn.crc);
}
else
{
DEBUG_PRINT("Mem:%d Getinfo [FAILED].\n", nOwIter);
}
}
#ifdef OW_READ_TEST
{
static uint8_t testbuf[129];
if (owRead(0, 0, OW_MAX_SIZE, testbuf))
{
for (nOwIter = 0; nOwIter < OW_MAX_SIZE; nOwIter++)
{
consolePrintf("%X ", testbuf[nOwIter]);
testbuf[nOwIter] = nOwIter;
}
consolePrintf("\n");
}
}
#endif
#ifdef OW_WRITE_TEST
if (owWrite(0, 0, sizeof(bqtestData), bqtestData))
{
DEBUG_PRINT("Write [OK].\n");
}
else
{
DEBUG_PRINT("Write [FAIL].\n");
}
#endif
return true;
}
void owSyslinkRecieve(SyslinkPacket *slp)
{
switch (slp->type)
{
case SYSLINK_OW_SCAN:
case SYSLINK_OW_GETINFO:
case SYSLINK_OW_READ:
case SYSLINK_OW_WRITE:
memcpy(&owCmdBuf, slp->data, sizeof(OwCommand));
//DEBUG_PRINT("t:%X n:%d:%X\n", slp->type, owCmdBuf.nmem, owCmdBuf.info.memId[0]);
owDataIsValid = true;
break;
default:
// Unknown reply
owDataIsValid = false;
break;
}
xSemaphoreGive(waitForReply);
}
static bool owSyslinkTransfer(uint8_t type, uint8_t length)
{
SyslinkPacket slp;
ASSERT(length <= SYSLINK_MTU);
slp.type = type;
slp.length = length;
memcpy(slp.data, &owCmdBuf, length);
syslinkSendPacket(&slp);
// Wait for reply
if (xSemaphoreTake(waitForReply, M2T(5000)) == pdTRUE)
//if (xSemaphoreTake(waitForReply, portMAX_DELAY))
{
// We have now got a reply and *owCmd has been filled with data
if (owDataIsValid)
{
owDataIsValid = false;
return true;
}
}
else
{
DEBUG_PRINT("Cmd 0x%X timeout.\n", slp.type);
}
return false;
}
bool owScan(uint8_t *nMem)
{
bool status = false;
xSemaphoreTake(lockCmdBuf, portMAX_DELAY);
if (owSyslinkTransfer(SYSLINK_OW_SCAN, 0))
{
*nMem = owCmdBuf.nmem;
status = true;
}
else
{
status = false;
}
xSemaphoreGive(lockCmdBuf);
return status;
}
bool owGetinfo(uint8_t selectMem, OwSerialNum *serialNum)
{
bool status = false;
xSemaphoreTake(lockCmdBuf, portMAX_DELAY);
owCmdBuf.nmem = selectMem;
if (owSyslinkTransfer(SYSLINK_OW_GETINFO, 1))
{
memcpy(serialNum, owCmdBuf.info.memId, sizeof(OwSerialNum));
if (owCmdBuf.nmem != 0xFF)
{
status = true;
}
}
else
{
status = false;
}
xSemaphoreGive(lockCmdBuf);
return status;
}
bool owRead(uint8_t selectMem, uint16_t address, uint8_t length, uint8_t *data)
{
bool status = true;
uint16_t currAddr = address;
uint16_t endAddr = address + length;
uint8_t bytesRead = 0;
ASSERT(length <= OW_MAX_SIZE);
xSemaphoreTake(lockCmdBuf, portMAX_DELAY);
owCmdBuf.nmem = selectMem;
while (currAddr < endAddr)
{
if (endAddr - currAddr < OW_READ_SIZE)
{
owCmdBuf.read.address = currAddr;
if (owSyslinkTransfer(SYSLINK_OW_READ, 3 + endAddr - currAddr))
{
memcpy(data + bytesRead, owCmdBuf.read.data, endAddr - currAddr);
currAddr += endAddr - currAddr;
bytesRead += endAddr - currAddr;
}
else
{
status = false;
break;
}
}
else // Size is bigger then OW_READ_SIZE
{
owCmdBuf.read.address = currAddr;
if (owSyslinkTransfer(SYSLINK_OW_READ, 3 + OW_READ_SIZE))
{
memcpy(data + bytesRead, owCmdBuf.read.data, OW_READ_SIZE);
currAddr += OW_READ_SIZE;
bytesRead += OW_READ_SIZE;
}
else
{
status = false;
break;
}
}
}
xSemaphoreGive(lockCmdBuf);
return status;
}
bool owWrite(uint8_t selectMem, uint16_t address, uint8_t length, uint8_t *data)
{
bool status = true;
uint16_t currAddr = address;
uint16_t endAddr = address + length;
uint8_t bytesWritten = 0;
ASSERT(length <= OW_MAX_SIZE);
xSemaphoreTake(lockCmdBuf, portMAX_DELAY);
owCmdBuf.nmem = selectMem;
while (currAddr < endAddr)
{
if (endAddr - currAddr < OW_MAX_WRITE_SIZE)
{
owCmdBuf.write.address = currAddr;
owCmdBuf.write.length = endAddr - currAddr;
memcpy(owCmdBuf.write.data, data + bytesWritten, owCmdBuf.write.length);
if (owSyslinkTransfer(SYSLINK_OW_WRITE, 5 + owCmdBuf.write.length))
{
currAddr += endAddr - currAddr;
bytesWritten += endAddr - currAddr;
}
else
{
status = false;
break;
}
}
else // Size is bigger then OW_MAX_WRITE_SIZE
{
owCmdBuf.write.address = currAddr;
owCmdBuf.write.length = OW_MAX_WRITE_SIZE;
memcpy(owCmdBuf.write.data, data + bytesWritten, owCmdBuf.write.length);
if (owSyslinkTransfer(SYSLINK_OW_WRITE, 5 + owCmdBuf.write.length))
{
currAddr += OW_MAX_WRITE_SIZE;
bytesWritten += OW_MAX_WRITE_SIZE;
}
else
{
status = false;
break;
}
}
}
xSemaphoreGive(lockCmdBuf);
return status;
}
crazyflie-firmware-src/src/hal/src/pm_f103.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* pm.c - Power Management driver and functions.
*/
#include "stm32fxxx.h"
#include <string.h>
#include <stdbool.h>
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "config.h"
#include "system.h"
#include "pm.h"
#include "led.h"
#include "log.h"
#include "adc.h"
#include "ledseq.h"
#include "commander.h"
#include "nrf24link.h"
// Power managment pins
#define PM_GPIO_SYSOFF_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_SYSOFF_PORT GPIOA
#define PM_GPIO_SYSOFF GPIO_Pin_1
#define PM_GPIO_EN1_PERIF RCC_APB2Periph_GPIOC
#define PM_GPIO_EN1_PORT GPIOC
#define PM_GPIO_EN1 GPIO_Pin_13
#define PM_GPIO_EN2_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_EN2_PORT GPIOA
#define PM_GPIO_EN2 GPIO_Pin_2
#define PM_GPIO_IN_CHG_PERIF RCC_APB2Periph_GPIOB
#define PM_GPIO_IN_CHG_PORT GPIOB
#define PM_GPIO_IN_CHG GPIO_Pin_2
#define PM_GPIO_IN_PGOOD_PERIF RCC_APB2Periph_GPIOC
#define PM_GPIO_IN_PGOOD_PORT GPIOC
#define PM_GPIO_IN_PGOOD GPIO_Pin_15
// Power managment pins
#define PM_GPIO_BAT_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_BAT_PORT GPIOA
#define PM_GPIO_BAT GPIO_Pin_3
//USB pins to detect adapter or host.
#define PM_GPIO_USB_CON_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_USB_CON_PORT GPIOA
#define PM_GPIO_USB_CON GPIO_Pin_0
#define PM_GPIO_USB_DM_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_USB_DM_PORT GPIOA
#define PM_GPIO_USB_DM GPIO_Pin_11
#define PM_GPIO_USB_DP_PERIF RCC_APB2Periph_GPIOA
#define PM_GPIO_USB_DP_PORT GPIOA
#define PM_GPIO_USB_DP GPIO_Pin_12
static float batteryVoltage;
static float batteryVoltageMin = 6.0;
static float batteryVoltageMax = 0.0;
static int32_t batteryVRawFilt = PM_BAT_ADC_FOR_3_VOLT;
static int32_t batteryVRefRawFilt = PM_BAT_ADC_FOR_1p2_VOLT;
static uint32_t batteryLowTimeStamp;
static uint32_t batteryCriticalLowTimeStamp;
static bool isInit;
static PMStates pmState;
static PMChargeStates pmChargeState;
static void pmSetBatteryVoltage(float voltage);
const static float bat671723HS25C[10] =
{
3.00, // 00%
3.78, // 10%
3.83, // 20%
3.87, // 30%
3.89, // 40%
3.92, // 50%
3.96, // 60%
4.00, // 70%
4.04, // 80%
4.10 // 90%
};
void pmInit(void)
{
GPIO_InitTypeDef GPIO_InitStructure;
if(isInit)
return;
RCC_APB2PeriphClockCmd(PM_GPIO_IN_PGOOD_PERIF | PM_GPIO_IN_CHG_PERIF |
PM_GPIO_SYSOFF_PERIF | PM_GPIO_EN1_PERIF |
PM_GPIO_EN2_PERIF | PM_GPIO_BAT_PERIF, ENABLE);
// Configure PM PGOOD pin (Power good)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = PM_GPIO_IN_PGOOD;
GPIO_Init(PM_GPIO_IN_PGOOD_PORT, &GPIO_InitStructure);
// Configure PM CHG pin (Charge)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Pin = PM_GPIO_IN_CHG;
GPIO_Init(PM_GPIO_IN_CHG_PORT, &GPIO_InitStructure);
// Configure PM EN2 pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_EN2;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(PM_GPIO_EN2_PORT, &GPIO_InitStructure);
// Configure PM EN1 pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_EN1;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(PM_GPIO_EN1_PORT, &GPIO_InitStructure);
// Configure PM SYSOFF pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_SYSOFF;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(PM_GPIO_SYSOFF_PORT, &GPIO_InitStructure);
// Configure battery ADC pin
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_InitStructure.GPIO_Pin = PM_GPIO_BAT;
GPIO_Init(PM_GPIO_BAT_PORT, &GPIO_InitStructure);
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Pin = PM_GPIO_USB_CON;
GPIO_Init(PM_GPIO_USB_CON_PORT, &GPIO_InitStructure);
xTaskCreate(pmTask, PM_TASK_NAME,
PM_TASK_STACKSIZE, NULL, PM_TASK_PRI, NULL);
isInit = true;
pmSetBatteryVoltage(3.7f); //TODO remove
}
bool pmTest(void)
{
return isInit;
}
/**
* Test USB signals for host or power adapter
*/
static PMUSBPower pmTestUSBPower(void)
{
PMUSBPower pmUSBPower = USB500mA;
#ifdef ENABLE_FAST_CHARGE
GPIO_InitTypeDef GPIO_InitStructure;
RCC_APB2PeriphClockCmd(PM_GPIO_USB_DM_PERIF | PM_GPIO_USB_DM_PERIF | PM_GPIO_USB_DP_PERIF, ENABLE);
// Configure USB connect pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_USB_CON;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_Init(PM_GPIO_USB_CON_PORT, &GPIO_InitStructure);
// Configure USB DM pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_USB_DM;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPD;
GPIO_Init(PM_GPIO_USB_DM_PORT, &GPIO_InitStructure);
// Configure USB DP pin
GPIO_InitStructure.GPIO_Pin = PM_GPIO_USB_DP;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IPU;
GPIO_Init(PM_GPIO_USB_DP_PORT, &GPIO_InitStructure);
// Enable 1.5K pull-up for USB DP signal
GPIO_SetBits(PM_GPIO_USB_CON_PORT, PM_GPIO_USB_CON);
// Let the voltage level setle.
vTaskDelay(M2T(1));
// Read the weak pull-down of USB-DM. If it is high, DP and DM are shorted.
if (GPIO_ReadInputDataBit(PM_GPIO_USB_DM_PORT, PM_GPIO_USB_DM) == Bit_SET)
{
pmUSBPower = USBWallAdapter;
}
else
{
pmUSBPower = USB500mA;
}
// Reset USB pins to default
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_10MHz;
GPIO_InitStructure.GPIO_Pin = PM_GPIO_USB_DM | PM_GPIO_USB_CON | PM_GPIO_USB_DP;
GPIO_Init(PM_GPIO_USB_DP_PORT, &GPIO_InitStructure);
#endif
return pmUSBPower;
}
/**
* IIR low pass filter the samples.
*/
static int16_t pmBatteryIIRLPFilter(uint16_t in, int32_t* filt)
{
int32_t inScaled;
int32_t filttmp = *filt;
int16_t out;
// Shift to keep accuracy
inScaled = in << PM_BAT_IIR_SHIFT;
// Calculate IIR filter
filttmp = filttmp + (((inScaled-filttmp) >> 8) * PM_BAT_IIR_LPF_ATT_FACTOR);
// Scale and round
out = (filttmp >> 8) + ((filttmp & (1 << (PM_BAT_IIR_SHIFT - 1))) >> (PM_BAT_IIR_SHIFT - 1));
*filt = filttmp;
return out;
}
/**
* Sets the battery voltage and its min and max values
*/
static void pmSetBatteryVoltage(float voltage)
{
batteryVoltage = voltage;
if (batteryVoltageMax < voltage)
{
batteryVoltageMax = voltage;
}
if (batteryVoltageMin > voltage)
{
batteryVoltageMin = voltage;
}
}
/**
* Shutdown system
*/
static void pmSystemShutdown(void)
{
#ifdef ACTIVATE_AUTO_SHUTDOWN
GPIO_SetBits(PM_GPIO_SYSOFF_PORT, PM_GPIO_SYSOFF);
#endif
}
/**
* Returns a number from 0 to 9 where 0 is completely discharged
* and 9 is 90% charged.
*/
static int32_t pmBatteryChargeFromVoltage(float voltage)
{
int charge = 0;
if (voltage < bat671723HS25C[0])
{
return 0;
}
if (voltage > bat671723HS25C[9])
{
return 9;
}
while (voltage > bat671723HS25C[charge])
{
charge++;
}
return charge;
}
float pmGetBatteryVoltage(void)
{
return batteryVoltage;
}
float pmGetBatteryVoltageMin(void)
{
return batteryVoltageMin;
}
float pmGetBatteryVoltageMax(void)
{
return batteryVoltageMax;
}
void pmBatteryUpdate(AdcGroup* adcValues)
{
float vBat;
int16_t vBatRaw;
int16_t vBatRefRaw;
vBatRaw = pmBatteryIIRLPFilter(adcValues->vbat.val, &batteryVRawFilt);
vBatRefRaw = pmBatteryIIRLPFilter(adcValues->vbat.vref, &batteryVRefRawFilt);
vBat = adcConvertToVoltageFloat(vBatRaw, vBatRefRaw) * PM_BAT_DIVIDER;
pmSetBatteryVoltage(vBat);
}
void pmSetChargeState(PMChargeStates chgState)
{
pmChargeState = chgState;
switch (chgState)
{
case charge100mA:
GPIO_ResetBits(PM_GPIO_EN1_PORT, PM_GPIO_EN1);
GPIO_ResetBits(PM_GPIO_EN2_PORT, PM_GPIO_EN2);
break;
case charge500mA:
GPIO_SetBits(PM_GPIO_EN1_PORT, PM_GPIO_EN1);
GPIO_ResetBits(PM_GPIO_EN2_PORT, PM_GPIO_EN2);
break;
case chargeMax:
GPIO_ResetBits(PM_GPIO_EN1_PORT, PM_GPIO_EN1);
GPIO_SetBits(PM_GPIO_EN2_PORT, PM_GPIO_EN2);
break;
}
}
PMChargeStates pmGetChargeState(void)
{
return pmChargeState;
}
PMStates pmUpdateState()
{
PMStates state;
bool isCharging = !GPIO_ReadInputDataBit(PM_GPIO_IN_CHG_PORT, PM_GPIO_IN_CHG);
bool isPgood = !GPIO_ReadInputDataBit(PM_GPIO_IN_PGOOD_PORT, PM_GPIO_IN_PGOOD);
uint32_t batteryLowTime;
batteryLowTime = xTaskGetTickCount() - batteryLowTimeStamp;
if (isPgood && !isCharging)
{
state = charged;
}
else if (isPgood && isCharging)
{
state = charging;
}
else if (!isPgood && !isCharging && (batteryLowTime > PM_BAT_LOW_TIMEOUT))
{
state = lowPower;
}
else
{
state = battery;
}
return state;
}
// return true if battery discharging
bool pmIsDischarging(void) {
PMStates pmState;
pmState = pmUpdateState();
return (pmState == lowPower )|| (pmState == battery);
}
void pmTask(void *param)
{
PMStates pmStateOld = battery;
uint32_t tickCount;
vTaskSetApplicationTaskTag(0, (void*)TASK_PM_ID_NBR);
tickCount = xTaskGetTickCount();
batteryLowTimeStamp = tickCount;
batteryCriticalLowTimeStamp = tickCount;
pmSetChargeState(charge500mA);
vTaskDelay(1000);
while(1)
{
vTaskDelay(100);
tickCount = xTaskGetTickCount();
if (pmGetBatteryVoltage() > PM_BAT_LOW_VOLTAGE)
{
batteryLowTimeStamp = tickCount;
}
if (pmGetBatteryVoltage() > PM_BAT_CRITICAL_LOW_VOLTAGE)
{
batteryCriticalLowTimeStamp = tickCount;
}
pmState = pmUpdateState();
if (pmState != pmStateOld)
{
// Actions on state change
switch (pmState)
{
case charged:
ledseqStop(LED_GREEN, seq_charging);
ledseqStop(LED_GREEN, seq_chargingMax);
ledseqRun(LED_GREEN, seq_charged);
systemSetCanFly(false);
break;
case charging:
ledseqStop(LED_RED, seq_lowbat);
ledseqStop(LED_GREEN, seq_charged);
if (pmTestUSBPower() == USBWallAdapter)
{
pmSetChargeState(chargeMax);
ledseqRun(LED_GREEN, seq_chargingMax);
}
else
{
pmSetChargeState(charge500mA);
ledseqRun(LED_GREEN, seq_charging);
}
systemSetCanFly(false);
//Due to voltage change radio must be restarted
nrf24linkReInit();
break;
case lowPower:
ledseqRun(LED_RED, seq_lowbat);
systemSetCanFly(true);
break;
case battery:
ledseqStop(LED_GREEN, seq_charging);
ledseqStop(LED_GREEN, seq_charged);
systemSetCanFly(true);
//Due to voltage change radio must be restarted
nrf24linkReInit();
break;
default:
systemSetCanFly(true);
break;
}
pmStateOld = pmState;
}
// Actions during state
switch (pmState)
{
case charged:
break;
case charging:
{
uint32_t onTime;
if (pmGetChargeState() == chargeMax)
{
onTime = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) *
(LEDSEQ_CHARGE_CYCLE_TIME_MAX / 10);
ledseqSetTimes(seq_chargingMax, onTime, LEDSEQ_CHARGE_CYCLE_TIME_MAX - onTime);
}
else
{
onTime = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) *
(LEDSEQ_CHARGE_CYCLE_TIME_500MA / 10);
ledseqSetTimes(seq_charging, onTime, LEDSEQ_CHARGE_CYCLE_TIME_500MA - onTime);
}
}
break;
case lowPower:
{
uint32_t batteryCriticalLowTime;
batteryCriticalLowTime = tickCount - batteryCriticalLowTimeStamp;
if (batteryCriticalLowTime > PM_BAT_CRITICAL_LOW_TIMEOUT)
{
pmSystemShutdown();
}
}
break;
case battery:
{
if ((commanderGetInactivityTime() > PM_SYSTEM_SHUTDOWN_TIMEOUT))
{
pmSystemShutdown();
}
}
break;
default:
break;
}
}
}
LOG_GROUP_START(pm)
LOG_ADD(LOG_FLOAT, vbat, &batteryVoltage)
LOG_ADD(LOG_INT8, state, &pmState)
LOG_GROUP_STOP(pm)
crazyflie-firmware-src/src/hal/src/pm_f405.c deleted 100755 → 0
View file @ 6291d058
/**
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* pm.c - Power Management driver and functions.
*/
#include "stm32fxxx.h"
#include <string.h>
#include <stdbool.h>
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "config.h"
#include "system.h"
#include "pm.h"
#include "led.h"
#include "log.h"
#include "ledseq.h"
#include "commander.h"
#include "sound.h"
#include "deck.h"
typedef struct _PmSyslinkInfo
{
union
{
uint8_t flags;
struct
{
uint8_t pgood : 1;
uint8_t chg : 1;
uint8_t unused : 6;
};
};
float vBat;
float chargeCurrent;
} __attribute__((packed)) PmSyslinkInfo;
static float batteryVoltage;
static uint16_t batteryVoltageMV;
static float batteryVoltageMin = 6.0;
static float batteryVoltageMax = 0.0;
static float extBatteryVoltage;
static uint16_t extBatteryVoltageMV;
static uint8_t extBatVoltDeckPin;
static float extBatVoltMultiplier;
static float extBatteryCurrent;
static uint8_t extBatCurrDeckPin;
static float extBatCurrAmpPerVolt;
static uint32_t batteryLowTimeStamp;
static uint32_t batteryCriticalLowTimeStamp;
static bool isInit;
static PMStates pmState;
static PmSyslinkInfo pmSyslinkInfo;
static void pmSetBatteryVoltage(float voltage);
const static float bat671723HS25C[10] =
{
3.00, // 00%
3.78, // 10%
3.83, // 20%
3.87, // 30%
3.89, // 40%
3.92, // 50%
3.96, // 60%
4.00, // 70%
4.04, // 80%
4.10 // 90%
};
void pmInit(void)
{
if(isInit)
return;
xTaskCreate(pmTask, PM_TASK_NAME,
PM_TASK_STACKSIZE, NULL, PM_TASK_PRI, NULL);
isInit = true;
pmSyslinkInfo.vBat = 3.7f;
pmSetBatteryVoltage(pmSyslinkInfo.vBat); //TODO remove
}
bool pmTest(void)
{
return isInit;
}
/**
* Sets the battery voltage and its min and max values
*/
static void pmSetBatteryVoltage(float voltage)
{
batteryVoltage = voltage;
batteryVoltageMV = (uint16_t)(voltage * 1000);
if (batteryVoltageMax < voltage)
{
batteryVoltageMax = voltage;
}
if (batteryVoltageMin > voltage)
{
batteryVoltageMin = voltage;
}
}
/**
* Shutdown system
*/
static void pmSystemShutdown(void)
{
#ifdef ACTIVATE_AUTO_SHUTDOWN
//TODO: Implement syslink call to shutdown
#endif
}
/**
* Returns a number from 0 to 9 where 0 is completely discharged
* and 9 is 90% charged.
*/
static int32_t pmBatteryChargeFromVoltage(float voltage)
{
int charge = 0;
if (voltage < bat671723HS25C[0])
{
return 0;
}
if (voltage > bat671723HS25C[9])
{
return 9;
}
while (voltage > bat671723HS25C[charge])
{
charge++;
}
return charge;
}
float pmGetBatteryVoltage(void)
{
return batteryVoltage;
}
float pmGetBatteryVoltageMin(void)
{
return batteryVoltageMin;
}
float pmGetBatteryVoltageMax(void)
{
return batteryVoltageMax;
}
void pmSyslinkUpdate(SyslinkPacket *slp)
{
memcpy(&pmSyslinkInfo, &slp->data[0], sizeof(pmSyslinkInfo));
pmSetBatteryVoltage(pmSyslinkInfo.vBat);
}
void pmSetChargeState(PMChargeStates chgState)
{
// TODO: Send syslink packafe with charge state
}
PMStates pmUpdateState()
{
PMStates state;
bool isCharging = pmSyslinkInfo.chg;
bool isPgood = pmSyslinkInfo.pgood;
uint32_t batteryLowTime;
batteryLowTime = xTaskGetTickCount() - batteryLowTimeStamp;
if (isPgood && !isCharging)
{
state = charged;
}
else if (isPgood && isCharging)
{
state = charging;
}
else if (!isPgood && !isCharging && (batteryLowTime > PM_BAT_LOW_TIMEOUT))
{
state = lowPower;
}
else
{
state = battery;
}
return state;
}
void pmEnableExtBatteryCurrMeasuring(uint8_t pin, float ampPerVolt)
{
extBatCurrDeckPin = pin;
extBatCurrAmpPerVolt = ampPerVolt;
}
float pmMeasureExtBatteryCurrent(void)
{
float current;
if (extBatCurrDeckPin)
{
current = analogReadVoltage(extBatCurrDeckPin) * extBatCurrAmpPerVolt;
}
else
{
current = 0.0;
}
return current;
}
void pmEnableExtBatteryVoltMeasuring(uint8_t pin, float multiplier)
{
extBatVoltDeckPin = pin;
extBatVoltMultiplier = multiplier;
}
float pmMeasureExtBatteryVoltage(void)
{
float voltage;
if (extBatVoltDeckPin)
{
voltage = analogReadVoltage(extBatVoltDeckPin) * extBatVoltMultiplier;
}
else
{
voltage = 0.0;
}
return voltage;
}
// return true if battery discharging
bool pmIsDischarging(void) {
PMStates pmState;
pmState = pmUpdateState();
return (pmState == lowPower )|| (pmState == battery);
}
void pmTask(void *param)
{
PMStates pmStateOld = battery;
uint32_t tickCount;
vTaskSetApplicationTaskTag(0, (void*)TASK_PM_ID_NBR);
tickCount = xTaskGetTickCount();
batteryLowTimeStamp = tickCount;
batteryCriticalLowTimeStamp = tickCount;
pmSetChargeState(charge500mA);
vTaskDelay(500);
while(1)
{
vTaskDelay(100);
tickCount = xTaskGetTickCount();
extBatteryVoltage = pmMeasureExtBatteryVoltage();
extBatteryVoltageMV = (uint16_t)(extBatteryVoltage * 1000);
extBatteryCurrent = pmMeasureExtBatteryCurrent();
if (pmGetBatteryVoltage() > PM_BAT_LOW_VOLTAGE)
{
batteryLowTimeStamp = tickCount;
}
if (pmGetBatteryVoltage() > PM_BAT_CRITICAL_LOW_VOLTAGE)
{
batteryCriticalLowTimeStamp = tickCount;
}
pmState = pmUpdateState();
if (pmState != pmStateOld)
{
// Actions on state change
switch (pmState)
{
case charged:
ledseqStop(CHG_LED, seq_charging);
ledseqRun(CHG_LED, seq_charged);
soundSetEffect(SND_BAT_FULL);
systemSetCanFly(false);
break;
case charging:
ledseqStop(LOWBAT_LED, seq_lowbat);
ledseqStop(CHG_LED, seq_charged);
ledseqRun(CHG_LED, seq_charging);
soundSetEffect(SND_USB_CONN);
systemSetCanFly(false);
break;
case lowPower:
ledseqRun(LOWBAT_LED, seq_lowbat);
soundSetEffect(SND_BAT_LOW);
systemSetCanFly(true);
break;
case battery:
ledseqStop(CHG_LED, seq_charging);
ledseqRun(CHG_LED, seq_charged);
soundSetEffect(SND_USB_DISC);
systemSetCanFly(true);
break;
default:
systemSetCanFly(true);
break;
}
pmStateOld = pmState;
}
// Actions during state
switch (pmState)
{
case charged:
break;
case charging:
{
uint32_t onTime;
onTime = pmBatteryChargeFromVoltage(pmGetBatteryVoltage()) *
(LEDSEQ_CHARGE_CYCLE_TIME_500MA / 10);
ledseqSetTimes(seq_charging, onTime, LEDSEQ_CHARGE_CYCLE_TIME_500MA - onTime);
}
break;
case lowPower:
{
uint32_t batteryCriticalLowTime;
batteryCriticalLowTime = tickCount - batteryCriticalLowTimeStamp;
if (batteryCriticalLowTime > PM_BAT_CRITICAL_LOW_TIMEOUT)
{
pmSystemShutdown();
}
}
break;
case battery:
{
if ((commanderGetInactivityTime() > PM_SYSTEM_SHUTDOWN_TIMEOUT))
{
pmSystemShutdown();
}
}
break;
default:
break;
}
}
}
LOG_GROUP_START(pm)
LOG_ADD(LOG_FLOAT, vbat, &batteryVoltage)
LOG_ADD(LOG_UINT16, vbatMV, &batteryVoltageMV)
LOG_ADD(LOG_FLOAT, extVbat, &extBatteryVoltage)
LOG_ADD(LOG_UINT16, extVbatMV, &extBatteryVoltageMV)
LOG_ADD(LOG_FLOAT, extCurr, &extBatteryCurrent)
LOG_ADD(LOG_FLOAT, chargeCurrent, &pmSyslinkInfo.chargeCurrent)
LOG_ADD(LOG_INT8, state, &pmState)
LOG_GROUP_STOP(pm)
crazyflie-firmware-src/src/hal/src/proximity.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2015 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* proximity.c - Implementation of hardware abstraction layer for proximity sensors
*/
#include <string.h>
#include "FreeRTOS.h"
#include "task.h"
#include "config.h"
#include "deck.h"
#include "proximity.h"
#include "maxsonar.h"
#include "system.h"
#include "param.h"
#include "log.h"
#include "stm32fxxx.h"
/* Flag indicating if the proximityInit() function has been called or not. */
static bool isInit = false;
/* Internal values exported by functions below. */
static uint32_t proximityDistance = 0; /* The distance measured in millimeters for the latest sample. */
static uint32_t proximityDistanceAvg = 0; /* Average distance in millimeters, initialized to zero. */
static uint32_t proximityDistanceMedian = 0; /* Median distance in millimeters, initialized to zero. */
static uint32_t proximityAccuracy = 0; /* The accuracy as reported by the sensor driver for the latest sample. */
/* The most recent samples in chronological order. Must be initialized before use. */
static uint32_t proximitySWin[PROXIMITY_SWIN_SIZE];
#if defined(PROXIMITY_ENABLED)
#if defined(PROXIMITY_LOG_ENABLED)
/* Define a log group. */
LOG_GROUP_START(proximity)
LOG_ADD(LOG_UINT32, distance, &proximityDistance)
LOG_ADD(LOG_UINT32, distanceAvg, &proximityDistanceAvg)
LOG_ADD(LOG_UINT32, distanceMed, &proximityDistanceMedian)
LOG_ADD(LOG_UINT32, accuracy, &proximityAccuracy)
LOG_GROUP_STOP(proximity)
#endif
/**
* This function returns the median value of an array.
*
* Internal sorting function by Bill Gentles Nov. 12 2010, seen
* on http://forum.arduino.cc/index.php?topic=20920.0
*
* @param proximitySWin Array of chronologically sequenced samples.
*
* @return Median value from the array.
*/
static uint32_t proximitySWinMedian(uint32_t *proximitySWin)
{
/* The most recent samples, sorted in increasing sample value order. Must be initialized before use. */
uint32_t proximitySorted[PROXIMITY_SWIN_SIZE];
/* Create a copy of the chronologically sequenced buffer. */
memcpy(proximitySorted, proximitySWin, sizeof(uint32_t)*PROXIMITY_SWIN_SIZE);
/* Now sort this copy. */
uint8_t n;
for (n = 1; n < PROXIMITY_SWIN_SIZE; ++n) {
uint32_t valn = proximitySorted[n];
int8_t m; /* May reach value of -1 */
for (m = n - 1; (m >= 0) && (valn < proximitySorted[m]); m--)
{
proximitySorted[m + 1] = proximitySorted[m];
}
proximitySorted[m + 1] = valn;
}
/* Return the median value of the samples. */
return proximitySorted[PROXIMITY_SWIN_SIZE / 2];
}
/**
* This function adds a distance measurement to the sliding window, discarding the oldest sample.
* After having added the new sample, a new average value of the samples is calculated and returned.
*
* @param distance The new sample to add to the sliding window.
*
* @return The new average value of the samples in the sliding window (after adding the new sample).
*/
static uint32_t proximitySWinAdd(uint32_t distance)
{
/* Discard oldest sample, move remaining samples one slot to the left. */
memmove(&proximitySWin[0], &proximitySWin[1], (PROXIMITY_SWIN_SIZE - 1) * sizeof(uint32_t));
/* Add the new sample in the last (right-most) slot. */
proximitySWin[PROXIMITY_SWIN_SIZE - 1] = distance;
/**
* Calculate the new average distance. Sum all the samples into a uint64_t,
* so that we only do a single division at the end.
*/
uint64_t proximityNewAvg = 0;
uint8_t n;
for (n = 0; n < PROXIMITY_SWIN_SIZE; n++) {
proximityNewAvg += proximitySWin[n];
}
proximityNewAvg = proximityNewAvg / PROXIMITY_SWIN_SIZE;
return (uint32_t)proximityNewAvg;
}
/**
* Proximity task running at PROXIMITY_TASK_FREQ Hz.
*
* @param param Currently unused.
*/
static void proximityTask(void* param)
{
uint32_t lastWakeTime;
vTaskSetApplicationTaskTag(0, (void*)TASK_PROXIMITY_ID_NBR);
//Wait for the system to be fully started to start stabilization loop
systemWaitStart();
lastWakeTime = xTaskGetTickCount();
while(1)
{
vTaskDelayUntil(&lastWakeTime, F2T(PROXIMITY_TASK_FREQ));
#if defined(MAXSONAR_ENABLED)
/* Read the MaxBotix sensor. */
proximityDistance = maxSonarReadDistance(MAXSONAR_MB1040_AN, &proximityAccuracy);
#endif
/* Get the latest average value calculated. */
proximityDistanceAvg = proximitySWinAdd(proximityDistance);
/* Get the latest median value calculated. */
proximityDistanceMedian = proximitySWinMedian(proximitySWin);
}
}
#endif
/**
* Initialization of the proximity task.
*/
void proximityInit(void)
{
if(isInit)
return;
/* Initialise the sliding window to zero. */
memset(&proximitySWin, 0, sizeof(uint32_t)*PROXIMITY_SWIN_SIZE);
#if defined(PROXIMITY_ENABLED)
/* Only start the task if the proximity subsystem is enabled in conf.h */
xTaskCreate(proximityTask, PROXIMITY_TASK_NAME,
PROXIMITY_TASK_STACKSIZE, NULL, PROXIMITY_TASK_PRI, NULL);
#endif
isInit = true;
}
/**
* Function returning the last proximity measurement.
*
* @return The last proximity measurement made.
*/
uint32_t proximityGetDistance(void)
{
return proximityDistance;
}
/**
* Function returning the result of the last, average proximity calculation.
* The calculation is a simple average of the last PROXIMITY_SWIN_SIZE samples.
*
* @return The result from the last, average proximity calculation.
*/
uint32_t proximityGetDistanceAvg(void)
{
return proximityDistanceAvg;
}
/**
* Function returning the result of the last, median proximity calculation.
* The calculation is the median of the last PROXIMITY_SWIN_SIZE samples.
*
* @return The result from the last, median proximity calculation.
*/
uint32_t proximityGetDistanceMedian(void)
{
return proximityDistanceMedian;
}
/**
* Function returning the accuracy of the last proximity measurement.
*
* @return The accuracy of the last proximity measurement made.
*/
uint32_t proximityGetAccuracy(void)
{
return proximityAccuracy;
}
crazyflie-firmware-src/src/hal/src/radiolink.c deleted 100755 → 0
View file @ 6291d058
/*
* || ____ _ __
* +------+ / __ )(_) /_______________ _____ ___
* | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \
* +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/
* || || /_____/_/\__/\___/_/ \__,_/ /___/\___/
*
* Crazyflie control firmware
*
* Copyright (C) 2011-2012 Bitcraze AB
*
* This program 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, in version 3.
*
* This program 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 <http://www.gnu.org/licenses/>.
*
* radiolink.c - Radio link layer
*/
#include <string.h>
#include <stdint.h>
/*FreeRtos includes*/
#include "FreeRTOS.h"
#include "task.h"
#include "semphr.h"
#include "queue.h"
#include "config.h"
#include "radiolink.h"
#include "syslink.h"
#include "crtp.h"
#include "configblock.h"
#include "log.h"
#include "led.h"
#include "ledseq.h"
#include "queuemonitor.h"
#define RADIOLINK_TX_QUEUE_SIZE (1)
static xQueueHandle txQueue;
static xQueueHandle crtpPacketDelivery;
static bool isInit;
static int radiolinkSendCRTPPacket(CRTPPacket *p);
static int radiolinkSetEnable(bool enable);
static int radiolinkReceiveCRTPPacket(CRTPPacket *p);
//Local RSSI variable used to enable logging of RSSI values from Radio
static uint8_t rssi;
static struct crtpLinkOperations radiolinkOp =
{
.setEnable = radiolinkSetEnable,
.sendPacket = radiolinkSendCRTPPacket,
.receivePacket = radiolinkReceiveCRTPPacket,
};
void radiolinkInit(void)
{
if (isInit)
return;
txQueue = xQueueCreate(RADIOLINK_TX_QUEUE_SIZE, sizeof(SyslinkPacket));
DEBUG_QUEUE_MONITOR_REGISTER(txQueue);
crtpPacketDelivery = xQueueCreate(5, sizeof(CRTPPacket));
DEBUG_QUEUE_MONITOR_REGISTER(crtpPacketDelivery);
ASSERT(crtpPacketDelivery);
syslinkInit();
radiolinkSetChannel(configblockGetRadioChannel());
radiolinkSetDatarate(configblockGetRadioSpeed());
radiolinkSetAddress(configblockGetRadioAddress());
isInit = true;
}
bool radiolinkTest(void)
{
return syslinkTest();
}
void radiolinkSetChannel(uint8_t channel)
{
SyslinkPacket slp;
slp.type = SYSLINK_RADIO_CHANNEL;
slp.length = 1;
slp.data[0] = channel;
syslinkSendPacket(&slp);
}
void radiolinkSetDatarate(uint8_t datarate)
{
SyslinkPacket slp;
slp.type = SYSLINK_RADIO_DATARATE;
slp.length = 1;
slp.data[0] = datarate;
syslinkSendPacket(&slp);
}
void radiolinkSetAddress(uint64_t address)
{
SyslinkPacket slp;
slp.type = SYSLINK_RADIO_ADDRESS;
slp.length = 5;
memcpy(&slp.data[0], &address, 5);
syslinkSendPacket(&slp);
}
void radiolinkSyslinkDispatch(SyslinkPacket *slp)
{
static SyslinkPacket txPacket;
if (slp->type == SYSLINK_RADIO_RAW)
{
slp->length--; // Decrease to get CRTP size.
xQueueSend(crtpPacketDelivery, &slp->length, 0);
ledseqRun(LINK_LED, seq_linkup);
// If a radio packet is received, one can be sent
if (xQueueReceive(txQueue, &txPacket, 0) == pdTRUE)
{
ledseqRun(LINK_DOWN_LED, seq_linkup);
syslinkSendPacket(&txPacket);
}
} else if (slp->type == SYSLINK_RADIO_RAW_BROADCAST)
{
slp->length--; // Decrease to get CRTP size.
xQueueSend(crtpPacketDelivery, &slp->length, 0);
ledseqRun(LINK_LED, seq_linkup);
// no ack for broadcasts
} else if (slp->type == SYSLINK_RADIO_RSSI)
{
//Extract RSSI sample sent from radio
memcpy(&rssi, slp->data, sizeof(uint8_t));
}
}
static int radiolinkReceiveCRTPPacket(CRTPPacket *p)
{
if (xQueueReceive(crtpPacketDelivery, p, M2T(100)) == pdTRUE)
{
return 0;
}
return -1;
}
static int radiolinkSendCRTPPacket(CRTPPacket *p)
{
static SyslinkPacket slp;
ASSERT(p->size <= CRTP_MAX_DATA_SIZE);
slp.type = SYSLINK_RADIO_RAW;
slp.length = p->size + 1;
memcpy(slp.data, &p->header, p->size + 1);
if (xQueueSend(txQueue, &slp, M2T(100)) == pdTRUE)
{
return true;
}
return false;
}
struct crtpLinkOperations * radiolinkGetLink()
{
return &radiolinkOp;
}
static int radiolinkSetEnable(bool enable)
{
return 0;
}
LOG_GROUP_START(radio)
LOG_ADD(LOG_UINT8, rssi, &rssi)
LOG_GROUP_STOP(radio)