diff --git a/src/main/drivers/accgyro/accgyro_mpu.c b/src/main/drivers/accgyro/accgyro_mpu.c index f1d028f287..e4b050a0d0 100644 --- a/src/main/drivers/accgyro/accgyro_mpu.c +++ b/src/main/drivers/accgyro/accgyro_mpu.c @@ -55,7 +55,9 @@ mpuResetFnPtr mpuResetFn; #define MPU_I2C_INSTANCE I2C_DEVICE #endif +#ifndef MPU_ADDRESS #define MPU_ADDRESS 0x68 +#endif // WHO_AM_I register contents for MPU3050, 6050 and 6500 #define MPU6500_WHO_AM_I_CONST (0x70) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index 79bc765958..4a738ffb11 100644 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -58,6 +58,7 @@ #include "rx/jetiexbus.h" #include "rx/crsf.h" #include "rx/rx_spi.h" +#include "rx/targetcustomserial.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -257,6 +258,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_CRSF: enabled = crsfRxInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_TARGET_CUSTOM + case SERIALRX_TARGET_CUSTOM: + enabled = targetCustomSerialRxInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 1c70389493..e08f5f0720 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -56,6 +56,7 @@ typedef enum { SERIALRX_JETIEXBUS = 8, SERIALRX_CRSF = 9, SERIALRX_SRXL = 10, + SERIALRX_TARGET_CUSTOM = 11, } SerialRXType; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 12 diff --git a/src/main/rx/targetcustomserial.h b/src/main/rx/targetcustomserial.h new file mode 100644 index 0000000000..167b4f8a8d --- /dev/null +++ b/src/main/rx/targetcustomserial.h @@ -0,0 +1,21 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +// Function to be implemented on a per-target basis under src/main/target//serialrx.c +bool targetCustomSerialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig); diff --git a/src/main/target/CRAZYFLIE2/crtp.h b/src/main/target/CRAZYFLIE2/crtp.h new file mode 100644 index 0000000000..4037239085 --- /dev/null +++ b/src/main/target/CRAZYFLIE2/crtp.h @@ -0,0 +1,96 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +/* + * This file contains definitions for the CRTP protocol, the data transport + * protocol used for OTA communication to the crazyflie (via either Nordic ESB or + * Bluetooth LE) and transmitted from the NRF51 to the STM32 via the syslink protocol + * + * For more details, see https://wiki.bitcraze.io/projects:crazyflie:crtp + */ + +#define CRTP_MAX_DATA_SIZE 30 + +typedef enum { + CRTP_PORT_CONSOLE = 0x00, + CRTP_PORT_PARAM = 0x02, + CRTP_PORT_SETPOINT = 0x03, + CRTP_PORT_MEM = 0x04, + CRTP_PORT_LOG = 0x05, + CRTP_PORT_LOCALIZATION = 0x06, + CRTP_PORT_SETPOINT_GENERIC = 0x07, + CRTP_PORT_PLATFORM = 0x0D, + CRTP_PORT_LINK = 0x0F, +} crtpPort_e; + +typedef struct crtpPacket_s +{ + struct{ + uint8_t chan : 2; + uint8_t link : 2; + uint8_t port : 4; + } header; + uint8_t data[CRTP_MAX_DATA_SIZE]; +} __attribute__((packed)) crtpPacket_t; + +typedef enum { + stopType = 0, + velocityWorldType = 1, + zDistanceType = 2, + cppmEmuType = 3, +} crtpCommanderPacketType_e; + +// Legacy RPYT data type for supporting existing clients +// See https://wiki.bitcraze.io/projects:crazyflie:crtp:commander +typedef struct crtpCommanderRPYT_s +{ + float roll; // deg + float pitch; // deg + float yaw; // deg + uint16_t thrust; +} __attribute__((packed)) crtpCommanderRPYT_t; + +// Commander packet type for emulating CPPM-style setpoints +// Corresponds to crtpCommanderPacketType_e::cppmEmuType +#define CRTP_CPPM_EMU_MAX_AUX_CHANNELS 10 +typedef struct crtpCommanderCPPMEmuPacket_s +{ + struct { + uint8_t numAuxChannels : 4; // Set to 0 through CRTP_CPPM_EMU_MAX_AUX_CHANNELS + uint8_t reserved : 4; + } hdr; + uint16_t channelRoll; + uint16_t channelPitch; + uint16_t channelYaw; + uint16_t channelThrust; + uint16_t channelAux[CRTP_CPPM_EMU_MAX_AUX_CHANNELS]; +} __attribute__((packed)) crtpCommanderCPPMEmuPacket_t; + +//typedef struct crtpCommander_s +//{ +// struct{ +// uint8_t chan : 2; +// uint8_t link : 2; +// uint8_t port : 4; +// }hdr; +// uint8_t type; +// uint8_t numChannels; +// uint16_t channels[14]; +// +//} __attribute__((packed)) crtpCommander_t; diff --git a/src/main/target/CRAZYFLIE2/serialrx.c b/src/main/target/CRAZYFLIE2/serialrx.c new file mode 100644 index 0000000000..e698eaf270 --- /dev/null +++ b/src/main/target/CRAZYFLIE2/serialrx.c @@ -0,0 +1,233 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +/* + * This file implements the custom Crazyflie serial Rx protocol which + * consists of CRTP packets sent from the onboard NRF51 over UART using + * the syslink protocol. + * + * The implementation supports two types of commander packets: + * - RPYT on port CRTP_PORT_SETPOINT + * - CPPM emulation on port CRTP_PORT_SETPOINT_GENERIC using type cppmEmuType + * + * The CPPM emulation packet type is recommended for use with this target. + * + * The RPYT type is mainly for legacy support (various mobile apps, python PC client, + * etc) and can be used with the following restrictions to ensure angles are accurately + * translated into PWM values: + * - Max angles for pitch and roll must be set to 50 degrees + * - Max yaw rate must be set to 400 degrees + * + * This implementation has been ported from the Crazyflie source code. + * + * For more information, see the following Crazyflie wiki pages: + * CRTP: https://wiki.bitcraze.io/projects:crazyflie:crtp + * Syslink: https://wiki.bitcraze.io/doc:crazyflie:syslink:index + */ + +#include +#include + +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/targetcustomserial.h" +#include "syslink.h" +#include "crtp.h" + +#define SYSLINK_BAUDRATE 1000000 + +// Variables for the Syslink Rx state machine +static syslinkRxState_e rxState = waitForFirstStart; +static syslinkPacket_t slp; +static uint8_t dataIndex = 0; +static uint8_t cksum[2] = {0}; +static uint8_t counter = 0; + +static rxRuntimeConfig_t *rxRuntimeConfigPtr; +static serialPort_t *serialPort; + +#define SUPPORTED_CHANNEL_COUNT (4 + CRTP_CPPM_EMU_MAX_AUX_CHANNELS) +static uint32_t channelData[SUPPORTED_CHANNEL_COUNT]; +static bool rcFrameComplete = false; + +static void routeIncommingPacket(syslinkPacket_t* slp) +{ + // Only support packets of type SYSLINK_RADIO_RAW + if(slp->type == SYSLINK_RADIO_RAW) { + crtpPacket_t *crtpPacket = (crtpPacket_t*)(slp->data); + + switch(crtpPacket->header.port) { + case CRTP_PORT_SETPOINT: + { + crtpCommanderRPYT_t *crtpRYPTPacket = + (crtpCommanderRPYT_t*)&crtpPacket->data[0]; + + // Write RPYT channels in TAER order + + // Translate thrust from 0-MAX_UINT16 into a PWM_style value (1000-2000) + channelData[0] = (crtpRYPTPacket->thrust * 1000 / UINT16_MAX) + 1000; + + // Translate RPY from an angle setpoint to a PWM-style value (1000-2000) + // For R and P, assume the client sends a max of -/+50 degrees (full range of 100) + // For Y, assume a max of -/+400 deg/s (full range of 800) + channelData[1] = (uint16_t)((crtpRYPTPacket->roll / 100 * 1000) + 1500); + channelData[2] = (uint16_t)((-crtpRYPTPacket->pitch / 100 * 1000) + 1500); // Pitch is inverted + channelData[3] = (uint16_t)((crtpRYPTPacket->yaw / 800 * 1000) + 1500); + + rcFrameComplete = true; + break; + } + case CRTP_PORT_SETPOINT_GENERIC: + // First byte of the packet is the type + // Only support the CPPM Emulation type + if(crtpPacket->data[0] == cppmEmuType) { + crtpCommanderCPPMEmuPacket_t *crtpCppmPacket = + (crtpCommanderCPPMEmuPacket_t*)&crtpPacket->data[1]; + + // Write RPYT channels in TAER order + channelData[0] = crtpCppmPacket->channelThrust; + channelData[1] = crtpCppmPacket->channelRoll; + channelData[2] = crtpCppmPacket->channelPitch; + channelData[3] = crtpCppmPacket->channelYaw; + + // Write the rest of the auxiliary channels + uint8_t i; + for (i = 0; i < crtpCppmPacket->hdr.numAuxChannels; i++) { + channelData[i + 4] = crtpCppmPacket->channelAux[i]; + } + } + rcFrameComplete = true; + break; + default: + // Unsupported port - do nothing + break; + } + } +} + +// Receive ISR callback +static void dataReceive(uint16_t c) +{ + counter++; + switch(rxState) { + case waitForFirstStart: + rxState = (c == SYSLINK_START_BYTE1) ? waitForSecondStart : waitForFirstStart; + break; + case waitForSecondStart: + rxState = (c == SYSLINK_START_BYTE2) ? waitForType : waitForFirstStart; + break; + case waitForType: + cksum[0] = c; + cksum[1] = c; + slp.type = c; + rxState = waitForLength; + break; + case waitForLength: + if (c <= SYSLINK_MTU) { + slp.length = c; + cksum[0] += c; + cksum[1] += cksum[0]; + dataIndex = 0; + rxState = (c > 0) ? waitForData : waitForChksum1; + } + else { + rxState = waitForFirstStart; + } + break; + case waitForData: + slp.data[dataIndex] = c; + cksum[0] += c; + cksum[1] += cksum[0]; + dataIndex++; + if (dataIndex == slp.length) { + rxState = waitForChksum1; + } + break; + case waitForChksum1: + if (cksum[0] == c) { + rxState = waitForChksum2; + } + else { + rxState = waitForFirstStart; //Checksum error + } + break; + case waitForChksum2: + if (cksum[1] == c) { + routeIncommingPacket(&slp); + } + else { + rxState = waitForFirstStart; //Checksum error + } + rxState = waitForFirstStart; + break; + default: + break; + } +} + +static uint8_t frameStatus(void) +{ + if (!rcFrameComplete) { + return RX_FRAME_PENDING; + } + + // Set rcFrameComplete to false so we don't process this one twice + rcFrameComplete = false; + + return RX_FRAME_COMPLETE; +} + +static uint16_t readRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +{ + if (chan >= rxRuntimeConfig->channelCount) { + return 0; + } + return channelData[chan]; +} + + +bool targetCustomSerialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + rxRuntimeConfigPtr = rxRuntimeConfig; + + if(rxConfig->serialrx_provider != SERIALRX_TARGET_CUSTOM) + { + return false; + } + + const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); + if (!portConfig) { + return false; + } + + rxRuntimeConfig->channelCount = SUPPORTED_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 20000; // Value taken from rx_spi.c (NRF24 is being used downstream) + rxRuntimeConfig->rcReadRawFn = readRawRC; + rxRuntimeConfig->rcFrameStatusFn = frameStatus; + + serialPort = openSerialPort(portConfig->identifier, + FUNCTION_RX_SERIAL, + dataReceive, + SYSLINK_BAUDRATE, + MODE_RX, + SERIAL_NOT_INVERTED | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO + ); + + return serialPort != NULL; +} + diff --git a/src/main/target/CRAZYFLIE2/syslink.h b/src/main/target/CRAZYFLIE2/syslink.h new file mode 100644 index 0000000000..8a956437a1 --- /dev/null +++ b/src/main/target/CRAZYFLIE2/syslink.h @@ -0,0 +1,72 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#pragma once + +/* + * This file contains definitions for the syslink protocol, a UART-based + * protocol for communication between the NRF51 MCU and the STM32 MCU on + * the Crazyflie flight controller board. + * + * For more details, see https://wiki.bitcraze.io/doc:crazyflie:syslink:index + */ + +#define SYSLINK_MTU 32 + +#define SYSLINK_START_BYTE1 0xBC +#define SYSLINK_START_BYTE2 0xCF + +#define SYSLINK_RADIO_GROUP 0x00 +#define SYSLINK_RADIO_RAW 0x00 +#define SYSLINK_RADIO_CHANNEL 0x01 +#define SYSLINK_RADIO_DATARATE 0x02 +#define SYSLINK_RADIO_CONTWAVE 0x03 +#define SYSLINK_RADIO_RSSI 0x04 +#define SYSLINK_RADIO_ADDRESS 0x05 +#define SYSLINK_RADIO_RAW_BROADCAST 0x06 + +#define SYSLINK_PM_GROUP 0x10 +#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_GROUP 0x20 +#define SYSLINK_OW_SCAN 0x20 +#define SYSLINK_OW_GETINFO 0x21 +#define SYSLINK_OW_READ 0x22 +#define SYSLINK_OW_WRITE 0x23 + +typedef struct syslinkPacket_s +{ + uint8_t type; + uint8_t length; + char data[SYSLINK_MTU]; +} __attribute__((packed)) syslinkPacket_t; + +// State machine states for receiving syslink packets +typedef enum +{ + waitForFirstStart, + waitForSecondStart, + waitForType, + waitForLength, + waitForData, + waitForChksum1, + waitForChksum2 +} syslinkRxState_e; diff --git a/src/main/target/CRAZYFLIE2/target.c b/src/main/target/CRAZYFLIE2/target.c new file mode 100644 index 0000000000..264dda66ae --- /dev/null +++ b/src/main/target/CRAZYFLIE2/target.c @@ -0,0 +1,38 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +#include + +#include +#include "drivers/io.h" + +#include "drivers/timer.h" +#include "drivers/dma.h" + +const timerHardware_t timerHardware[USABLE_TIMER_CHANNEL_COUNT] = { +#ifndef CRAZYFLIE2_USE_BIG_QUAD_DECK + { TIM2, IO_TAG(PB11), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // PWM2 - OUT2 (Motor 2) + { TIM2, IO_TAG(PA1), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // PWM1 - OUT1 (Motor 1) + { TIM2, IO_TAG(PA15), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // PWM3 - OUT3 (Motor 3) + { TIM4, IO_TAG(PB9), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM4, NULL, 0, 0 }, // PWM4 - OUT4 (Motor 4) +#else + { TIM3, IO_TAG(PB4), TIM_Channel_1, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // PWM2 - OUT2 (Motor 2) + { TIM2, IO_TAG(PA2), TIM_Channel_3, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // PWM1 - OUT1 (Motor 1) + { TIM3, IO_TAG(PB5), TIM_Channel_2, TIM_USE_MOTOR, 1, GPIO_AF_TIM3, NULL, 0, 0 }, // PWM3 - OUT3 (Motor 3) + { TIM2, IO_TAG(PA3), TIM_Channel_4, TIM_USE_MOTOR, 1, GPIO_AF_TIM2, NULL, 0, 0 }, // PWM4 - OUT4 (Motor 4) +#endif +}; diff --git a/src/main/target/CRAZYFLIE2/target.h b/src/main/target/CRAZYFLIE2/target.h new file mode 100644 index 0000000000..38099f5974 --- /dev/null +++ b/src/main/target/CRAZYFLIE2/target.h @@ -0,0 +1,101 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight 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 Cleanflight. If not, see . + */ + +/* + * This target is for the Crazyflie 2.0 nanocopter board + * + * For details on using this target with the Crazyflie see: + * https://wiki.bitcraze.io/misc:hacks:betaflight + * + * Target code written and maintained by Sean Kelly (theseankelly@outlook.com) + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "CF20" + +#define USBD_PRODUCT_STRING "Crazyflie 2.0" + +// Uncomment this define to build for the Crazyflie +// using the BigQuad expansion deck +//#define CRAZYFLIE2_USE_BIG_QUAD_DECK + +#define USABLE_TIMER_CHANNEL_COUNT 14 +#ifndef CRAZYFLIE2_USE_BIG_QUAD_DECK +#define USED_TIMERS ( TIM_N(2) | TIM_N(4) ) +#define BRUSHED_MOTORS +#else +#define USED_TIMERS ( TIM_N(2) | TIM_N(3) ) +#endif //CRAZYFLIE2_USE_BIG_QUAD_DECK + +#define LED0 PD2 +#define LED1 PC0 +#define LED2 PC3 + +// Using STM32F405RG, 64 pin package (LQFP64) +// 16 pins per port, ports A, B, C, and also PD2 +#define TARGET_IO_PORTA 0xFFFF +#define TARGET_IO_PORTB 0xFFFF +#define TARGET_IO_PORTC 0xFFFF +#define TARGET_IO_PORTD (BIT(2)) + +#define USE_VCP + +#define USE_UART2 +#define UART2_TX_PIN PA1 +#define UART2_RX_PIN PA3 + +#define USE_UART3 +#define UART3_TX_PIN PC10 +#define UART3_RX_PIN PC11 + +#define USE_UART6 +#define UART6_TX_PIN PC6 +#define UART6_RX_PIN PC7 + +#define SERIAL_PORT_COUNT 4 + +#define USE_I2C +#define USE_I2C_DEVICE_3 +#define I2C_DEVICE (I2CDEV_3) + +// MPU9250 has the AD0 pin held high so the +// address is 0x69 instead of the default 0x68 +#define MPU_ADDRESS 0x69 + +#define GYRO +#define USE_GYRO_MPU6500 +#define GYRO_MPU6500_ALIGN CW270_DEG + +#define ACC +#define USE_ACC_MPU6500 +#define ACC_MPU6500_ALIGN CW270_DEG + +// Mag isn't working quite right -- disabling it for now +//#define MAG +//#define USE_MPU9250_MAG // Enables bypass configuration on the MPU9250 I2C bus +//#define USE_MAG_AK8963 +//#define MAG_AK8963_ALIGN CW270_DEG + +#define USE_EXTI +#define MPU_INT_EXTI PC13 + +#define USE_SERIALRX_TARGET_CUSTOM +#define DEFAULT_RX_FEATURE FEATURE_RX_SERIAL +#define SERIALRX_UART SERIAL_PORT_USART6 +#define SERIALRX_PROVIDER SERIALRX_TARGET_CUSTOM +#define RX_CHANNELS_TAER diff --git a/src/main/target/CRAZYFLIE2/target.mk b/src/main/target/CRAZYFLIE2/target.mk new file mode 100644 index 0000000000..a2e36c3569 --- /dev/null +++ b/src/main/target/CRAZYFLIE2/target.mk @@ -0,0 +1,7 @@ +F405_TARGETS += $(TARGET) +FEATURES += VCP + +TARGET_SRC = \ + drivers/accgyro/accgyro_mpu.c \ + drivers/accgyro/accgyro_mpu6500.c \ + drivers/compass/compass_ak8963.c \