├── .gitignore ├── Art └── flyhero_logo.svg ├── Data_Analysis ├── .gitignore ├── Makefile ├── main │ ├── component.mk │ └── main.cpp └── sdkconfig ├── Docs ├── esp32_components.eli ├── esp32_devkitc_header.step ├── esp32_patterns.lib ├── pcb.dip ├── pcb.dipb1 ├── schem.dch └── schem.pdf ├── Flyhero_App ├── .gitignore ├── Makefile ├── main │ ├── component.mk │ └── main.cpp ├── partitions.csv └── sdkconfig ├── LICENSE ├── README.md ├── components ├── Barometer │ ├── component.mk │ ├── include │ │ └── MS5611.h │ └── src │ │ └── MS5611.cpp ├── IMU │ ├── Kconfig │ ├── component.mk │ ├── include │ │ ├── Biquad_Filter.h │ │ ├── Complementary_Filter.h │ │ ├── Fusion_Filter.h │ │ ├── IMU.h │ │ ├── IMU_Detector.h │ │ ├── MPU6000.h │ │ ├── MPU6050.h │ │ ├── MPU9250.h │ │ ├── Mahony_Filter.h │ │ └── Math.h │ └── src │ │ ├── Biquad_Filter.cpp │ │ ├── Complementary_Filter.cpp │ │ ├── IMU_Detector.cpp │ │ ├── MPU6000.cpp │ │ ├── MPU6050.cpp │ │ ├── MPU9250.cpp │ │ └── Mahony_Filter.cpp ├── LEDs │ ├── component.mk │ ├── include │ │ └── LEDs.h │ └── src │ │ └── LEDs.cpp ├── Logging │ ├── component.mk │ ├── include │ │ └── Logger.h │ ├── src │ │ └── Logger.cpp │ └── test │ │ ├── Logger_test.cpp │ │ └── component.mk ├── Motors │ ├── component.mk │ ├── include │ │ ├── Motors_Controller.h │ │ ├── Motors_Protocol.h │ │ └── OneShot125.h │ └── src │ │ ├── Motors_Controller.cpp │ │ └── OneShot125.cpp ├── PID │ ├── component.mk │ ├── include │ │ └── PID.h │ └── src │ │ └── PID.cpp ├── Ultrasonic │ ├── component.mk │ ├── include │ │ └── HC_SR04.h │ └── src │ │ └── HC_SR04.cpp ├── Utility │ ├── component.mk │ └── include │ │ └── Counting_Median_Finder.h └── WiFi │ ├── Kconfig │ ├── component.mk │ ├── include │ ├── CRC.h │ └── WiFi_Controller.h │ └── src │ └── WiFi_Controller.cpp ├── flyhero_crash_debug.bash ├── flyhero_crash_info.bash └── flyhero_test.bash /.gitignore: -------------------------------------------------------------------------------- 1 | /.metadata/ 2 | /RemoteSystemsTempFiles/ 3 | /.idea/ 4 | /CMakeLists.txt 5 | /cmake-build-debug/ 6 | -------------------------------------------------------------------------------- /Art/flyhero_logo.svg: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 19 | 21 | 43 | 45 | 46 | 48 | image/svg+xml 49 | 51 | 52 | 53 | 54 | 55 | 60 | 63 | 67 | 75 | 82 | 83 | 86 | 88 | 94 | 99 | 103 | 107 | 111 | 115 | 119 | 123 | 127 | 131 | 135 | 139 | 143 | 147 | 151 | 155 | 159 | 163 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | -------------------------------------------------------------------------------- /Data_Analysis/.gitignore: -------------------------------------------------------------------------------- 1 | /build/ 2 | /.settings/ 3 | /sdkconfig.old 4 | /.project 5 | /.cproject 6 | -------------------------------------------------------------------------------- /Data_Analysis/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # This is a project Makefile. It is assumed the directory this Makefile resides in is a 3 | # project subdirectory. 4 | # 5 | 6 | PROJECT_NAME := Data_Analysis 7 | EXTRA_COMPONENT_DIRS:= $(PWD)/../components 8 | 9 | include $(IDF_PATH)/make/project.mk 10 | 11 | -------------------------------------------------------------------------------- /Data_Analysis/main/component.mk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Data_Analysis/main/component.mk -------------------------------------------------------------------------------- /Data_Analysis/main/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "IMU_Detector.h" 4 | #include "CRC.h" 5 | #include "WiFi_Controller.h" 6 | #include "Motors_Controller.h" 7 | #include "Complementary_Filter.h" 8 | #include "Mahony_Filter.h" 9 | 10 | 11 | using namespace flyhero; 12 | 13 | 14 | void imu_task(void *args); 15 | 16 | void wifi_task(void *args); 17 | 18 | Motors_Controller &motors_controller = Motors_Controller::Instance(); 19 | 20 | extern "C" void app_main(void) 21 | { 22 | // Initialize NVS 23 | esp_err_t nvs_status = nvs_flash_init(); 24 | 25 | if (nvs_status == ESP_ERR_NVS_NO_FREE_PAGES) 26 | { 27 | ESP_ERROR_CHECK(nvs_flash_erase()); 28 | ESP_ERROR_CHECK(nvs_flash_init()); 29 | } 30 | 31 | LEDs::Init(); 32 | 33 | motors_controller.Init(); 34 | 35 | xTaskCreatePinnedToCore(imu_task, "IMU task", 4096, NULL, 2, NULL, 1); 36 | xTaskCreatePinnedToCore(wifi_task, "WiFi task", 4096, NULL, 2, NULL, 0); 37 | } 38 | 39 | void imu_task(void *args) 40 | { 41 | IMU::Sensor_Data accel, gyro; 42 | IMU::Euler_Angles euler; 43 | IMU::Read_Data_Type data_type; 44 | euler.roll = 0; 45 | euler.pitch = 0; 46 | euler.yaw = 0; 47 | 48 | //IMU::Euler_Angles complementary_euler, mahony_euler; 49 | //Complementary_Filter complementary_filter(0.97f); 50 | //Mahony_Filter mahony_filter(25, 0); 51 | 52 | IMU &imu = IMU_Detector::Detect_IMU(); 53 | 54 | imu.Init(); 55 | 56 | // we are not able to write data over UART that fast yet 57 | assert(imu.Get_Gyro_Sample_Rate() <= 1000); 58 | 59 | // wait for a while so that we can calibrate 60 | vTaskDelay(2000 / portTICK_RATE_MS); 61 | imu.Gyro_Calibrate(); 62 | 63 | while (!imu.Start()) 64 | imu.Accel_Calibrate(); 65 | 66 | while (true) 67 | { 68 | if (imu.Data_Ready()) 69 | { 70 | data_type = imu.Read_Data(accel, gyro); 71 | 72 | //complementary_filter.Compute(accel, gyro, complementary_euler); 73 | //mahony_filter.Compute(accel, gyro, mahony_euler); 74 | 75 | if (data_type == IMU::Read_Data_Type::ACCEL_GYRO) 76 | { 77 | motors_controller.Feed_Stab_PIDs(euler); 78 | motors_controller.Feed_Rate_PIDs(gyro); 79 | } else 80 | motors_controller.Feed_Rate_PIDs(gyro); 81 | 82 | printf("%f %f %f %f %f %f\n", accel.x, accel.y, accel.z, gyro.x, gyro.y, gyro.z); 83 | } 84 | } 85 | } 86 | 87 | void wifi_task(void *args) 88 | { 89 | WiFi_Controller &wifi = WiFi_Controller::Instance(); 90 | 91 | const char * received_data; 92 | size_t received_length; 93 | bool process_tcp = true; 94 | 95 | wifi.Init(); 96 | 97 | ESP_ERROR_CHECK(wifi.TCP_Server_Start()); 98 | ESP_ERROR_CHECK(wifi.TCP_Wait_For_Client()); 99 | 100 | while (process_tcp) 101 | { 102 | if (wifi.TCP_Receive(received_data, received_length)) 103 | { 104 | if (strncmp(received_data, "start", 5) == 0) 105 | { 106 | ESP_ERROR_CHECK(wifi.UDP_Server_Start()); 107 | wifi.TCP_Send("yup", 3); 108 | process_tcp = false; 109 | } else 110 | wifi.TCP_Send("nah", 3); 111 | } 112 | } 113 | ESP_ERROR_CHECK(wifi.TCP_Server_Stop()); 114 | 115 | WiFi_Controller::In_Datagram_Data datagram_data; 116 | 117 | while (true) 118 | { 119 | if (wifi.UDP_Receive(datagram_data)) 120 | { 121 | float rate_parameters[3][3] = { 122 | { datagram_data.rate_roll_kp * 0.01f, 0, 0 }, 123 | { datagram_data.rate_pitch_kp * 0.01f, 0, 0 }, 124 | { datagram_data.rate_yaw_kp * 0.01f, 0, 0 } 125 | }; 126 | 127 | float stab_parameters[3][3] = { 128 | { datagram_data.stab_roll_kp * 0.01f, datagram_data.stab_roll_ki * 0.01f, 0 }, 129 | { datagram_data.stab_pitch_kp * 0.01f, datagram_data.stab_pitch_ki * 0.01f, 0 }, 130 | { 0, 0, 0 } 131 | }; 132 | 133 | motors_controller.Set_Throttle(datagram_data.throttle); 134 | motors_controller.Set_PID_Constants(Motors_Controller::RATE, rate_parameters); 135 | motors_controller.Set_PID_Constants(Motors_Controller::STABILIZE, stab_parameters); 136 | } 137 | } 138 | } 139 | -------------------------------------------------------------------------------- /Data_Analysis/sdkconfig: -------------------------------------------------------------------------------- 1 | # 2 | # Automatically generated file; DO NOT EDIT. 3 | # Espressif IoT Development Framework Configuration 4 | # 5 | 6 | # 7 | # SDK tool configuration 8 | # 9 | CONFIG_TOOLPREFIX="xtensa-esp32-elf-" 10 | CONFIG_PYTHON="python" 11 | CONFIG_MAKE_WARN_UNDEFINED_VARIABLES=y 12 | 13 | # 14 | # Bootloader config 15 | # 16 | CONFIG_LOG_BOOTLOADER_LEVEL_NONE= 17 | CONFIG_LOG_BOOTLOADER_LEVEL_ERROR= 18 | CONFIG_LOG_BOOTLOADER_LEVEL_WARN= 19 | CONFIG_LOG_BOOTLOADER_LEVEL_INFO=y 20 | CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG= 21 | CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE= 22 | CONFIG_LOG_BOOTLOADER_LEVEL=3 23 | CONFIG_BOOTLOADER_VDDSDIO_BOOST=y 24 | 25 | # 26 | # Security features 27 | # 28 | CONFIG_SECURE_BOOT_ENABLED= 29 | CONFIG_FLASH_ENCRYPTION_ENABLED= 30 | 31 | # 32 | # Serial flasher config 33 | # 34 | CONFIG_ESPTOOLPY_PORT="/dev/ttyUSB0" 35 | CONFIG_ESPTOOLPY_BAUD_115200B= 36 | CONFIG_ESPTOOLPY_BAUD_230400B= 37 | CONFIG_ESPTOOLPY_BAUD_921600B=y 38 | CONFIG_ESPTOOLPY_BAUD_2MB= 39 | CONFIG_ESPTOOLPY_BAUD_OTHER= 40 | CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200 41 | CONFIG_ESPTOOLPY_BAUD=921600 42 | CONFIG_ESPTOOLPY_COMPRESSED=y 43 | CONFIG_FLASHMODE_QIO= 44 | CONFIG_FLASHMODE_QOUT= 45 | CONFIG_FLASHMODE_DIO=y 46 | CONFIG_FLASHMODE_DOUT= 47 | CONFIG_ESPTOOLPY_FLASHMODE="dio" 48 | CONFIG_ESPTOOLPY_FLASHFREQ_80M= 49 | CONFIG_ESPTOOLPY_FLASHFREQ_40M=y 50 | CONFIG_ESPTOOLPY_FLASHFREQ_26M= 51 | CONFIG_ESPTOOLPY_FLASHFREQ_20M= 52 | CONFIG_ESPTOOLPY_FLASHFREQ="40m" 53 | CONFIG_ESPTOOLPY_FLASHSIZE_1MB= 54 | CONFIG_ESPTOOLPY_FLASHSIZE_2MB=y 55 | CONFIG_ESPTOOLPY_FLASHSIZE_4MB= 56 | CONFIG_ESPTOOLPY_FLASHSIZE_8MB= 57 | CONFIG_ESPTOOLPY_FLASHSIZE_16MB= 58 | CONFIG_ESPTOOLPY_FLASHSIZE="2MB" 59 | CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y 60 | CONFIG_ESPTOOLPY_BEFORE_RESET=y 61 | CONFIG_ESPTOOLPY_BEFORE_NORESET= 62 | CONFIG_ESPTOOLPY_BEFORE="default_reset" 63 | CONFIG_ESPTOOLPY_AFTER_RESET=y 64 | CONFIG_ESPTOOLPY_AFTER_NORESET= 65 | CONFIG_ESPTOOLPY_AFTER="hard_reset" 66 | CONFIG_MONITOR_BAUD_9600B= 67 | CONFIG_MONITOR_BAUD_57600B= 68 | CONFIG_MONITOR_BAUD_115200B= 69 | CONFIG_MONITOR_BAUD_230400B= 70 | CONFIG_MONITOR_BAUD_921600B=y 71 | CONFIG_MONITOR_BAUD_2MB= 72 | CONFIG_MONITOR_BAUD_OTHER= 73 | CONFIG_MONITOR_BAUD_OTHER_VAL=115200 74 | CONFIG_MONITOR_BAUD=921600 75 | 76 | # 77 | # Partition Table 78 | # 79 | CONFIG_PARTITION_TABLE_SINGLE_APP=y 80 | CONFIG_PARTITION_TABLE_TWO_OTA= 81 | CONFIG_PARTITION_TABLE_CUSTOM= 82 | CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" 83 | CONFIG_PARTITION_TABLE_CUSTOM_APP_BIN_OFFSET=0x10000 84 | CONFIG_PARTITION_TABLE_FILENAME="partitions_singleapp_coredump.csv" 85 | CONFIG_APP_OFFSET=0x10000 86 | CONFIG_PARTITION_TABLE_MD5=y 87 | 88 | # 89 | # Compiler options 90 | # 91 | CONFIG_OPTIMIZATION_LEVEL_DEBUG=y 92 | CONFIG_OPTIMIZATION_LEVEL_RELEASE= 93 | CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y 94 | CONFIG_OPTIMIZATION_ASSERTIONS_SILENT= 95 | CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED= 96 | CONFIG_CXX_EXCEPTIONS= 97 | CONFIG_STACK_CHECK_NONE=y 98 | CONFIG_STACK_CHECK_NORM= 99 | CONFIG_STACK_CHECK_STRONG= 100 | CONFIG_STACK_CHECK_ALL= 101 | CONFIG_STACK_CHECK= 102 | 103 | # 104 | # Component config 105 | # 106 | 107 | # 108 | # Flyhero IMU 109 | # 110 | CONFIG_FLYHERO_IMU_USE_SOFT_LPF=y 111 | CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF=10 112 | CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF=60 113 | CONFIG_FLYHERO_IMU_USE_NOTCH=y 114 | CONFIG_FLYHERO_IMU_GYRO_NOTCH=140 115 | CONFIG_FLYHERO_IMU_HARD_LPF_256HZ= 116 | CONFIG_FLYHERO_IMU_HARD_LPF_188HZ= 117 | CONFIG_FLYHERO_IMU_HARD_LPF_98HZ= 118 | CONFIG_FLYHERO_IMU_HARD_LPF_42HZ= 119 | CONFIG_FLYHERO_IMU_HARD_LPF_20HZ=y 120 | CONFIG_FLYHERO_IMU_HARD_LPF_10HZ= 121 | CONFIG_FLYHERO_IMU_HARD_LPF_5HZ= 122 | CONFIG_FLYHERO_IMU_ACCEL_FSR_16=y 123 | CONFIG_FLYHERO_IMU_ACCEL_FSR_8= 124 | CONFIG_FLYHERO_IMU_ACCEL_FSR_4= 125 | CONFIG_FLYHERO_IMU_ACCEL_FSR_2= 126 | CONFIG_FLYHERO_IMU_GYRO_FSR_2000=y 127 | CONFIG_FLYHERO_IMU_GYRO_FSR_1000= 128 | CONFIG_FLYHERO_IMU_GYRO_FSR_500= 129 | CONFIG_FLYHERO_IMU_GYRO_FSR_250= 130 | 131 | # 132 | # Flyhero WiFi 133 | # 134 | CONFIG_FLYHERO_WIFI_TCP_PORT=4821 135 | CONFIG_FLYHERO_WIFI_UDP_PORT=4789 136 | CONFIG_FLYHERO_WIFI_SSID="DRONE WIFI" 137 | CONFIG_FLYHERO_WIFI_SSID_VISIBLE=y 138 | CONFIG_FLYHERO_WIFI_WPA2_PASSPHRASE="flyhero00" 139 | 140 | # 141 | # Application Level Tracing 142 | # 143 | CONFIG_ESP32_APPTRACE_DEST_TRAX= 144 | CONFIG_ESP32_APPTRACE_DEST_NONE=y 145 | CONFIG_ESP32_APPTRACE_ENABLE= 146 | CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y 147 | 148 | # 149 | # FreeRTOS SystemView Tracing 150 | # 151 | CONFIG_AWS_IOT_SDK= 152 | 153 | # 154 | # Bluetooth 155 | # 156 | CONFIG_BT_ENABLED= 157 | CONFIG_BTDM_CONTROLLER_PINNED_TO_CORE=0 158 | CONFIG_BT_RESERVE_DRAM=0 159 | 160 | # 161 | # ESP32-specific 162 | # 163 | CONFIG_ESP32_DEFAULT_CPU_FREQ_80= 164 | CONFIG_ESP32_DEFAULT_CPU_FREQ_160= 165 | CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y 166 | CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240 167 | CONFIG_MEMMAP_SMP=y 168 | CONFIG_SPIRAM_SUPPORT= 169 | CONFIG_MEMMAP_TRACEMEM= 170 | CONFIG_MEMMAP_TRACEMEM_TWOBANKS= 171 | CONFIG_ESP32_TRAX= 172 | CONFIG_TRACEMEM_RESERVE_DRAM=0x0 173 | CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH=y 174 | CONFIG_ESP32_ENABLE_COREDUMP_TO_UART= 175 | CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE= 176 | CONFIG_ESP32_ENABLE_COREDUMP=y 177 | CONFIG_ESP32_CORE_DUMP_LOG_LEVEL=1 178 | CONFIG_TWO_UNIVERSAL_MAC_ADDRESS= 179 | CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y 180 | CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4 181 | CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32 182 | CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4096 183 | CONFIG_MAIN_TASK_STACK_SIZE=4096 184 | CONFIG_IPC_TASK_STACK_SIZE=1024 185 | CONFIG_TIMER_TASK_STACK_SIZE=4096 186 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y 187 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF= 188 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR= 189 | CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF= 190 | CONFIG_NEWLIB_STDIN_LINE_ENDING_LF= 191 | CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y 192 | CONFIG_NEWLIB_NANO_FORMAT= 193 | CONFIG_CONSOLE_UART_DEFAULT=y 194 | CONFIG_CONSOLE_UART_CUSTOM= 195 | CONFIG_CONSOLE_UART_NONE= 196 | CONFIG_CONSOLE_UART_NUM=0 197 | CONFIG_CONSOLE_UART_BAUDRATE=921600 198 | CONFIG_ULP_COPROC_ENABLED= 199 | CONFIG_ULP_COPROC_RESERVE_MEM=0 200 | CONFIG_ESP32_PANIC_PRINT_HALT=y 201 | CONFIG_ESP32_PANIC_PRINT_REBOOT= 202 | CONFIG_ESP32_PANIC_SILENT_REBOOT= 203 | CONFIG_ESP32_PANIC_GDBSTUB= 204 | CONFIG_ESP32_DEBUG_OCDAWARE=y 205 | CONFIG_INT_WDT=y 206 | CONFIG_INT_WDT_TIMEOUT_MS=300 207 | CONFIG_INT_WDT_CHECK_CPU1=y 208 | CONFIG_TASK_WDT=y 209 | CONFIG_TASK_WDT_PANIC= 210 | CONFIG_TASK_WDT_TIMEOUT_S=5 211 | CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU0= 212 | CONFIG_TASK_WDT_CHECK_IDLE_TASK_CPU1= 213 | CONFIG_BROWNOUT_DET=y 214 | CONFIG_BROWNOUT_DET_LVL_SEL_0=y 215 | CONFIG_BROWNOUT_DET_LVL_SEL_1= 216 | CONFIG_BROWNOUT_DET_LVL_SEL_2= 217 | CONFIG_BROWNOUT_DET_LVL_SEL_3= 218 | CONFIG_BROWNOUT_DET_LVL_SEL_4= 219 | CONFIG_BROWNOUT_DET_LVL_SEL_5= 220 | CONFIG_BROWNOUT_DET_LVL_SEL_6= 221 | CONFIG_BROWNOUT_DET_LVL_SEL_7= 222 | CONFIG_BROWNOUT_DET_LVL=0 223 | CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y 224 | CONFIG_ESP32_TIME_SYSCALL_USE_RTC= 225 | CONFIG_ESP32_TIME_SYSCALL_USE_FRC1= 226 | CONFIG_ESP32_TIME_SYSCALL_USE_NONE= 227 | CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y 228 | CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL= 229 | CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024 230 | CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000 231 | CONFIG_ESP32_XTAL_FREQ_40=y 232 | CONFIG_ESP32_XTAL_FREQ_26= 233 | CONFIG_ESP32_XTAL_FREQ_AUTO= 234 | CONFIG_ESP32_XTAL_FREQ=40 235 | CONFIG_DISABLE_BASIC_ROM_CONSOLE= 236 | CONFIG_NO_BLOBS= 237 | CONFIG_ESP_TIMER_PROFILING= 238 | CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS= 239 | 240 | # 241 | # Wi-Fi 242 | # 243 | CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10 244 | CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32 245 | CONFIG_ESP32_WIFI_STATIC_TX_BUFFER= 246 | CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y 247 | CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1 248 | CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32 249 | CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y 250 | CONFIG_ESP32_WIFI_TX_BA_WIN=6 251 | CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y 252 | CONFIG_ESP32_WIFI_RX_BA_WIN=6 253 | CONFIG_ESP32_WIFI_NVS_ENABLED=y 254 | 255 | # 256 | # PHY 257 | # 258 | CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y 259 | CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION= 260 | CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 261 | CONFIG_ESP32_PHY_MAX_TX_POWER=20 262 | 263 | # 264 | # Power Management 265 | # 266 | CONFIG_PM_ENABLE= 267 | 268 | # 269 | # ADC-Calibration 270 | # 271 | CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y 272 | CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y 273 | CONFIG_ADC_CAL_LUT_ENABLE=y 274 | 275 | # 276 | # Ethernet 277 | # 278 | CONFIG_DMA_RX_BUF_NUM=10 279 | CONFIG_DMA_TX_BUF_NUM=10 280 | CONFIG_EMAC_L2_TO_L3_RX_BUF_MODE= 281 | CONFIG_EMAC_TASK_PRIORITY=20 282 | 283 | # 284 | # FAT Filesystem support 285 | # 286 | CONFIG_FATFS_CODEPAGE_DYNAMIC= 287 | CONFIG_FATFS_CODEPAGE_437=y 288 | CONFIG_FATFS_CODEPAGE_720= 289 | CONFIG_FATFS_CODEPAGE_737= 290 | CONFIG_FATFS_CODEPAGE_771= 291 | CONFIG_FATFS_CODEPAGE_775= 292 | CONFIG_FATFS_CODEPAGE_850= 293 | CONFIG_FATFS_CODEPAGE_852= 294 | CONFIG_FATFS_CODEPAGE_855= 295 | CONFIG_FATFS_CODEPAGE_857= 296 | CONFIG_FATFS_CODEPAGE_860= 297 | CONFIG_FATFS_CODEPAGE_861= 298 | CONFIG_FATFS_CODEPAGE_862= 299 | CONFIG_FATFS_CODEPAGE_863= 300 | CONFIG_FATFS_CODEPAGE_864= 301 | CONFIG_FATFS_CODEPAGE_865= 302 | CONFIG_FATFS_CODEPAGE_866= 303 | CONFIG_FATFS_CODEPAGE_869= 304 | CONFIG_FATFS_CODEPAGE_932= 305 | CONFIG_FATFS_CODEPAGE_936= 306 | CONFIG_FATFS_CODEPAGE_949= 307 | CONFIG_FATFS_CODEPAGE_950= 308 | CONFIG_FATFS_CODEPAGE=437 309 | CONFIG_FATFS_LFN_NONE=y 310 | CONFIG_FATFS_LFN_HEAP= 311 | CONFIG_FATFS_LFN_STACK= 312 | CONFIG_FATFS_FS_LOCK=0 313 | CONFIG_FATFS_TIMEOUT_MS=10000 314 | CONFIG_FATFS_PER_FILE_CACHE=y 315 | 316 | # 317 | # FreeRTOS 318 | # 319 | CONFIG_FREERTOS_UNICORE= 320 | CONFIG_FREERTOS_CORETIMER_0=y 321 | CONFIG_FREERTOS_CORETIMER_1= 322 | CONFIG_FREERTOS_HZ=1000 323 | CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y 324 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE= 325 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL= 326 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y 327 | CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK= 328 | CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y 329 | CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1 330 | CONFIG_FREERTOS_ASSERT_FAIL_ABORT=y 331 | CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE= 332 | CONFIG_FREERTOS_ASSERT_DISABLE= 333 | CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1024 334 | CONFIG_FREERTOS_ISR_STACKSIZE=1536 335 | CONFIG_FREERTOS_LEGACY_HOOKS= 336 | CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16 337 | CONFIG_SUPPORT_STATIC_ALLOCATION= 338 | CONFIG_TIMER_TASK_PRIORITY=1 339 | CONFIG_TIMER_TASK_STACK_DEPTH=2048 340 | CONFIG_TIMER_QUEUE_LENGTH=10 341 | CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0 342 | CONFIG_FREERTOS_USE_TRACE_FACILITY= 343 | CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS= 344 | CONFIG_FREERTOS_DEBUG_INTERNALS= 345 | 346 | # 347 | # Heap memory debugging 348 | # 349 | CONFIG_HEAP_POISONING_DISABLED=y 350 | CONFIG_HEAP_POISONING_LIGHT= 351 | CONFIG_HEAP_POISONING_COMPREHENSIVE= 352 | CONFIG_HEAP_TRACING= 353 | 354 | # 355 | # libsodium 356 | # 357 | CONFIG_LIBSODIUM_USE_MBEDTLS_SHA=y 358 | 359 | # 360 | # Log output 361 | # 362 | CONFIG_LOG_DEFAULT_LEVEL_NONE= 363 | CONFIG_LOG_DEFAULT_LEVEL_ERROR= 364 | CONFIG_LOG_DEFAULT_LEVEL_WARN= 365 | CONFIG_LOG_DEFAULT_LEVEL_INFO=y 366 | CONFIG_LOG_DEFAULT_LEVEL_DEBUG= 367 | CONFIG_LOG_DEFAULT_LEVEL_VERBOSE= 368 | CONFIG_LOG_DEFAULT_LEVEL=3 369 | CONFIG_LOG_COLORS=y 370 | 371 | # 372 | # LWIP 373 | # 374 | CONFIG_L2_TO_L3_COPY= 375 | CONFIG_LWIP_IRAM_OPTIMIZATION= 376 | CONFIG_LWIP_MAX_SOCKETS=10 377 | CONFIG_LWIP_SO_REUSE= 378 | CONFIG_LWIP_SO_RCVBUF= 379 | CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1 380 | CONFIG_LWIP_IP_FRAG= 381 | CONFIG_LWIP_IP_REASSEMBLY= 382 | CONFIG_LWIP_STATS= 383 | CONFIG_LWIP_ETHARP_TRUST_IP_MAC=y 384 | CONFIG_TCPIP_RECVMBOX_SIZE=32 385 | CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y 386 | 387 | # 388 | # DHCP server 389 | # 390 | CONFIG_LWIP_DHCPS_LEASE_UNIT=60 391 | CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8 392 | CONFIG_LWIP_AUTOIP= 393 | CONFIG_LWIP_NETIF_LOOPBACK=y 394 | CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8 395 | 396 | # 397 | # TCP 398 | # 399 | CONFIG_LWIP_MAX_ACTIVE_TCP=16 400 | CONFIG_LWIP_MAX_LISTENING_TCP=16 401 | CONFIG_TCP_MAXRTX=12 402 | CONFIG_TCP_SYNMAXRTX=6 403 | CONFIG_TCP_MSS=1436 404 | CONFIG_TCP_MSL=60000 405 | CONFIG_TCP_SND_BUF_DEFAULT=5744 406 | CONFIG_TCP_WND_DEFAULT=5744 407 | CONFIG_TCP_RECVMBOX_SIZE=6 408 | CONFIG_TCP_QUEUE_OOSEQ=y 409 | CONFIG_TCP_OVERSIZE_MSS=y 410 | CONFIG_TCP_OVERSIZE_QUARTER_MSS= 411 | CONFIG_TCP_OVERSIZE_DISABLE= 412 | 413 | # 414 | # UDP 415 | # 416 | CONFIG_LWIP_MAX_UDP_PCBS=16 417 | CONFIG_UDP_RECVMBOX_SIZE=6 418 | CONFIG_TCPIP_TASK_STACK_SIZE=2560 419 | CONFIG_PPP_SUPPORT= 420 | 421 | # 422 | # ICMP 423 | # 424 | CONFIG_LWIP_MULTICAST_PING= 425 | CONFIG_LWIP_BROADCAST_PING= 426 | 427 | # 428 | # LWIP RAW API 429 | # 430 | CONFIG_LWIP_MAX_RAW_PCBS=16 431 | 432 | # 433 | # mbedTLS 434 | # 435 | CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384 436 | CONFIG_MBEDTLS_DEBUG= 437 | CONFIG_MBEDTLS_HARDWARE_AES=y 438 | CONFIG_MBEDTLS_HARDWARE_MPI= 439 | CONFIG_MBEDTLS_HARDWARE_SHA= 440 | CONFIG_MBEDTLS_HAVE_TIME=y 441 | CONFIG_MBEDTLS_HAVE_TIME_DATE= 442 | CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y 443 | CONFIG_MBEDTLS_TLS_SERVER_ONLY= 444 | CONFIG_MBEDTLS_TLS_CLIENT_ONLY= 445 | CONFIG_MBEDTLS_TLS_DISABLED= 446 | CONFIG_MBEDTLS_TLS_SERVER=y 447 | CONFIG_MBEDTLS_TLS_CLIENT=y 448 | CONFIG_MBEDTLS_TLS_ENABLED=y 449 | 450 | # 451 | # TLS Key Exchange Methods 452 | # 453 | CONFIG_MBEDTLS_PSK_MODES= 454 | CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y 455 | CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y 456 | CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y 457 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y 458 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y 459 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y 460 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y 461 | CONFIG_MBEDTLS_SSL_RENEGOTIATION=y 462 | CONFIG_MBEDTLS_SSL_PROTO_SSL3= 463 | CONFIG_MBEDTLS_SSL_PROTO_TLS1=y 464 | CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y 465 | CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y 466 | CONFIG_MBEDTLS_SSL_PROTO_DTLS= 467 | CONFIG_MBEDTLS_SSL_ALPN=y 468 | CONFIG_MBEDTLS_SSL_SESSION_TICKETS=y 469 | 470 | # 471 | # Symmetric Ciphers 472 | # 473 | CONFIG_MBEDTLS_AES_C=y 474 | CONFIG_MBEDTLS_CAMELLIA_C= 475 | CONFIG_MBEDTLS_DES_C= 476 | CONFIG_MBEDTLS_RC4_DISABLED=y 477 | CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT= 478 | CONFIG_MBEDTLS_RC4_ENABLED= 479 | CONFIG_MBEDTLS_BLOWFISH_C= 480 | CONFIG_MBEDTLS_XTEA_C= 481 | CONFIG_MBEDTLS_CCM_C=y 482 | CONFIG_MBEDTLS_GCM_C=y 483 | CONFIG_MBEDTLS_RIPEMD160_C= 484 | 485 | # 486 | # Certificates 487 | # 488 | CONFIG_MBEDTLS_PEM_PARSE_C=y 489 | CONFIG_MBEDTLS_PEM_WRITE_C=y 490 | CONFIG_MBEDTLS_X509_CRL_PARSE_C=y 491 | CONFIG_MBEDTLS_X509_CSR_PARSE_C=y 492 | CONFIG_MBEDTLS_ECP_C=y 493 | CONFIG_MBEDTLS_ECDH_C=y 494 | CONFIG_MBEDTLS_ECDSA_C=y 495 | CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y 496 | CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y 497 | CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y 498 | CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y 499 | CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y 500 | CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y 501 | CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y 502 | CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y 503 | CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y 504 | CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y 505 | CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y 506 | CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y 507 | CONFIG_MBEDTLS_ECP_NIST_OPTIM=y 508 | 509 | # 510 | # OpenSSL 511 | # 512 | CONFIG_OPENSSL_DEBUG= 513 | CONFIG_OPENSSL_ASSERT_DO_NOTHING=y 514 | CONFIG_OPENSSL_ASSERT_EXIT= 515 | 516 | # 517 | # PThreads 518 | # 519 | CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5 520 | CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=2048 521 | 522 | # 523 | # SPI Flash driver 524 | # 525 | CONFIG_SPI_FLASH_VERIFY_WRITE= 526 | CONFIG_SPI_FLASH_ENABLE_COUNTERS= 527 | CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y 528 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y 529 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS= 530 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED= 531 | 532 | # 533 | # SPIFFS Configuration 534 | # 535 | CONFIG_SPIFFS_MAX_PARTITIONS=3 536 | 537 | # 538 | # SPIFFS Cache Configuration 539 | # 540 | CONFIG_SPIFFS_CACHE=y 541 | CONFIG_SPIFFS_CACHE_WR=y 542 | CONFIG_SPIFFS_CACHE_STATS= 543 | CONFIG_SPIFFS_PAGE_CHECK=y 544 | CONFIG_SPIFFS_GC_MAX_RUNS=10 545 | CONFIG_SPIFFS_GC_STATS= 546 | CONFIG_SPIFFS_PAGE_SIZE=256 547 | CONFIG_SPIFFS_OBJ_NAME_LEN=32 548 | CONFIG_SPIFFS_USE_MAGIC=y 549 | CONFIG_SPIFFS_USE_MAGIC_LENGTH=y 550 | CONFIG_SPIFFS_META_LENGTH=4 551 | CONFIG_SPIFFS_USE_MTIME=y 552 | 553 | # 554 | # Debug Configuration 555 | # 556 | CONFIG_SPIFFS_DBG= 557 | CONFIG_SPIFFS_API_DBG= 558 | CONFIG_SPIFFS_GC_DBG= 559 | CONFIG_SPIFFS_CACHE_DBG= 560 | CONFIG_SPIFFS_CHECK_DBG= 561 | CONFIG_SPIFFS_TEST_VISUALISATION= 562 | 563 | # 564 | # tcpip adapter 565 | # 566 | CONFIG_IP_LOST_TIMER_INTERVAL=120 567 | 568 | # 569 | # Wear Levelling 570 | # 571 | CONFIG_WL_SECTOR_SIZE_512= 572 | CONFIG_WL_SECTOR_SIZE_4096=y 573 | CONFIG_WL_SECTOR_SIZE=4096 574 | -------------------------------------------------------------------------------- /Docs/esp32_components.eli: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/esp32_components.eli -------------------------------------------------------------------------------- /Docs/esp32_patterns.lib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/esp32_patterns.lib -------------------------------------------------------------------------------- /Docs/pcb.dip: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/pcb.dip -------------------------------------------------------------------------------- /Docs/pcb.dipb1: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/pcb.dipb1 -------------------------------------------------------------------------------- /Docs/schem.dch: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/schem.dch -------------------------------------------------------------------------------- /Docs/schem.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Docs/schem.pdf -------------------------------------------------------------------------------- /Flyhero_App/.gitignore: -------------------------------------------------------------------------------- 1 | /build/ 2 | /.settings/ 3 | /sdkconfig.old 4 | /.project 5 | /.cproject 6 | -------------------------------------------------------------------------------- /Flyhero_App/Makefile: -------------------------------------------------------------------------------- 1 | # 2 | # This is a project Makefile. It is assumed the directory this Makefile resides in is a 3 | # project subdirectory. 4 | # 5 | 6 | PROJECT_NAME:= Flyhero_App 7 | EXTRA_COMPONENT_DIRS:= $(PWD)/../components 8 | 9 | include $(IDF_PATH)/make/project.mk 10 | 11 | 12 | -------------------------------------------------------------------------------- /Flyhero_App/main/component.mk: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/michprev/flyhero-esp32/9ad6ec7a9c997595a2ba2c48e796daac542a0c57/Flyhero_App/main/component.mk -------------------------------------------------------------------------------- /Flyhero_App/main/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include "Motors_Controller.h" 10 | #include "IMU_Detector.h" 11 | #include "Mahony_Filter.h" 12 | #include "Complementary_Filter.h" 13 | #include "WiFi_Controller.h" 14 | 15 | 16 | using namespace flyhero; 17 | 18 | Motors_Controller & motors_controller = Motors_Controller::Instance(); 19 | 20 | void wifi_task(void * args); 21 | 22 | void imu_task(void * args); 23 | 24 | void gpio_isr_task(void * args) 25 | { 26 | ESP_ERROR_CHECK(gpio_install_isr_service(ESP_INTR_FLAG_LEVEL3)); 27 | 28 | vTaskDelete(NULL); 29 | } 30 | 31 | void TCP_process(const char * command); 32 | 33 | extern "C" void app_main(void) 34 | { 35 | // Initialize NVS 36 | esp_err_t nvs_status = nvs_flash_init(); 37 | 38 | if (nvs_status == ESP_ERR_NVS_NO_FREE_PAGES) 39 | { 40 | ESP_ERROR_CHECK(nvs_flash_erase()); 41 | ESP_ERROR_CHECK(nvs_flash_init()); 42 | } 43 | 44 | xTaskCreatePinnedToCore(gpio_isr_task, "GPIO ISR register task", 2048, NULL, 2, NULL, 1); 45 | vTaskDelay(1000 / portTICK_PERIOD_MS); 46 | 47 | LEDs::Init(); 48 | motors_controller.Init(); 49 | 50 | IMU & imu = IMU_Detector::Detect_IMU(); 51 | imu.Init(); 52 | 53 | xTaskCreatePinnedToCore(wifi_task, "WiFi task", 4096, NULL, 2, NULL, 0); 54 | } 55 | 56 | void imu_task(void * args) 57 | { 58 | IMU::Sensor_Data accel, gyro; 59 | IMU::Euler_Angles complementary_euler; 60 | IMU::Read_Data_Type data_type; 61 | 62 | IMU & imu = IMU_Detector::Detect_IMU(); 63 | Complementary_Filter complementary(0.97f); 64 | 65 | // some dummy reads to heat up SPI/I2C drivers 66 | imu.Read_Data(accel, gyro); 67 | imu.Read_Data(accel, gyro); 68 | imu.Read_Data(accel, gyro); 69 | 70 | periph_module_enable(PERIPH_TIMG1_MODULE); // enable TIMG1 watchdog clock 71 | TIMERG1.wdt_wprotect = 0x50d83aa1; // disable write protection 72 | TIMERG1.wdt_config0.sys_reset_length = 7; 73 | TIMERG1.wdt_config0.cpu_reset_length = 7; 74 | TIMERG1.wdt_config0.level_int_en = 0; // disable level interrupt 75 | TIMERG1.wdt_config0.edge_int_en = 0; // disable edge interrupt 76 | TIMERG1.wdt_config0.stg0 = 3; // reset chip when reaching stage 0 limit 77 | TIMERG1.wdt_config0.stg1 = 0; // 78 | TIMERG1.wdt_config0.stg2 = 0; // disable other stages 79 | TIMERG1.wdt_config0.stg3 = 0; // 80 | TIMERG1.wdt_config1.clk_prescale = 80; // tick every 1 us 81 | TIMERG1.wdt_config2 = 1100; // wait 1100 ticks (1100 microseconds) before resetting chip 82 | TIMERG1.wdt_config0.en = 1; 83 | TIMERG1.wdt_feed = 1; 84 | TIMERG1.wdt_wprotect = 0; // enable write protection 85 | 86 | while (true) 87 | { 88 | if (imu.Data_Ready()) 89 | { 90 | TIMERG1.wdt_wprotect = 0x50d83aa1; 91 | TIMERG1.wdt_feed = 1; 92 | TIMERG1.wdt_wprotect = 0; 93 | 94 | data_type = imu.Read_Data(accel, gyro); 95 | 96 | complementary.Compute(accel, gyro, complementary_euler); 97 | 98 | if (data_type == IMU::Read_Data_Type::ACCEL_GYRO) 99 | { 100 | motors_controller.Feed_Stab_PIDs(complementary_euler); 101 | motors_controller.Feed_Rate_PIDs(gyro); 102 | } else 103 | motors_controller.Feed_Rate_PIDs(gyro); 104 | } 105 | } 106 | } 107 | 108 | void wifi_task(void * args) 109 | { 110 | WiFi_Controller & wifi = WiFi_Controller::Instance(); 111 | WiFi_Controller::In_Datagram_Data in_datagram_data; 112 | 113 | const char * received_data; 114 | size_t received_length; 115 | bool process_tcp, process_udp; 116 | 117 | wifi.Init(); 118 | 119 | ESP_ERROR_CHECK(wifi.UDP_Server_Start()); 120 | ESP_ERROR_CHECK(wifi.TCP_Server_Start()); 121 | ESP_ERROR_CHECK(wifi.TCP_Wait_For_Client()); 122 | 123 | while (true) 124 | { 125 | // TCP phase 126 | 127 | process_tcp = true; 128 | 129 | while (process_tcp) 130 | { 131 | if (wifi.TCP_Receive(received_data, received_length)) 132 | { 133 | if (strncmp(received_data, "start", strlen("start")) == 0) 134 | { 135 | IMU_Detector::Detect_IMU().Gyro_Calibrate(); 136 | if (IMU_Detector::Detect_IMU().Start()) 137 | { 138 | motors_controller.Start(); 139 | wifi.TCP_Send("yup", 3); 140 | process_tcp = false; 141 | } else 142 | wifi.TCP_Send("nah", 3); 143 | } else 144 | TCP_process(received_data); 145 | } 146 | vTaskDelay(20); 147 | } 148 | 149 | TaskHandle_t imu_task_handle; 150 | assert(xTaskCreatePinnedToCore(imu_task, "IMU task", 4096, NULL, 20, &imu_task_handle, 1) == pdTRUE); 151 | 152 | periph_module_enable(PERIPH_TIMG0_MODULE); // enable TIMG0 watchdog clock 153 | TIMERG0.wdt_wprotect = 0x50d83aa1; // disable write protection 154 | TIMERG0.wdt_config0.sys_reset_length = 7; 155 | TIMERG0.wdt_config0.cpu_reset_length = 7; 156 | TIMERG0.wdt_config0.level_int_en = 0; // disable level interrupt 157 | TIMERG0.wdt_config0.edge_int_en = 0; // disable edge interrupt 158 | TIMERG0.wdt_config0.stg0 = 3; // reset chip when reaching stage 0 limit 159 | TIMERG0.wdt_config0.stg1 = 0; // 160 | TIMERG0.wdt_config0.stg2 = 0; // disable other stages 161 | TIMERG0.wdt_config0.stg3 = 0; // 162 | TIMERG0.wdt_config1.clk_prescale = 40000; // tick every 500 us 163 | TIMERG0.wdt_config2 = 2000; // wait 2000 ticks (1 second) before resetting chip 164 | TIMERG0.wdt_config0.en = 1; 165 | TIMERG0.wdt_feed = 1; 166 | TIMERG0.wdt_wprotect = 0; // enable write protection 167 | 168 | // UDP phase 169 | 170 | process_udp = true; 171 | 172 | while (process_udp) 173 | { 174 | if (wifi.UDP_Receive(in_datagram_data)) 175 | { 176 | TIMERG0.wdt_wprotect = 0x50d83aa1; 177 | TIMERG0.wdt_feed = 1; 178 | TIMERG0.wdt_wprotect = 0; 179 | 180 | float rate_parameters[3][3] = { 181 | { in_datagram_data.rate_roll_kp * 0.01f, 0, 0 }, 182 | { in_datagram_data.rate_pitch_kp * 0.01f, 0, 0 }, 183 | { in_datagram_data.rate_yaw_kp * 0.01f, 0, 0 } 184 | }; 185 | 186 | float stab_parameters[3][3] = { 187 | { in_datagram_data.stab_roll_kp * 0.01f, in_datagram_data.stab_roll_ki * 0.01f, 0 }, 188 | { in_datagram_data.stab_pitch_kp * 0.01f, in_datagram_data.stab_pitch_ki * 0.01f, 0 }, 189 | { 0, 0, 0 } 190 | }; 191 | 192 | motors_controller.Set_Throttle(in_datagram_data.throttle); 193 | motors_controller.Set_PID_Constants(Motors_Controller::RATE, rate_parameters); 194 | motors_controller.Set_PID_Constants(Motors_Controller::STABILIZE, stab_parameters); 195 | } 196 | if (wifi.TCP_Receive(received_data, received_length)) 197 | { 198 | if (strncmp(received_data, "stop", strlen("stop")) == 0) 199 | { 200 | motors_controller.Stop(); 201 | 202 | // motors disabled, safe to disable both watchdogs 203 | TIMERG0.wdt_wprotect = 0x50d83aa1; 204 | TIMERG0.wdt_config0.en = 0; 205 | TIMERG0.wdt_wprotect = 0; 206 | 207 | TIMERG1.wdt_wprotect = 0x50d83aa1; 208 | TIMERG1.wdt_config0.en = 0; 209 | TIMERG1.wdt_wprotect = 0; 210 | 211 | vTaskDelete(imu_task_handle); 212 | IMU_Detector::Detect_IMU().Stop(); 213 | process_udp = false; 214 | 215 | wifi.TCP_Send("yup", 3); 216 | } else 217 | wifi.TCP_Send("nah", 3); 218 | } 219 | vTaskDelay(20); 220 | } 221 | } 222 | } 223 | 224 | void TCP_process(const char * command) 225 | { 226 | WiFi_Controller & wifi = WiFi_Controller::Instance(); 227 | 228 | if (strncmp(command, "calibrate", strlen("calibrate")) == 0) 229 | { 230 | IMU_Detector::Detect_IMU().Accel_Calibrate(); 231 | wifi.TCP_Send("yup", 3); 232 | } else 233 | wifi.TCP_Send("nah", 3); 234 | } -------------------------------------------------------------------------------- /Flyhero_App/partitions.csv: -------------------------------------------------------------------------------- 1 | # Name, Type, SubType, Offset, Size 2 | # Note: if you change the phy_init or app partition offset, make sure to change the offset in Kconfig.projbuild 3 | nvs, data, nvs, 0x9000, 0x6000 4 | phy_init, data, phy, 0xf000, 0x1000 5 | factory, app, factory, 0x10000, 1M 6 | coredump, data, coredump,, 120K 7 | log_storage, 0x40, 0x40, , 2M 8 | -------------------------------------------------------------------------------- /Flyhero_App/sdkconfig: -------------------------------------------------------------------------------- 1 | # 2 | # Automatically generated file; DO NOT EDIT. 3 | # Espressif IoT Development Framework Configuration 4 | # 5 | 6 | # 7 | # SDK tool configuration 8 | # 9 | CONFIG_TOOLPREFIX="xtensa-esp32-elf-" 10 | CONFIG_PYTHON="python" 11 | CONFIG_MAKE_WARN_UNDEFINED_VARIABLES=y 12 | 13 | # 14 | # Bootloader config 15 | # 16 | CONFIG_LOG_BOOTLOADER_LEVEL_NONE= 17 | CONFIG_LOG_BOOTLOADER_LEVEL_ERROR= 18 | CONFIG_LOG_BOOTLOADER_LEVEL_WARN= 19 | CONFIG_LOG_BOOTLOADER_LEVEL_INFO=y 20 | CONFIG_LOG_BOOTLOADER_LEVEL_DEBUG= 21 | CONFIG_LOG_BOOTLOADER_LEVEL_VERBOSE= 22 | CONFIG_LOG_BOOTLOADER_LEVEL=3 23 | CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_8V= 24 | CONFIG_BOOTLOADER_VDDSDIO_BOOST_1_9V=y 25 | CONFIG_BOOTLOADER_FACTORY_RESET= 26 | CONFIG_BOOTLOADER_APP_TEST= 27 | 28 | # 29 | # Security features 30 | # 31 | CONFIG_SECURE_BOOT_ENABLED= 32 | CONFIG_FLASH_ENCRYPTION_ENABLED= 33 | 34 | # 35 | # Serial flasher config 36 | # 37 | CONFIG_ESPTOOLPY_PORT="/dev/ttyUSB0" 38 | CONFIG_ESPTOOLPY_BAUD_115200B= 39 | CONFIG_ESPTOOLPY_BAUD_230400B= 40 | CONFIG_ESPTOOLPY_BAUD_921600B=y 41 | CONFIG_ESPTOOLPY_BAUD_2MB= 42 | CONFIG_ESPTOOLPY_BAUD_OTHER= 43 | CONFIG_ESPTOOLPY_BAUD_OTHER_VAL=115200 44 | CONFIG_ESPTOOLPY_BAUD=921600 45 | CONFIG_ESPTOOLPY_COMPRESSED=y 46 | CONFIG_FLASHMODE_QIO= 47 | CONFIG_FLASHMODE_QOUT= 48 | CONFIG_FLASHMODE_DIO=y 49 | CONFIG_FLASHMODE_DOUT= 50 | CONFIG_ESPTOOLPY_FLASHMODE="dio" 51 | CONFIG_ESPTOOLPY_FLASHFREQ_80M= 52 | CONFIG_ESPTOOLPY_FLASHFREQ_40M=y 53 | CONFIG_ESPTOOLPY_FLASHFREQ_26M= 54 | CONFIG_ESPTOOLPY_FLASHFREQ_20M= 55 | CONFIG_ESPTOOLPY_FLASHFREQ="40m" 56 | CONFIG_ESPTOOLPY_FLASHSIZE_1MB= 57 | CONFIG_ESPTOOLPY_FLASHSIZE_2MB= 58 | CONFIG_ESPTOOLPY_FLASHSIZE_4MB=y 59 | CONFIG_ESPTOOLPY_FLASHSIZE_8MB= 60 | CONFIG_ESPTOOLPY_FLASHSIZE_16MB= 61 | CONFIG_ESPTOOLPY_FLASHSIZE="4MB" 62 | CONFIG_ESPTOOLPY_FLASHSIZE_DETECT=y 63 | CONFIG_ESPTOOLPY_BEFORE_RESET=y 64 | CONFIG_ESPTOOLPY_BEFORE_NORESET= 65 | CONFIG_ESPTOOLPY_BEFORE="default_reset" 66 | CONFIG_ESPTOOLPY_AFTER_RESET=y 67 | CONFIG_ESPTOOLPY_AFTER_NORESET= 68 | CONFIG_ESPTOOLPY_AFTER="hard_reset" 69 | CONFIG_MONITOR_BAUD_9600B= 70 | CONFIG_MONITOR_BAUD_57600B= 71 | CONFIG_MONITOR_BAUD_115200B=y 72 | CONFIG_MONITOR_BAUD_230400B= 73 | CONFIG_MONITOR_BAUD_921600B= 74 | CONFIG_MONITOR_BAUD_2MB= 75 | CONFIG_MONITOR_BAUD_OTHER= 76 | CONFIG_MONITOR_BAUD_OTHER_VAL=115200 77 | CONFIG_MONITOR_BAUD=115200 78 | 79 | # 80 | # Partition Table 81 | # 82 | CONFIG_PARTITION_TABLE_SINGLE_APP= 83 | CONFIG_PARTITION_TABLE_TWO_OTA= 84 | CONFIG_PARTITION_TABLE_CUSTOM=y 85 | CONFIG_PARTITION_TABLE_CUSTOM_FILENAME="partitions.csv" 86 | CONFIG_PARTITION_TABLE_FILENAME="partitions.csv" 87 | CONFIG_PARTITION_TABLE_OFFSET=0x8000 88 | CONFIG_PARTITION_TABLE_MD5=y 89 | 90 | # 91 | # Compiler options 92 | # 93 | CONFIG_OPTIMIZATION_LEVEL_DEBUG=y 94 | CONFIG_OPTIMIZATION_LEVEL_RELEASE= 95 | CONFIG_OPTIMIZATION_ASSERTIONS_ENABLED=y 96 | CONFIG_OPTIMIZATION_ASSERTIONS_SILENT= 97 | CONFIG_OPTIMIZATION_ASSERTIONS_DISABLED= 98 | CONFIG_CXX_EXCEPTIONS= 99 | CONFIG_STACK_CHECK_NONE=y 100 | CONFIG_STACK_CHECK_NORM= 101 | CONFIG_STACK_CHECK_STRONG= 102 | CONFIG_STACK_CHECK_ALL= 103 | CONFIG_STACK_CHECK= 104 | CONFIG_WARN_WRITE_STRINGS= 105 | 106 | # 107 | # Component config 108 | # 109 | 110 | # 111 | # Flyhero IMU 112 | # 113 | CONFIG_FLYHERO_IMU_USE_SOFT_LPF=y 114 | CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF=10 115 | CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF=60 116 | CONFIG_FLYHERO_IMU_USE_NOTCH=y 117 | CONFIG_FLYHERO_IMU_GYRO_NOTCH=140 118 | CONFIG_FLYHERO_IMU_HARD_LPF_256HZ= 119 | CONFIG_FLYHERO_IMU_HARD_LPF_188HZ=y 120 | CONFIG_FLYHERO_IMU_HARD_LPF_98HZ= 121 | CONFIG_FLYHERO_IMU_HARD_LPF_42HZ= 122 | CONFIG_FLYHERO_IMU_HARD_LPF_20HZ= 123 | CONFIG_FLYHERO_IMU_HARD_LPF_10HZ= 124 | CONFIG_FLYHERO_IMU_HARD_LPF_5HZ= 125 | CONFIG_FLYHERO_IMU_ACCEL_FSR_16=y 126 | CONFIG_FLYHERO_IMU_ACCEL_FSR_8= 127 | CONFIG_FLYHERO_IMU_ACCEL_FSR_4= 128 | CONFIG_FLYHERO_IMU_ACCEL_FSR_2= 129 | CONFIG_FLYHERO_IMU_GYRO_FSR_2000=y 130 | CONFIG_FLYHERO_IMU_GYRO_FSR_1000= 131 | CONFIG_FLYHERO_IMU_GYRO_FSR_500= 132 | CONFIG_FLYHERO_IMU_GYRO_FSR_250= 133 | 134 | # 135 | # Flyhero WiFi 136 | # 137 | CONFIG_FLYHERO_WIFI_TCP_PORT=4821 138 | CONFIG_FLYHERO_WIFI_UDP_PORT=4789 139 | CONFIG_FLYHERO_WIFI_SSID="DRONE WIFI" 140 | CONFIG_FLYHERO_WIFI_SSID_VISIBLE=y 141 | CONFIG_FLYHERO_WIFI_WPA2_PASSPHRASE="flyhero00" 142 | 143 | # 144 | # Application Level Tracing 145 | # 146 | CONFIG_ESP32_APPTRACE_DEST_TRAX= 147 | CONFIG_ESP32_APPTRACE_DEST_NONE=y 148 | CONFIG_ESP32_APPTRACE_ENABLE= 149 | CONFIG_ESP32_APPTRACE_LOCK_ENABLE=y 150 | CONFIG_AWS_IOT_SDK= 151 | 152 | # 153 | # Bluetooth 154 | # 155 | CONFIG_BT_ENABLED= 156 | CONFIG_BTDM_CONTROLLER_PINNED_TO_CORE=0 157 | CONFIG_BT_RESERVE_DRAM=0 158 | 159 | # 160 | # Driver configurations 161 | # 162 | 163 | # 164 | # ADC configuration 165 | # 166 | CONFIG_ADC_FORCE_XPD_FSM= 167 | CONFIG_ADC2_DISABLE_DAC=y 168 | 169 | # 170 | # SPI master configuration 171 | # 172 | CONFIG_SPI_MASTER_IN_IRAM= 173 | CONFIG_SPI_MASTER_ISR_IN_IRAM=y 174 | 175 | # 176 | # ESP32-specific 177 | # 178 | CONFIG_ESP32_DEFAULT_CPU_FREQ_80= 179 | CONFIG_ESP32_DEFAULT_CPU_FREQ_160= 180 | CONFIG_ESP32_DEFAULT_CPU_FREQ_240=y 181 | CONFIG_ESP32_DEFAULT_CPU_FREQ_MHZ=240 182 | CONFIG_SPIRAM_SUPPORT= 183 | CONFIG_MEMMAP_TRACEMEM= 184 | CONFIG_MEMMAP_TRACEMEM_TWOBANKS= 185 | CONFIG_ESP32_TRAX= 186 | CONFIG_TRACEMEM_RESERVE_DRAM=0x0 187 | CONFIG_ESP32_ENABLE_COREDUMP_TO_FLASH= 188 | CONFIG_ESP32_ENABLE_COREDUMP_TO_UART= 189 | CONFIG_ESP32_ENABLE_COREDUMP_TO_NONE=y 190 | CONFIG_ESP32_ENABLE_COREDUMP= 191 | CONFIG_TWO_UNIVERSAL_MAC_ADDRESS= 192 | CONFIG_FOUR_UNIVERSAL_MAC_ADDRESS=y 193 | CONFIG_NUMBER_OF_UNIVERSAL_MAC_ADDRESS=4 194 | CONFIG_SYSTEM_EVENT_QUEUE_SIZE=32 195 | CONFIG_SYSTEM_EVENT_TASK_STACK_SIZE=4096 196 | CONFIG_MAIN_TASK_STACK_SIZE=4096 197 | CONFIG_IPC_TASK_STACK_SIZE=1024 198 | CONFIG_TIMER_TASK_STACK_SIZE=4096 199 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_CRLF=y 200 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_LF= 201 | CONFIG_NEWLIB_STDOUT_LINE_ENDING_CR= 202 | CONFIG_NEWLIB_STDIN_LINE_ENDING_CRLF= 203 | CONFIG_NEWLIB_STDIN_LINE_ENDING_LF= 204 | CONFIG_NEWLIB_STDIN_LINE_ENDING_CR=y 205 | CONFIG_NEWLIB_NANO_FORMAT= 206 | CONFIG_CONSOLE_UART_DEFAULT=y 207 | CONFIG_CONSOLE_UART_CUSTOM= 208 | CONFIG_CONSOLE_UART_NONE= 209 | CONFIG_CONSOLE_UART_NUM=0 210 | CONFIG_CONSOLE_UART_BAUDRATE=115200 211 | CONFIG_ULP_COPROC_ENABLED= 212 | CONFIG_ULP_COPROC_RESERVE_MEM=0 213 | CONFIG_ESP32_PANIC_PRINT_HALT= 214 | CONFIG_ESP32_PANIC_PRINT_REBOOT=y 215 | CONFIG_ESP32_PANIC_SILENT_REBOOT= 216 | CONFIG_ESP32_PANIC_GDBSTUB= 217 | CONFIG_ESP32_DEBUG_OCDAWARE=y 218 | CONFIG_ESP32_DEBUG_STUBS_ENABLE=y 219 | CONFIG_INT_WDT= 220 | CONFIG_TASK_WDT= 221 | CONFIG_BROWNOUT_DET=y 222 | CONFIG_BROWNOUT_DET_LVL_SEL_0=y 223 | CONFIG_BROWNOUT_DET_LVL_SEL_1= 224 | CONFIG_BROWNOUT_DET_LVL_SEL_2= 225 | CONFIG_BROWNOUT_DET_LVL_SEL_3= 226 | CONFIG_BROWNOUT_DET_LVL_SEL_4= 227 | CONFIG_BROWNOUT_DET_LVL_SEL_5= 228 | CONFIG_BROWNOUT_DET_LVL_SEL_6= 229 | CONFIG_BROWNOUT_DET_LVL_SEL_7= 230 | CONFIG_BROWNOUT_DET_LVL=0 231 | CONFIG_ESP32_TIME_SYSCALL_USE_RTC_FRC1=y 232 | CONFIG_ESP32_TIME_SYSCALL_USE_RTC= 233 | CONFIG_ESP32_TIME_SYSCALL_USE_FRC1= 234 | CONFIG_ESP32_TIME_SYSCALL_USE_NONE= 235 | CONFIG_ESP32_RTC_CLOCK_SOURCE_INTERNAL_RC=y 236 | CONFIG_ESP32_RTC_CLOCK_SOURCE_EXTERNAL_CRYSTAL= 237 | CONFIG_ESP32_RTC_CLK_CAL_CYCLES=1024 238 | CONFIG_ESP32_DEEP_SLEEP_WAKEUP_DELAY=2000 239 | CONFIG_ESP32_XTAL_FREQ_40=y 240 | CONFIG_ESP32_XTAL_FREQ_26= 241 | CONFIG_ESP32_XTAL_FREQ_AUTO= 242 | CONFIG_ESP32_XTAL_FREQ=40 243 | CONFIG_DISABLE_BASIC_ROM_CONSOLE= 244 | CONFIG_NO_BLOBS= 245 | CONFIG_ESP_TIMER_PROFILING= 246 | CONFIG_COMPATIBLE_PRE_V2_1_BOOTLOADERS= 247 | CONFIG_ESP_ERR_TO_NAME_LOOKUP=y 248 | 249 | # 250 | # Wi-Fi 251 | # 252 | CONFIG_ESP32_WIFI_STATIC_RX_BUFFER_NUM=10 253 | CONFIG_ESP32_WIFI_DYNAMIC_RX_BUFFER_NUM=32 254 | CONFIG_ESP32_WIFI_STATIC_TX_BUFFER= 255 | CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER=y 256 | CONFIG_ESP32_WIFI_TX_BUFFER_TYPE=1 257 | CONFIG_ESP32_WIFI_DYNAMIC_TX_BUFFER_NUM=32 258 | CONFIG_ESP32_WIFI_CSI_ENABLED= 259 | CONFIG_ESP32_WIFI_AMPDU_TX_ENABLED=y 260 | CONFIG_ESP32_WIFI_TX_BA_WIN=6 261 | CONFIG_ESP32_WIFI_AMPDU_RX_ENABLED=y 262 | CONFIG_ESP32_WIFI_RX_BA_WIN=6 263 | CONFIG_ESP32_WIFI_NVS_ENABLED=y 264 | CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_0=y 265 | CONFIG_ESP32_WIFI_TASK_PINNED_TO_CORE_1= 266 | 267 | # 268 | # PHY 269 | # 270 | CONFIG_ESP32_PHY_CALIBRATION_AND_DATA_STORAGE=y 271 | CONFIG_ESP32_PHY_INIT_DATA_IN_PARTITION= 272 | CONFIG_ESP32_PHY_MAX_WIFI_TX_POWER=20 273 | CONFIG_ESP32_PHY_MAX_TX_POWER=20 274 | 275 | # 276 | # Power Management 277 | # 278 | CONFIG_PM_ENABLE= 279 | 280 | # 281 | # ADC-Calibration 282 | # 283 | CONFIG_ADC_CAL_EFUSE_TP_ENABLE=y 284 | CONFIG_ADC_CAL_EFUSE_VREF_ENABLE=y 285 | CONFIG_ADC_CAL_LUT_ENABLE=y 286 | 287 | # 288 | # ESP HTTP client 289 | # 290 | CONFIG_ESP_HTTP_CLIENT_ENABLE_HTTPS= 291 | 292 | # 293 | # Ethernet 294 | # 295 | CONFIG_DMA_RX_BUF_NUM=10 296 | CONFIG_DMA_TX_BUF_NUM=10 297 | CONFIG_EMAC_L2_TO_L3_RX_BUF_MODE= 298 | CONFIG_EMAC_TASK_PRIORITY=20 299 | 300 | # 301 | # FAT Filesystem support 302 | # 303 | CONFIG_FATFS_CODEPAGE_DYNAMIC= 304 | CONFIG_FATFS_CODEPAGE_437=y 305 | CONFIG_FATFS_CODEPAGE_720= 306 | CONFIG_FATFS_CODEPAGE_737= 307 | CONFIG_FATFS_CODEPAGE_771= 308 | CONFIG_FATFS_CODEPAGE_775= 309 | CONFIG_FATFS_CODEPAGE_850= 310 | CONFIG_FATFS_CODEPAGE_852= 311 | CONFIG_FATFS_CODEPAGE_855= 312 | CONFIG_FATFS_CODEPAGE_857= 313 | CONFIG_FATFS_CODEPAGE_860= 314 | CONFIG_FATFS_CODEPAGE_861= 315 | CONFIG_FATFS_CODEPAGE_862= 316 | CONFIG_FATFS_CODEPAGE_863= 317 | CONFIG_FATFS_CODEPAGE_864= 318 | CONFIG_FATFS_CODEPAGE_865= 319 | CONFIG_FATFS_CODEPAGE_866= 320 | CONFIG_FATFS_CODEPAGE_869= 321 | CONFIG_FATFS_CODEPAGE_932= 322 | CONFIG_FATFS_CODEPAGE_936= 323 | CONFIG_FATFS_CODEPAGE_949= 324 | CONFIG_FATFS_CODEPAGE_950= 325 | CONFIG_FATFS_CODEPAGE=437 326 | CONFIG_FATFS_LFN_NONE=y 327 | CONFIG_FATFS_LFN_HEAP= 328 | CONFIG_FATFS_LFN_STACK= 329 | CONFIG_FATFS_FS_LOCK=0 330 | CONFIG_FATFS_TIMEOUT_MS=10000 331 | CONFIG_FATFS_PER_FILE_CACHE=y 332 | 333 | # 334 | # FreeRTOS 335 | # 336 | CONFIG_FREERTOS_UNICORE= 337 | CONFIG_FREERTOS_CORETIMER_0=y 338 | CONFIG_FREERTOS_CORETIMER_1= 339 | CONFIG_FREERTOS_HZ=1000 340 | CONFIG_FREERTOS_ASSERT_ON_UNTESTED_FUNCTION=y 341 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_NONE= 342 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_PTRVAL= 343 | CONFIG_FREERTOS_CHECK_STACKOVERFLOW_CANARY=y 344 | CONFIG_FREERTOS_WATCHPOINT_END_OF_STACK= 345 | CONFIG_FREERTOS_INTERRUPT_BACKTRACE=y 346 | CONFIG_FREERTOS_THREAD_LOCAL_STORAGE_POINTERS=1 347 | CONFIG_FREERTOS_ASSERT_FAIL_ABORT=y 348 | CONFIG_FREERTOS_ASSERT_FAIL_PRINT_CONTINUE= 349 | CONFIG_FREERTOS_ASSERT_DISABLE= 350 | CONFIG_FREERTOS_IDLE_TASK_STACKSIZE=1024 351 | CONFIG_FREERTOS_ISR_STACKSIZE=1536 352 | CONFIG_FREERTOS_LEGACY_HOOKS= 353 | CONFIG_FREERTOS_MAX_TASK_NAME_LEN=16 354 | CONFIG_SUPPORT_STATIC_ALLOCATION= 355 | CONFIG_TIMER_TASK_PRIORITY=1 356 | CONFIG_TIMER_TASK_STACK_DEPTH=2048 357 | CONFIG_TIMER_QUEUE_LENGTH=10 358 | CONFIG_FREERTOS_QUEUE_REGISTRY_SIZE=0 359 | CONFIG_FREERTOS_USE_TRACE_FACILITY= 360 | CONFIG_FREERTOS_GENERATE_RUN_TIME_STATS= 361 | CONFIG_FREERTOS_DEBUG_INTERNALS= 362 | 363 | # 364 | # Heap memory debugging 365 | # 366 | CONFIG_HEAP_POISONING_DISABLED=y 367 | CONFIG_HEAP_POISONING_LIGHT= 368 | CONFIG_HEAP_POISONING_COMPREHENSIVE= 369 | CONFIG_HEAP_TRACING= 370 | 371 | # 372 | # libsodium 373 | # 374 | CONFIG_LIBSODIUM_USE_MBEDTLS_SHA=y 375 | 376 | # 377 | # Log output 378 | # 379 | CONFIG_LOG_DEFAULT_LEVEL_NONE= 380 | CONFIG_LOG_DEFAULT_LEVEL_ERROR= 381 | CONFIG_LOG_DEFAULT_LEVEL_WARN= 382 | CONFIG_LOG_DEFAULT_LEVEL_INFO=y 383 | CONFIG_LOG_DEFAULT_LEVEL_DEBUG= 384 | CONFIG_LOG_DEFAULT_LEVEL_VERBOSE= 385 | CONFIG_LOG_DEFAULT_LEVEL=3 386 | CONFIG_LOG_COLORS=y 387 | 388 | # 389 | # LWIP 390 | # 391 | CONFIG_L2_TO_L3_COPY= 392 | CONFIG_LWIP_IRAM_OPTIMIZATION= 393 | CONFIG_LWIP_MAX_SOCKETS=10 394 | CONFIG_USE_ONLY_LWIP_SELECT= 395 | CONFIG_LWIP_SO_REUSE= 396 | CONFIG_LWIP_SO_RCVBUF= 397 | CONFIG_LWIP_DHCP_MAX_NTP_SERVERS=1 398 | CONFIG_LWIP_IP_FRAG= 399 | CONFIG_LWIP_IP_REASSEMBLY= 400 | CONFIG_LWIP_STATS= 401 | CONFIG_LWIP_ETHARP_TRUST_IP_MAC=y 402 | CONFIG_TCPIP_RECVMBOX_SIZE=32 403 | CONFIG_LWIP_DHCP_DOES_ARP_CHECK=y 404 | 405 | # 406 | # DHCP server 407 | # 408 | CONFIG_LWIP_DHCPS_LEASE_UNIT=60 409 | CONFIG_LWIP_DHCPS_MAX_STATION_NUM=8 410 | CONFIG_LWIP_AUTOIP= 411 | CONFIG_LWIP_NETIF_LOOPBACK=y 412 | CONFIG_LWIP_LOOPBACK_MAX_PBUFS=8 413 | 414 | # 415 | # TCP 416 | # 417 | CONFIG_LWIP_MAX_ACTIVE_TCP=16 418 | CONFIG_LWIP_MAX_LISTENING_TCP=16 419 | CONFIG_TCP_MAXRTX=12 420 | CONFIG_TCP_SYNMAXRTX=6 421 | CONFIG_TCP_MSS=1436 422 | CONFIG_TCP_MSL=60000 423 | CONFIG_TCP_SND_BUF_DEFAULT=5744 424 | CONFIG_TCP_WND_DEFAULT=5744 425 | CONFIG_TCP_RECVMBOX_SIZE=6 426 | CONFIG_TCP_QUEUE_OOSEQ=y 427 | CONFIG_ESP_TCP_KEEP_CONNECTION_WHEN_IP_CHANGES= 428 | CONFIG_TCP_OVERSIZE_MSS=y 429 | CONFIG_TCP_OVERSIZE_QUARTER_MSS= 430 | CONFIG_TCP_OVERSIZE_DISABLE= 431 | 432 | # 433 | # UDP 434 | # 435 | CONFIG_LWIP_MAX_UDP_PCBS=16 436 | CONFIG_UDP_RECVMBOX_SIZE=6 437 | CONFIG_TCPIP_TASK_STACK_SIZE=2560 438 | CONFIG_PPP_SUPPORT= 439 | 440 | # 441 | # ICMP 442 | # 443 | CONFIG_LWIP_MULTICAST_PING= 444 | CONFIG_LWIP_BROADCAST_PING= 445 | 446 | # 447 | # LWIP RAW API 448 | # 449 | CONFIG_LWIP_MAX_RAW_PCBS=16 450 | 451 | # 452 | # mbedTLS 453 | # 454 | CONFIG_MBEDTLS_SSL_MAX_CONTENT_LEN=16384 455 | CONFIG_MBEDTLS_DEBUG= 456 | CONFIG_MBEDTLS_HARDWARE_AES=y 457 | CONFIG_MBEDTLS_HARDWARE_MPI= 458 | CONFIG_MBEDTLS_HARDWARE_SHA= 459 | CONFIG_MBEDTLS_HAVE_TIME=y 460 | CONFIG_MBEDTLS_HAVE_TIME_DATE= 461 | CONFIG_MBEDTLS_TLS_SERVER_AND_CLIENT=y 462 | CONFIG_MBEDTLS_TLS_SERVER_ONLY= 463 | CONFIG_MBEDTLS_TLS_CLIENT_ONLY= 464 | CONFIG_MBEDTLS_TLS_DISABLED= 465 | CONFIG_MBEDTLS_TLS_SERVER=y 466 | CONFIG_MBEDTLS_TLS_CLIENT=y 467 | CONFIG_MBEDTLS_TLS_ENABLED=y 468 | 469 | # 470 | # TLS Key Exchange Methods 471 | # 472 | CONFIG_MBEDTLS_PSK_MODES= 473 | CONFIG_MBEDTLS_KEY_EXCHANGE_RSA=y 474 | CONFIG_MBEDTLS_KEY_EXCHANGE_DHE_RSA=y 475 | CONFIG_MBEDTLS_KEY_EXCHANGE_ELLIPTIC_CURVE=y 476 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_RSA=y 477 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDHE_ECDSA=y 478 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_ECDSA=y 479 | CONFIG_MBEDTLS_KEY_EXCHANGE_ECDH_RSA=y 480 | CONFIG_MBEDTLS_SSL_RENEGOTIATION=y 481 | CONFIG_MBEDTLS_SSL_PROTO_SSL3= 482 | CONFIG_MBEDTLS_SSL_PROTO_TLS1=y 483 | CONFIG_MBEDTLS_SSL_PROTO_TLS1_1=y 484 | CONFIG_MBEDTLS_SSL_PROTO_TLS1_2=y 485 | CONFIG_MBEDTLS_SSL_PROTO_DTLS= 486 | CONFIG_MBEDTLS_SSL_ALPN=y 487 | CONFIG_MBEDTLS_SSL_SESSION_TICKETS=y 488 | 489 | # 490 | # Symmetric Ciphers 491 | # 492 | CONFIG_MBEDTLS_AES_C=y 493 | CONFIG_MBEDTLS_CAMELLIA_C= 494 | CONFIG_MBEDTLS_DES_C= 495 | CONFIG_MBEDTLS_RC4_DISABLED=y 496 | CONFIG_MBEDTLS_RC4_ENABLED_NO_DEFAULT= 497 | CONFIG_MBEDTLS_RC4_ENABLED= 498 | CONFIG_MBEDTLS_BLOWFISH_C= 499 | CONFIG_MBEDTLS_XTEA_C= 500 | CONFIG_MBEDTLS_CCM_C=y 501 | CONFIG_MBEDTLS_GCM_C=y 502 | CONFIG_MBEDTLS_RIPEMD160_C= 503 | 504 | # 505 | # Certificates 506 | # 507 | CONFIG_MBEDTLS_PEM_PARSE_C=y 508 | CONFIG_MBEDTLS_PEM_WRITE_C=y 509 | CONFIG_MBEDTLS_X509_CRL_PARSE_C=y 510 | CONFIG_MBEDTLS_X509_CSR_PARSE_C=y 511 | CONFIG_MBEDTLS_ECP_C=y 512 | CONFIG_MBEDTLS_ECDH_C=y 513 | CONFIG_MBEDTLS_ECDSA_C=y 514 | CONFIG_MBEDTLS_ECP_DP_SECP192R1_ENABLED=y 515 | CONFIG_MBEDTLS_ECP_DP_SECP224R1_ENABLED=y 516 | CONFIG_MBEDTLS_ECP_DP_SECP256R1_ENABLED=y 517 | CONFIG_MBEDTLS_ECP_DP_SECP384R1_ENABLED=y 518 | CONFIG_MBEDTLS_ECP_DP_SECP521R1_ENABLED=y 519 | CONFIG_MBEDTLS_ECP_DP_SECP192K1_ENABLED=y 520 | CONFIG_MBEDTLS_ECP_DP_SECP224K1_ENABLED=y 521 | CONFIG_MBEDTLS_ECP_DP_SECP256K1_ENABLED=y 522 | CONFIG_MBEDTLS_ECP_DP_BP256R1_ENABLED=y 523 | CONFIG_MBEDTLS_ECP_DP_BP384R1_ENABLED=y 524 | CONFIG_MBEDTLS_ECP_DP_BP512R1_ENABLED=y 525 | CONFIG_MBEDTLS_ECP_DP_CURVE25519_ENABLED=y 526 | CONFIG_MBEDTLS_ECP_NIST_OPTIM=y 527 | 528 | # 529 | # mDNS 530 | # 531 | CONFIG_MDNS_MAX_SERVICES=10 532 | 533 | # 534 | # OpenSSL 535 | # 536 | CONFIG_OPENSSL_DEBUG= 537 | CONFIG_OPENSSL_ASSERT_DO_NOTHING=y 538 | CONFIG_OPENSSL_ASSERT_EXIT= 539 | 540 | # 541 | # PThreads 542 | # 543 | CONFIG_ESP32_PTHREAD_TASK_PRIO_DEFAULT=5 544 | CONFIG_ESP32_PTHREAD_TASK_STACK_SIZE_DEFAULT=2048 545 | 546 | # 547 | # SPI Flash driver 548 | # 549 | CONFIG_SPI_FLASH_VERIFY_WRITE= 550 | CONFIG_SPI_FLASH_ENABLE_COUNTERS= 551 | CONFIG_SPI_FLASH_ROM_DRIVER_PATCH=y 552 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ABORTS=y 553 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_FAILS= 554 | CONFIG_SPI_FLASH_WRITING_DANGEROUS_REGIONS_ALLOWED= 555 | 556 | # 557 | # SPIFFS Configuration 558 | # 559 | CONFIG_SPIFFS_MAX_PARTITIONS=3 560 | 561 | # 562 | # SPIFFS Cache Configuration 563 | # 564 | CONFIG_SPIFFS_CACHE=y 565 | CONFIG_SPIFFS_CACHE_WR=y 566 | CONFIG_SPIFFS_CACHE_STATS= 567 | CONFIG_SPIFFS_PAGE_CHECK=y 568 | CONFIG_SPIFFS_GC_MAX_RUNS=10 569 | CONFIG_SPIFFS_GC_STATS= 570 | CONFIG_SPIFFS_PAGE_SIZE=256 571 | CONFIG_SPIFFS_OBJ_NAME_LEN=32 572 | CONFIG_SPIFFS_USE_MAGIC=y 573 | CONFIG_SPIFFS_USE_MAGIC_LENGTH=y 574 | CONFIG_SPIFFS_META_LENGTH=4 575 | CONFIG_SPIFFS_USE_MTIME=y 576 | 577 | # 578 | # Debug Configuration 579 | # 580 | CONFIG_SPIFFS_DBG= 581 | CONFIG_SPIFFS_API_DBG= 582 | CONFIG_SPIFFS_GC_DBG= 583 | CONFIG_SPIFFS_CACHE_DBG= 584 | CONFIG_SPIFFS_CHECK_DBG= 585 | CONFIG_SPIFFS_TEST_VISUALISATION= 586 | 587 | # 588 | # tcpip adapter 589 | # 590 | CONFIG_IP_LOST_TIMER_INTERVAL=120 591 | 592 | # 593 | # Virtual file system 594 | # 595 | CONFIG_SUPPRESS_SELECT_DEBUG_OUTPUT=y 596 | 597 | # 598 | # Wear Levelling 599 | # 600 | CONFIG_WL_SECTOR_SIZE_512= 601 | CONFIG_WL_SECTOR_SIZE_4096=y 602 | CONFIG_WL_SECTOR_SIZE=4096 603 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 michprev 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /components/Barometer/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/Barometer/include/MS5611.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MS5611.h 3 | * 4 | * Created on: 11. 12. 2016 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace flyhero 18 | { 19 | 20 | class MS5611 21 | { 22 | private: 23 | MS5611(); 24 | 25 | MS5611(MS5611 const &); 26 | 27 | MS5611 &operator=(MS5611 const &); 28 | 29 | spi_device_handle_t spi; 30 | uint16_t c[6]; 31 | uint32_t d1; 32 | uint32_t d2; 33 | 34 | esp_err_t spi_init(); 35 | 36 | esp_err_t reset(); 37 | 38 | esp_err_t load_prom(); 39 | 40 | esp_err_t read_d1(); 41 | 42 | esp_err_t read_d2(); 43 | 44 | public: 45 | static MS5611 &Instance(); 46 | 47 | void Init(); 48 | 49 | void Get_Data(int32_t &temperature, int32_t &pressure); 50 | }; 51 | 52 | } 53 | -------------------------------------------------------------------------------- /components/Barometer/src/MS5611.cpp: -------------------------------------------------------------------------------- 1 | #include "MS5611.h" 2 | 3 | 4 | namespace flyhero 5 | { 6 | 7 | MS5611 &MS5611::Instance() 8 | { 9 | static MS5611 instance; 10 | 11 | return instance; 12 | } 13 | 14 | MS5611::MS5611() 15 | { 16 | this->spi = NULL; 17 | std::memset(this->c, 0, 6); 18 | this->d1 = 0; 19 | this->d2 = 0; 20 | } 21 | 22 | esp_err_t MS5611::spi_init() 23 | { 24 | esp_err_t ret; 25 | 26 | spi_bus_config_t buscfg; 27 | buscfg.miso_io_num = GPIO_NUM_19; 28 | buscfg.mosi_io_num = GPIO_NUM_22; 29 | buscfg.sclk_io_num = GPIO_NUM_23; 30 | buscfg.quadwp_io_num = -1; 31 | buscfg.quadhd_io_num = -1; 32 | buscfg.max_transfer_sz = 0; 33 | 34 | spi_device_interface_config_t devcfg; 35 | devcfg.command_bits = 8; 36 | devcfg.address_bits = 0; 37 | devcfg.dummy_bits = 0; 38 | devcfg.mode = 0; 39 | devcfg.duty_cycle_pos = 128; 40 | devcfg.cs_ena_pretrans = 0; 41 | devcfg.cs_ena_posttrans = 0; 42 | devcfg.clock_speed_hz = 20000000; 43 | devcfg.spics_io_num = GPIO_NUM_21; 44 | devcfg.flags = 0; 45 | devcfg.queue_size = 7; 46 | devcfg.pre_cb = 0; 47 | devcfg.post_cb = 0; 48 | 49 | //Initialize the SPI bus 50 | if ((ret = spi_bus_initialize(VSPI_HOST, &buscfg, 0))) 51 | return ret; 52 | 53 | if ((ret = spi_bus_add_device(VSPI_HOST, &devcfg, &this->spi))) 54 | return ret; 55 | 56 | return ESP_OK; 57 | } 58 | 59 | esp_err_t MS5611::reset() 60 | { 61 | spi_transaction_t trans; 62 | trans.flags = 0; 63 | trans.cmd = 0x1E; 64 | trans.addr = 0; 65 | trans.length = 0; 66 | trans.rxlength = 0; 67 | trans.user = 0; 68 | trans.tx_buffer = NULL; 69 | trans.rx_buffer = NULL; 70 | 71 | return spi_device_transmit(this->spi, &trans); 72 | } 73 | 74 | esp_err_t MS5611::load_prom() 75 | { 76 | esp_err_t ret; 77 | 78 | spi_transaction_t trans; 79 | trans.flags = SPI_TRANS_USE_RXDATA; 80 | trans.cmd = 0; 81 | trans.addr = 0; 82 | trans.length = 16; 83 | trans.rxlength = 16; 84 | trans.user = 0; 85 | trans.tx_buffer = NULL; 86 | 87 | for (uint8_t i = 0; i < 6; i++) 88 | { 89 | trans.cmd = 0xA2 + i * 2; 90 | 91 | if ((ret = spi_device_transmit(this->spi, &trans))) 92 | return ret; 93 | 94 | this->c[i] = trans.rx_data[0] << 8; 95 | this->c[i] |= trans.rx_data[1]; 96 | } 97 | 98 | return ESP_OK; 99 | } 100 | 101 | esp_err_t MS5611::read_d1() 102 | { 103 | esp_err_t ret; 104 | 105 | // convert D1 (OSR = 4096) 106 | spi_transaction_t trans; 107 | trans.flags = 0; 108 | trans.cmd = 0x48; 109 | trans.addr = 0; 110 | trans.length = 0; 111 | trans.rxlength = 0; 112 | trans.user = 0; 113 | trans.rx_buffer = NULL; 114 | trans.tx_buffer = NULL; 115 | 116 | if ((ret = spi_device_transmit(this->spi, &trans))) 117 | return ret; 118 | 119 | // not supported yet 120 | //TickType_t current_ticks = xTaskGetTickCount(); 121 | //vTaskDelayUntil(¤t_ticks, 10 / portTICK_RATE_MS); 122 | 123 | // wait until conversion done 124 | vTaskDelay(10 / portTICK_RATE_MS); 125 | 126 | // read converted D1 value 127 | trans.flags = SPI_TRANS_USE_RXDATA; 128 | trans.cmd = 0x00; 129 | trans.addr = 0; 130 | trans.length = 24; 131 | trans.rxlength = 24; 132 | trans.user = 0; 133 | trans.tx_buffer = NULL; 134 | 135 | if ((ret = spi_device_transmit(this->spi, &trans))) 136 | return ret; 137 | 138 | this->d1 = trans.rx_data[0] << 16; 139 | this->d1 |= trans.rx_data[1] << 8; 140 | this->d1 |= trans.rx_data[2]; 141 | 142 | return ESP_OK; 143 | } 144 | 145 | esp_err_t MS5611::read_d2() 146 | { 147 | esp_err_t ret; 148 | 149 | // convert D2 (OSR = 4096) 150 | spi_transaction_t trans; 151 | trans.flags = 0; 152 | trans.cmd = 0x58; 153 | trans.addr = 0; 154 | trans.length = 0; 155 | trans.rxlength = 0; 156 | trans.user = 0; 157 | trans.rx_buffer = NULL; 158 | trans.tx_buffer = NULL; 159 | 160 | if ((ret = spi_device_transmit(this->spi, &trans))) 161 | return ret; 162 | 163 | // wait until conversion done 164 | vTaskDelay(10 / portTICK_RATE_MS); 165 | 166 | // not supported yet 167 | //TickType_t current_ticks = xTaskGetTickCount(); 168 | //vTaskDelayUntil(¤t_ticks, 10 / portTICK_RATE_MS); 169 | 170 | // read converted D2 value 171 | trans.flags = SPI_TRANS_USE_RXDATA; 172 | trans.cmd = 0x00; 173 | trans.addr = 0; 174 | trans.length = 24; 175 | trans.rxlength = 24; 176 | trans.user = 0; 177 | trans.tx_buffer = NULL; 178 | 179 | if ((ret = spi_device_transmit(this->spi, &trans))) 180 | return ret; 181 | 182 | this->d2 = trans.rx_data[0] << 16; 183 | this->d2 |= trans.rx_data[1] << 8; 184 | this->d2 |= trans.rx_data[2]; 185 | 186 | return ESP_OK; 187 | } 188 | 189 | void MS5611::Init() 190 | { 191 | 192 | ESP_ERROR_CHECK(this->spi_init()); 193 | 194 | ESP_ERROR_CHECK(this->reset()); 195 | 196 | // wait until reset done 197 | vTaskDelay(20 / portTICK_RATE_MS); 198 | 199 | ESP_ERROR_CHECK(this->load_prom()); 200 | } 201 | 202 | void MS5611::Get_Data(int32_t &temperature, int32_t &pressure) 203 | { 204 | ESP_ERROR_CHECK(this->read_d1()); 205 | 206 | ESP_ERROR_CHECK(this->read_d2()); 207 | 208 | int32_t d_t = this->d2 - (uint32_t) this->c[4] * 256; 209 | int32_t temp = 2000 + ((int64_t) d_t * this->c[5]) / 8388608; 210 | 211 | int64_t off = (int64_t) this->c[1] * 65536 + (int64_t) this->c[3] * d_t / 128; 212 | int64_t sens = (int64_t) this->c[0] * 32768 + (int64_t) this->c[2] * d_t / 256; 213 | 214 | int32_t temp_2 = 0; 215 | int64_t off_2 = 0; 216 | int64_t sens_2 = 0; 217 | 218 | if (temp < 2000) 219 | { 220 | temp_2 = ((int64_t) d_t * d_t) / 2147483648; 221 | off_2 = 5 * ((temp - 2000) * (temp - 2000)) / 2; 222 | sens_2 = 5 * ((temp - 2000) * (temp - 2000)) / 4; 223 | } 224 | if (temp < -1500) 225 | { 226 | off_2 = off_2 + 7 * (temp + 1500) * (temp + 1500); 227 | sens_2 = sens_2 + 11 * (temp + 1500) * (temp + 1500) / 2; 228 | } 229 | 230 | temp -= temp_2; 231 | off -= off_2; 232 | sens -= sens_2; 233 | 234 | int32_t p = (this->d1 * sens / 2097152 - off) / 32768; 235 | 236 | temperature = temp; 237 | pressure = p; 238 | } 239 | 240 | } 241 | -------------------------------------------------------------------------------- /components/IMU/Kconfig: -------------------------------------------------------------------------------- 1 | menu "Flyhero IMU" 2 | 3 | config FLYHERO_IMU_USE_SOFT_LPF 4 | bool "Use software LPF" 5 | default y 6 | 7 | config FLYHERO_IMU_ACCEL_SOFT_LPF 8 | int "Accelerometer software LPF" 9 | range 1 1000 10 | default 10 11 | depends on FLYHERO_IMU_USE_SOFT_LPF 12 | 13 | config FLYHERO_IMU_GYRO_SOFT_LPF 14 | int "Gyroscope software LPF" 15 | range 1 1000 16 | default 60 17 | depends on FLYHERO_IMU_USE_SOFT_LPF 18 | 19 | config FLYHERO_IMU_USE_NOTCH 20 | bool "Use gyro Notch" 21 | default y 22 | 23 | config FLYHERO_IMU_GYRO_NOTCH 24 | int "Gyro Notch frequency" 25 | range 1 500 26 | default 140 27 | depends on FLYHERO_IMU_USE_NOTCH 28 | 29 | choice FLYHERO_IMU_HARD_LPF 30 | prompt "Hardware LPF" 31 | default FLYHERO_IMU_HARD_LPF_256HZ 32 | 33 | config FLYHERO_IMU_HARD_LPF_256HZ 34 | bool "256 Hz" 35 | config FLYHERO_IMU_HARD_LPF_188HZ 36 | bool "188 Hz" 37 | config FLYHERO_IMU_HARD_LPF_98HZ 38 | bool "98 Hz" 39 | config FLYHERO_IMU_HARD_LPF_42HZ 40 | bool "42 Hz" 41 | config FLYHERO_IMU_HARD_LPF_20HZ 42 | bool "20 Hz" 43 | config FLYHERO_IMU_HARD_LPF_10HZ 44 | bool "10 Hz" 45 | config FLYHERO_IMU_HARD_LPF_5HZ 46 | bool "5 Hz" 47 | 48 | endchoice 49 | 50 | choice FLYHERO_IMU_ACCEL_FSR 51 | prompt "Accelerometer full-scale range" 52 | default FLYHERO_IMU_ACCEL_FSR_16 53 | 54 | config FLYHERO_IMU_ACCEL_FSR_16 55 | bool "+- 16g" 56 | config FLYHERO_IMU_ACCEL_FSR_8 57 | bool "+- 8g" 58 | config FLYHERO_IMU_ACCEL_FSR_4 59 | bool "+- 4g" 60 | config FLYHERO_IMU_ACCEL_FSR_2 61 | bool "+- 2g" 62 | 63 | endchoice 64 | 65 | choice FLYHERO_IMU_GYRO_FSR 66 | prompt "Gyroscope full-scale range" 67 | default FLYHERO_IMU_GYRO_FSR_2000 68 | 69 | config FLYHERO_IMU_GYRO_FSR_2000 70 | bool "+- 2000 deg/sec" 71 | config FLYHERO_IMU_GYRO_FSR_1000 72 | bool "+- 1000 deg/sec" 73 | config FLYHERO_IMU_GYRO_FSR_500 74 | bool "+- 500 deg/sec" 75 | config FLYHERO_IMU_GYRO_FSR_250 76 | bool "+- 250 deg/sec" 77 | 78 | endchoice 79 | 80 | endmenu 81 | -------------------------------------------------------------------------------- /components/IMU/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/IMU/include/Biquad_Filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Biquad_Filter.h 3 | * 4 | * Created on: 24. 6. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "Math.h" 13 | 14 | 15 | namespace flyhero 16 | { 17 | 18 | // Many thanks to http://www.earlevel.com/main/2012/11/26/biquad-c-source-code/ 19 | 20 | class Biquad_Filter 21 | { 22 | private: 23 | float a0, a1, a2; 24 | float b1, b2; 25 | float z1, z2; 26 | 27 | public: 28 | enum Filter_Type 29 | { 30 | FILTER_LOW_PASS, FILTER_NOTCH 31 | }; 32 | 33 | Biquad_Filter(Filter_Type type, float sample_frequency, float cut_frequency); 34 | 35 | inline float Apply_Filter(float value); 36 | }; 37 | 38 | // https://en.wikipedia.org/wiki/Digital_biquad_filter - Transposed direct forms 39 | float Biquad_Filter::Apply_Filter(float value) 40 | { 41 | float ret = value * this->a0 + this->z1; 42 | this->z1 = value * this->a1 + this->z2 - this->b1 * ret; 43 | this->z2 = value * this->a2 - this->b2 * ret; 44 | 45 | return ret; 46 | } 47 | 48 | } /* namespace flyhero */ 49 | -------------------------------------------------------------------------------- /components/IMU/include/Complementary_Filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Complementary_Filter.h 3 | * 4 | * Created on: 10. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | #include "Fusion_Filter.h" 13 | #include "Math.h" 14 | 15 | 16 | namespace flyhero 17 | { 18 | 19 | class Complementary_Filter : public Fusion_Filter 20 | { 21 | private: 22 | IMU::Euler_Angles euler; 23 | int64_t last_time; 24 | const float COMPLEMENTARY_COEFFICIENT; 25 | 26 | public: 27 | Complementary_Filter(float coeff); 28 | 29 | ~Complementary_Filter() override = default; 30 | 31 | void Compute(IMU::Sensor_Data accel, IMU::Sensor_Data gyro, IMU::Euler_Angles &euler) override; 32 | 33 | void Reset() override; 34 | }; 35 | 36 | } /* namespace flyhero */ 37 | -------------------------------------------------------------------------------- /components/IMU/include/Fusion_Filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Fusion_Filter.h 3 | * 4 | * Created on: 10. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "IMU.h" 11 | 12 | 13 | namespace flyhero 14 | { 15 | 16 | class Fusion_Filter 17 | { 18 | public: 19 | virtual ~Fusion_Filter() = 0; 20 | 21 | virtual void Compute(IMU::Sensor_Data accel, IMU::Sensor_Data gyro, IMU::Euler_Angles &euler) = 0; 22 | 23 | virtual void Reset() = 0; 24 | }; 25 | 26 | inline Fusion_Filter::~Fusion_Filter() 27 | { 28 | 29 | } 30 | 31 | } /* namespace flyhero */ 32 | -------------------------------------------------------------------------------- /components/IMU/include/IMU.h: -------------------------------------------------------------------------------- 1 | /* 2 | * IMU.h 3 | * 4 | * Created on: 10. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | 13 | namespace flyhero 14 | { 15 | 16 | class IMU 17 | { 18 | public: 19 | struct __attribute__((__packed__)) Raw_Data 20 | { 21 | int16_t x, y, z; 22 | }; 23 | 24 | struct __attribute__((__packed__)) Sensor_Data 25 | { 26 | float x, y, z; 27 | }; 28 | 29 | struct __attribute__((__packed__)) Euler_Angles 30 | { 31 | float roll, pitch, yaw; 32 | }; 33 | 34 | struct __attribute__((__packed__)) Quaternion 35 | { 36 | float q0, q1, q2, q3; 37 | }; 38 | 39 | enum class Read_Data_Type 40 | { 41 | GYRO_ONLY, 42 | ACCEL_GYRO 43 | }; 44 | 45 | virtual void Init() = 0; 46 | 47 | virtual bool Start() = 0; 48 | 49 | virtual void Stop() = 0; 50 | 51 | virtual void Accel_Calibrate() = 0; 52 | 53 | virtual void Gyro_Calibrate() = 0; 54 | 55 | virtual float Get_Accel_Sample_Rate() = 0; 56 | 57 | virtual float Get_Gyro_Sample_Rate() = 0; 58 | 59 | virtual uint8_t Get_Sample_Rates_Ratio() = 0; 60 | 61 | virtual void Read_Raw(Raw_Data &raw_accel, Raw_Data &raw_gyro) = 0; 62 | 63 | virtual Read_Data_Type Read_Data(Sensor_Data &accel, Sensor_Data &gyro) = 0; 64 | 65 | virtual void Data_Ready_Callback() = 0; 66 | 67 | virtual bool Data_Ready() = 0; 68 | }; 69 | 70 | } /* namespace flyhero */ 71 | -------------------------------------------------------------------------------- /components/IMU/include/IMU_Detector.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 22.10.17. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | 10 | #include "IMU.h" 11 | #include "MPU6050.h" 12 | #include "MPU9250.h" 13 | #include "MPU6000.h" 14 | 15 | 16 | namespace flyhero 17 | { 18 | 19 | class IMU_Detector 20 | { 21 | private: 22 | static spi_device_handle_t spi; 23 | static IMU *imu; 24 | static bool detected; 25 | 26 | static bool i2c_init(); 27 | 28 | static void i2c_deinit(); 29 | 30 | static bool spi_init(); 31 | 32 | static void spi_deinit(); 33 | 34 | static bool try_i2c_imu(const uint8_t DEVICE_ADDRESS_WRITE, const uint8_t DEVICE_ADDRESS_READ, 35 | const uint8_t WHO_AM_I_REGISTER, const uint8_t EXPECTED_VALUE); 36 | 37 | static bool try_spi_imu(const uint8_t WHO_AM_I_REGISTER, const uint8_t EXPECTED_VALUE, const gpio_num_t CS_NUM); 38 | 39 | public: 40 | static IMU &Detect_IMU(); 41 | 42 | }; 43 | 44 | } 45 | -------------------------------------------------------------------------------- /components/IMU/include/MPU6000.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6000.h 3 | * 4 | * Created on: 18. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "Biquad_Filter.h" 15 | #include "IMU.h" 16 | #include "Counting_Median_Finder.h" 17 | 18 | 19 | namespace flyhero 20 | { 21 | 22 | class MPU6000 : public IMU 23 | { 24 | private: 25 | MPU6000(); 26 | 27 | MPU6000(MPU6000 const &); 28 | 29 | MPU6000 &operator=(MPU6000 const &); 30 | 31 | enum gyro_fsr 32 | { 33 | GYRO_FSR_250 = 0x00, 34 | GYRO_FSR_500 = 0x08, 35 | GYRO_FSR_1000 = 0x10, 36 | GYRO_FSR_2000 = 0x18, 37 | GYRO_FSR_NOT_SET = 0xFF 38 | }; 39 | 40 | enum accel_fsr 41 | { 42 | ACCEL_FSR_2 = 0x00, 43 | ACCEL_FSR_4 = 0x08, 44 | ACCEL_FSR_8 = 0x10, 45 | ACCEL_FSR_16 = 0x18, 46 | ACCEL_FSR_NOT_SET = 0xFF 47 | }; 48 | 49 | enum lpf_bandwidth 50 | { 51 | // 8 kHz 52 | LPF_256HZ = 0x00, 53 | // 1 kHz 54 | LPF_188HZ = 0x01, 55 | LPF_98HZ = 0x02, 56 | LPF_42HZ = 0x03, 57 | LPF_20HZ = 0x04, 58 | LPF_10HZ = 0x05, 59 | LPF_5HZ = 0x06, 60 | LPF_NOT_SET = 0xFF 61 | }; 62 | 63 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 64 | const float GYRO_SAMPLE_RATE = 8000; 65 | const float ACCEL_SAMPLE_RATE = 1000; 66 | const uint8_t SAMPLE_RATES_RATIO = 8; 67 | #else 68 | const float GYRO_SAMPLE_RATE = 1000; 69 | const float ACCEL_SAMPLE_RATE = 1000; 70 | const uint8_t SAMPLE_RATES_RATIO = 1; 71 | #endif 72 | 73 | #if CONFIG_FLYHERO_IMU_GYRO_FSR_250 74 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_250; 75 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_500 76 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_500; 77 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_1000 78 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_1000; 79 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_2000 80 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_2000; 81 | #else 82 | #error "Gyro FSR not set" 83 | #endif 84 | 85 | #if CONFIG_FLYHERO_IMU_ACCEL_FSR_2 86 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_2; 87 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_4 88 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_4; 89 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_8 90 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_8; 91 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_16 92 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_16; 93 | #else 94 | #error "Accel FSR not set" 95 | #endif 96 | 97 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 98 | const lpf_bandwidth TARGET_LPF = LPF_256HZ; 99 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_188HZ 100 | const lpf_bandwidth TARGET_LPF = LPF_188HZ; 101 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_98HZ 102 | const lpf_bandwidth TARGET_LPF = LPF_98HZ; 103 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_42HZ 104 | const lpf_bandwidth TARGET_LPF = LPF_42HZ; 105 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_20HZ 106 | const lpf_bandwidth TARGET_LPF = LPF_20HZ; 107 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_10HZ 108 | const lpf_bandwidth TARGET_LPF = LPF_10HZ; 109 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_5HZ 110 | const lpf_bandwidth TARGET_LPF = LPF_5HZ; 111 | #else 112 | #error "Gyro hardware LPF not set" 113 | #endif 114 | 115 | const uint8_t ADC_BITS = 16; 116 | 117 | const struct 118 | { 119 | uint8_t ACCEL_X_OFFSET = 0x06; 120 | uint8_t GYRO_X_OFFSET = 0x13; 121 | uint8_t SMPRT_DIV = 0x19; 122 | uint8_t CONFIG = 0x1A; 123 | uint8_t GYRO_CONFIG = 0x1B; 124 | uint8_t ACCEL_CONFIG = 0x1C; 125 | uint8_t FIFO_EN = 0x23; 126 | uint8_t INT_PIN_CFG = 0x37; 127 | uint8_t INT_ENABLE = 0x38; 128 | uint8_t INT_STATUS = 0x3A; 129 | uint8_t ACCEL_XOUT_H = 0x3B; 130 | uint8_t GYRO_XOUT_H = 0x43; 131 | uint8_t SIGNAL_PATH_RESET = 0x68; 132 | uint8_t USER_CTRL = 0x6A; 133 | uint8_t PWR_MGMT_1 = 0x6B; 134 | uint8_t PWR_MGMT_2 = 0x6C; 135 | uint8_t FIFO_COUNT_H = 0x72; 136 | uint8_t FIFO_COUNT_L = 0x73; 137 | uint8_t FIFO_R_W = 0x74; 138 | uint8_t WHO_AM_I = 0x75; 139 | } REGISTERS; 140 | 141 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 142 | Biquad_Filter accel_x_filter, accel_y_filter, accel_z_filter; 143 | Biquad_Filter gyro_x_filter, gyro_y_filter, gyro_z_filter; 144 | #endif 145 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 146 | Biquad_Filter gyro_x_notch_filter, gyro_y_notch_filter, gyro_z_notch_filter; 147 | #endif 148 | 149 | spi_device_handle_t spi; 150 | gyro_fsr g_fsr; 151 | float g_mult; 152 | float a_mult; 153 | accel_fsr a_fsr; 154 | lpf_bandwidth lpf; 155 | bool sample_rate_divider_set; 156 | uint8_t sample_rate_divider; 157 | int16_t accel_offsets[3]; 158 | int16_t gyro_offsets[3]; 159 | volatile bool data_ready; 160 | Sensor_Data last_accel; 161 | uint8_t readings_counter; 162 | 163 | esp_err_t spi_init(); 164 | 165 | esp_err_t int_init(); 166 | 167 | esp_err_t spi_reg_write(uint8_t reg, uint8_t data); 168 | 169 | esp_err_t spi_reg_read(uint8_t reg, uint8_t &data); 170 | 171 | esp_err_t set_gyro_fsr(gyro_fsr fsr); 172 | 173 | esp_err_t set_accel_fsr(accel_fsr fsr); 174 | 175 | esp_err_t set_lpf(lpf_bandwidth lpf); 176 | 177 | esp_err_t set_sample_rate_divider(uint8_t divider); 178 | 179 | esp_err_t set_interrupt(bool enable); 180 | 181 | esp_err_t load_accel_offsets(); 182 | 183 | public: 184 | static MPU6000 &Instance(); 185 | 186 | void Init() override; 187 | 188 | bool Start() override; 189 | 190 | void Stop() override; 191 | 192 | void Accel_Calibrate() override; 193 | 194 | void Gyro_Calibrate() override; 195 | 196 | float Get_Accel_Sample_Rate() override; 197 | 198 | float Get_Gyro_Sample_Rate() override; 199 | 200 | uint8_t Get_Sample_Rates_Ratio() override; 201 | 202 | void Read_Raw(Raw_Data &accel, Raw_Data &gyro) override; 203 | 204 | IMU::Read_Data_Type Read_Data(Sensor_Data &accel, Sensor_Data &gyro) override; 205 | 206 | void Data_Ready_Callback() override; 207 | 208 | bool Data_Ready() override; 209 | }; 210 | 211 | } /* namespace flyhero */ 212 | -------------------------------------------------------------------------------- /components/IMU/include/MPU6050.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6050.h 3 | * 4 | * Created on: 24. 4. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | 14 | #include "Biquad_Filter.h" 15 | #include "IMU.h" 16 | #include "Counting_Median_Finder.h" 17 | 18 | 19 | namespace flyhero 20 | { 21 | 22 | class MPU6050 : public IMU 23 | { 24 | private: 25 | MPU6050(); 26 | 27 | MPU6050(MPU6050 const &); 28 | 29 | MPU6050 &operator=(MPU6050 const &); 30 | 31 | enum gyro_fsr 32 | { 33 | GYRO_FSR_250 = 0x00, 34 | GYRO_FSR_500 = 0x08, 35 | GYRO_FSR_1000 = 0x10, 36 | GYRO_FSR_2000 = 0x18, 37 | GYRO_FSR_NOT_SET = 0xFF 38 | }; 39 | 40 | enum accel_fsr 41 | { 42 | ACCEL_FSR_2 = 0x00, 43 | ACCEL_FSR_4 = 0x08, 44 | ACCEL_FSR_8 = 0x10, 45 | ACCEL_FSR_16 = 0x18, 46 | ACCEL_FSR_NOT_SET = 0xFF 47 | }; 48 | 49 | enum lpf_bandwidth 50 | { 51 | // 8 kHz 52 | LPF_256HZ = 0x00, 53 | // 1 kHz 54 | LPF_188HZ = 0x01, 55 | LPF_98HZ = 0x02, 56 | LPF_42HZ = 0x03, 57 | LPF_20HZ = 0x04, 58 | LPF_10HZ = 0x05, 59 | LPF_5HZ = 0x06, 60 | LPF_NOT_SET = 0xFF 61 | }; 62 | 63 | const float GYRO_SAMPLE_RATE = 1000; 64 | const float ACCEL_SAMPLE_RATE = 1000; 65 | const uint8_t SAMPLE_RATES_RATIO = 1; 66 | const uint8_t ADC_BITS = 16; 67 | const uint8_t I2C_ADDRESS_WRITE = (0x68 << 1); 68 | const uint8_t I2C_ADDRESS_READ = ((0x68 << 1) | 0x01); 69 | const uint16_t I2C_TIMEOUT = 500; 70 | 71 | const struct 72 | { 73 | uint8_t ACCEL_X_OFFSET = 0x06; 74 | uint8_t GYRO_X_OFFSET = 0x13; 75 | uint8_t SMPRT_DIV = 0x19; 76 | uint8_t CONFIG = 0x1A; 77 | uint8_t GYRO_CONFIG = 0x1B; 78 | uint8_t ACCEL_CONFIG = 0x1C; 79 | uint8_t FIFO_EN = 0x23; 80 | uint8_t INT_PIN_CFG = 0x37; 81 | uint8_t INT_ENABLE = 0x38; 82 | uint8_t INT_STATUS = 0x3A; 83 | uint8_t ACCEL_XOUT_H = 0x3B; 84 | uint8_t GYRO_XOUT_H = 0x43; 85 | uint8_t SIGNAL_PATH_RESET = 0x68; 86 | uint8_t USER_CTRL = 0x6A; 87 | uint8_t PWR_MGMT_1 = 0x6B; 88 | uint8_t PWR_MGMT_2 = 0x6C; 89 | uint8_t FIFO_COUNT_H = 0x72; 90 | uint8_t FIFO_COUNT_L = 0x73; 91 | uint8_t FIFO_R_W = 0x74; 92 | uint8_t WHO_AM_I = 0x75; 93 | } REGISTERS; 94 | 95 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 96 | Biquad_Filter accel_x_filter, accel_y_filter, accel_z_filter; 97 | Biquad_Filter gyro_x_filter, gyro_y_filter, gyro_z_filter; 98 | #endif 99 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 100 | Biquad_Filter gyro_x_notch_filter, gyro_y_notch_filter, gyro_z_notch_filter; 101 | #endif 102 | 103 | #if CONFIG_FLYHERO_IMU_GYRO_FSR_250 104 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_250; 105 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_500 106 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_500; 107 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_1000 108 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_1000; 109 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_2000 110 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_2000; 111 | #else 112 | #error "Gyro FSR not set" 113 | #endif 114 | 115 | #if CONFIG_FLYHERO_IMU_ACCEL_FSR_2 116 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_2; 117 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_4 118 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_4; 119 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_8 120 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_8; 121 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_16 122 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_16; 123 | #else 124 | #error "Accel FSR not set" 125 | #endif 126 | 127 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 128 | const lpf_bandwidth TARGET_LPF = LPF_256HZ; 129 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_188HZ 130 | const lpf_bandwidth TARGET_LPF = LPF_188HZ; 131 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_98HZ 132 | const lpf_bandwidth TARGET_LPF = LPF_98HZ; 133 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_42HZ 134 | const lpf_bandwidth TARGET_LPF = LPF_42HZ; 135 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_20HZ 136 | const lpf_bandwidth TARGET_LPF = LPF_20HZ; 137 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_10HZ 138 | const lpf_bandwidth TARGET_LPF = LPF_10HZ; 139 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_5HZ 140 | const lpf_bandwidth TARGET_LPF = LPF_5HZ; 141 | #else 142 | #error "Gyro hardware LPF not set" 143 | #endif 144 | 145 | gyro_fsr g_fsr; 146 | float g_mult; 147 | float a_mult; 148 | accel_fsr a_fsr; 149 | lpf_bandwidth lpf; 150 | bool sample_rate_divider_set; 151 | uint8_t sample_rate_divider; 152 | int16_t accel_offsets[3]; 153 | int16_t gyro_offsets[3]; 154 | volatile bool data_ready; 155 | 156 | esp_err_t i2c_init(); 157 | 158 | esp_err_t int_init(); 159 | 160 | esp_err_t i2c_write(uint8_t reg, uint8_t data); 161 | 162 | esp_err_t i2c_write(uint8_t reg, uint8_t *data, uint8_t data_size); 163 | 164 | esp_err_t i2c_read(uint8_t reg, uint8_t *data); 165 | 166 | esp_err_t i2c_read(uint8_t reg, uint8_t *data, uint8_t data_size); 167 | 168 | esp_err_t set_gyro_fsr(gyro_fsr fsr); 169 | 170 | esp_err_t set_accel_fsr(accel_fsr fsr); 171 | 172 | esp_err_t set_lpf(lpf_bandwidth lpf); 173 | 174 | esp_err_t set_sample_rate_divider(uint8_t divider); 175 | 176 | esp_err_t set_interrupt(bool enable); 177 | 178 | esp_err_t load_accel_offsets(); 179 | 180 | public: 181 | static MPU6050 &Instance(); 182 | 183 | void Init() override; 184 | 185 | bool Start() override; 186 | 187 | void Stop() override; 188 | 189 | void Accel_Calibrate() override; 190 | 191 | void Gyro_Calibrate() override; 192 | 193 | float Get_Accel_Sample_Rate() override; 194 | 195 | float Get_Gyro_Sample_Rate() override; 196 | 197 | uint8_t Get_Sample_Rates_Ratio() override; 198 | 199 | void Read_Raw(Raw_Data &raw_accel, Raw_Data &raw_gyro) override; 200 | 201 | IMU::Read_Data_Type Read_Data(Sensor_Data &accel, Sensor_Data &gyro) override; 202 | 203 | void Data_Ready_Callback() override; 204 | 205 | bool Data_Ready() override; 206 | }; 207 | 208 | } /* namespace The_Eye */ 209 | -------------------------------------------------------------------------------- /components/IMU/include/MPU9250.h: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU9250.h 3 | * 4 | * Created on: 7. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include "Biquad_Filter.h" 18 | #include "IMU.h" 19 | #include "Counting_Median_Finder.h" 20 | 21 | 22 | namespace flyhero 23 | { 24 | 25 | class MPU9250 : public IMU 26 | { 27 | private: 28 | /* Singleton begin */ 29 | MPU9250(); 30 | 31 | MPU9250(MPU9250 const &); 32 | 33 | MPU9250 &operator=(MPU9250 const &); 34 | 35 | /*Singleton end */ 36 | 37 | enum gyro_fsr 38 | { 39 | GYRO_FSR_250 = 0x00, 40 | GYRO_FSR_500 = 0x08, 41 | GYRO_FSR_1000 = 0x10, 42 | GYRO_FSR_2000 = 0x18, 43 | GYRO_FSR_NOT_SET = 0xFF 44 | }; 45 | 46 | enum accel_fsr 47 | { 48 | ACCEL_FSR_2 = 0x00, 49 | ACCEL_FSR_4 = 0x08, 50 | ACCEL_FSR_8 = 0x10, 51 | ACCEL_FSR_16 = 0x18, 52 | ACCEL_FSR_NOT_SET = 0xFF 53 | }; 54 | 55 | enum gyro_lpf 56 | { 57 | // 32 kHz 58 | GYRO_LPF_8800HZ = 0x07, 59 | // 32 kHz 60 | GYRO_LPF_3600Hz = 0x08, 61 | // 8 kHz 62 | GYRO_LPF_250HZ = 0x00, 63 | // 1 kHz 64 | GYRO_LPF_184HZ = 0x01, 65 | GYRO_LPF_92HZ = 0x02, 66 | GYRO_LPF_41HZ = 0x03, 67 | GYRO_LPF_20HZ = 0x04, 68 | GYRO_LPF_10HZ = 0x05, 69 | GYRO_LPF_5HZ = 0x06, 70 | GYRO_LPF_NOT_SET = 0xFF 71 | }; 72 | 73 | enum accel_lpf 74 | { 75 | // 4 kHz 76 | ACCEL_LPF_1046HZ = 0x08, 77 | // 1 kHz 78 | ACCEL_LPF_218HZ = 0x00, 79 | // same as above 80 | ACCEL_LPF_218HZ_2 = 0x01, 81 | ACCEL_LPF_99HZ = 0x02, 82 | ACCEL_LPF_45HZ = 0x03, 83 | ACCEL_LPF_21HZ = 0x04, 84 | ACCEL_LPF_10HZ = 0x05, 85 | ACCEL_LPF_5HZ = 0x06, 86 | // do not use - outputs strange values 87 | ACCEL_LPF_420HZ = 0x07, 88 | ACCEL_LPF_NOT_SET = 0xFF 89 | }; 90 | 91 | const struct 92 | { 93 | uint8_t SMPLRT_DIV = 25; 94 | uint8_t CONFIG = 26; 95 | uint8_t GYRO_CONFIG = 27; 96 | uint8_t ACCEL_CONFIG = 28; 97 | uint8_t ACCEL_CONFIG2 = 29; 98 | uint8_t INT_PIN_CFG = 55; 99 | uint8_t INT_ENABLE = 56; 100 | uint8_t ACCEL_XOUT_H = 59; 101 | uint8_t GYRO_XOUT_H = 67; 102 | uint8_t SIGNAL_PATH_RESET = 104; 103 | uint8_t USER_CTRL = 106; 104 | uint8_t PWR_MGMT_1 = 107; 105 | uint8_t PWR_MGMT_2 = 108; 106 | uint8_t WHO_AM_I = 117; 107 | } REGISTERS; 108 | 109 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 110 | const float GYRO_SAMPLE_RATE = 8000; 111 | const float ACCEL_SAMPLE_RATE = 1000; 112 | const uint8_t SAMPLE_RATES_RATIO = 8; 113 | #else 114 | const float GYRO_SAMPLE_RATE = 1000; 115 | const float ACCEL_SAMPLE_RATE = 1000; 116 | const uint8_t SAMPLE_RATES_RATIO = 1; 117 | #endif 118 | 119 | #if CONFIG_FLYHERO_IMU_GYRO_FSR_250 120 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_250; 121 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_500 122 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_500; 123 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_1000 124 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_1000; 125 | #elif CONFIG_FLYHERO_IMU_GYRO_FSR_2000 126 | const gyro_fsr TARGET_GYRO_FSR = GYRO_FSR_2000; 127 | #else 128 | #error "Gyro FSR not set" 129 | #endif 130 | 131 | #if CONFIG_FLYHERO_IMU_ACCEL_FSR_2 132 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_2; 133 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_4 134 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_4; 135 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_8 136 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_8; 137 | #elif CONFIG_FLYHERO_IMU_ACCEL_FSR_16 138 | const accel_fsr TARGET_ACCEL_FSR = ACCEL_FSR_16; 139 | #else 140 | #error "Accel FSR not set" 141 | #endif 142 | 143 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 144 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_250HZ; 145 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_218HZ; 146 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_188HZ 147 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_184HZ; 148 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_218HZ; 149 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_98HZ 150 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_92HZ; 151 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_99HZ; 152 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_42HZ 153 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_41HZ; 154 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_45HZ; 155 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_20HZ 156 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_20HZ; 157 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_21HZ; 158 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_10HZ 159 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_10HZ; 160 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_10HZ; 161 | #elif CONFIG_FLYHERO_IMU_HARD_LPF_5HZ 162 | const gyro_lpf TARGET_GYRO_LPF = GYRO_LPF_5HZ; 163 | const accel_lpf TARGET_ACCEL_LPF = ACCEL_LPF_5HZ; 164 | #else 165 | #error "Gyro hardware LPF not set" 166 | #endif 167 | 168 | const uint8_t ADC_BITS = 16; 169 | 170 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 171 | Biquad_Filter accel_x_filter, accel_y_filter, accel_z_filter; 172 | Biquad_Filter gyro_x_filter, gyro_y_filter, gyro_z_filter; 173 | #endif 174 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 175 | Biquad_Filter gyro_x_notch_filter, gyro_y_notch_filter, gyro_z_notch_filter; 176 | #endif 177 | 178 | gyro_fsr g_fsr; 179 | accel_fsr a_fsr; 180 | gyro_lpf g_lpf; 181 | accel_lpf a_lpf; 182 | float g_mult, a_mult; 183 | bool sample_rate_divider_set; 184 | uint8_t sample_rate_divider; 185 | spi_device_handle_t spi; 186 | volatile bool data_ready; 187 | int16_t accel_offsets[3]; 188 | int16_t gyro_offsets[3]; 189 | Sensor_Data last_accel; 190 | uint8_t readings_counter; 191 | 192 | esp_err_t spi_init(); 193 | 194 | esp_err_t int_init(); 195 | 196 | esp_err_t spi_reg_read(uint8_t reg, uint8_t &data); 197 | 198 | esp_err_t spi_reg_write(uint8_t reg, uint8_t data); 199 | 200 | esp_err_t set_gyro_fsr(gyro_fsr fsr); 201 | 202 | esp_err_t set_accel_fsr(accel_fsr fsr); 203 | 204 | esp_err_t set_gyro_lpf(gyro_lpf lpf); 205 | 206 | esp_err_t set_accel_lpf(accel_lpf lpf); 207 | 208 | esp_err_t set_sample_rate_divider(uint8_t divider); 209 | 210 | esp_err_t set_interrupt(bool enable); 211 | 212 | esp_err_t load_accel_offsets(); 213 | 214 | 215 | public: 216 | static MPU9250 &Instance(); 217 | 218 | void Init() override; 219 | 220 | bool Start() override; 221 | 222 | void Stop() override; 223 | 224 | void Accel_Calibrate() override; 225 | 226 | void Gyro_Calibrate() override; 227 | 228 | float Get_Accel_Sample_Rate() override; 229 | 230 | float Get_Gyro_Sample_Rate() override; 231 | 232 | uint8_t Get_Sample_Rates_Ratio() override; 233 | 234 | void Read_Raw(Raw_Data &raw_accel, Raw_Data &raw_gyro) override; 235 | 236 | IMU::Read_Data_Type Read_Data(Sensor_Data &accel, Sensor_Data &gyro) override; 237 | 238 | void Data_Ready_Callback() override; 239 | 240 | bool Data_Ready() override; 241 | }; 242 | 243 | } /* namespace flyhero */ 244 | -------------------------------------------------------------------------------- /components/IMU/include/Mahony_Filter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Mahony_Filter.h 3 | * 4 | * Created on: 9. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | #include "Fusion_Filter.h" 14 | #include "Math.h" 15 | 16 | 17 | namespace flyhero 18 | { 19 | 20 | class Mahony_Filter : public Fusion_Filter 21 | { 22 | private: 23 | IMU::Quaternion quaternion; 24 | const float TWO_KP, TWO_KI; 25 | int64_t first_time, last_time; 26 | IMU::Sensor_Data error_integral; 27 | 28 | public: 29 | Mahony_Filter(float kp, float ki); 30 | 31 | ~Mahony_Filter() override = default; 32 | 33 | void Compute(IMU::Sensor_Data accel, IMU::Sensor_Data gyro, IMU::Euler_Angles &euler) override; 34 | 35 | void Reset() override; 36 | }; 37 | 38 | } /* namespace flyhero */ 39 | -------------------------------------------------------------------------------- /components/IMU/include/Math.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Math.h 3 | * 4 | * Created on: 9. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | 13 | namespace flyhero 14 | { 15 | 16 | class Math 17 | { 18 | private: 19 | Math(); 20 | 21 | public: 22 | static constexpr float PI = 3.14159265358979323846f; 23 | static constexpr float DEG_TO_RAD = PI / 180; 24 | static constexpr float RAD_TO_DEG = 180 / PI; 25 | }; 26 | 27 | } /* namespace flyhero */ 28 | -------------------------------------------------------------------------------- /components/IMU/src/Biquad_Filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Biquad_Filter.cpp 3 | * 4 | * Created on: 24. 6. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "Biquad_Filter.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | Biquad_Filter::Biquad_Filter(Filter_Type type, float sample_frequency, float cut_frequency) 15 | { 16 | float K = tanf(Math::PI * cut_frequency / sample_frequency); 17 | float Q = 1.0f / sqrtf(2); // let Q be 1 / sqrt(2) for Butterworth 18 | 19 | float norm; 20 | 21 | switch (type) 22 | { 23 | case FILTER_LOW_PASS: 24 | norm = 1.0f / (1 + K / Q + K * K); 25 | 26 | this->a0 = K * K * norm; 27 | this->a1 = 2 * this->a0; 28 | this->a2 = this->a0; 29 | this->b1 = 2 * (K * K - 1) * norm; 30 | this->b2 = (1 - K / Q + K * K) * norm; 31 | 32 | break; 33 | case FILTER_NOTCH: 34 | norm = 1.0f / (1 + K / Q + K * K); 35 | 36 | this->a0 = (1 + K * K) * norm; 37 | this->a1 = 2 * (K * K - 1) * norm; 38 | this->a2 = this->a0; 39 | this->b1 = this->a1; 40 | this->b2 = (1 - K / Q + K * K) * norm; 41 | 42 | break; 43 | } 44 | 45 | this->z1 = 0; 46 | this->z2 = 0; 47 | } 48 | 49 | } /* namespace flyhero */ 50 | -------------------------------------------------------------------------------- /components/IMU/src/Complementary_Filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Complementary_Filter.cpp 3 | * 4 | * Created on: 10. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "Complementary_Filter.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | Complementary_Filter::Complementary_Filter(float coeff) 15 | : COMPLEMENTARY_COEFFICIENT(coeff) 16 | { 17 | this->last_time = 0; 18 | this->euler.roll = 0; 19 | this->euler.pitch = 0; 20 | this->euler.yaw = 0; 21 | } 22 | 23 | void Complementary_Filter::Compute(IMU::Sensor_Data accel, IMU::Sensor_Data gyro, IMU::Euler_Angles &euler) 24 | { 25 | float delta_t; 26 | 27 | if (this->last_time == 0) 28 | { 29 | delta_t = 0; 30 | 31 | this->last_time = esp_timer_get_time(); 32 | } else 33 | { 34 | int64_t tmp = esp_timer_get_time(); 35 | delta_t = (tmp - this->last_time) * 0.000001f; 36 | 37 | this->last_time = tmp; 38 | } 39 | 40 | float accel_roll = atan2f(accel.y, accel.z) * Math::RAD_TO_DEG; 41 | 42 | float accel_pitch = 43 | atan2f(-accel.x, sqrtf(accel.y * accel.y + accel.z * accel.z)) * Math::RAD_TO_DEG; 44 | 45 | this->euler.roll = this->COMPLEMENTARY_COEFFICIENT * (this->euler.roll + gyro.y * delta_t) 46 | + (1 - this->COMPLEMENTARY_COEFFICIENT) * accel_roll; 47 | 48 | this->euler.pitch = this->COMPLEMENTARY_COEFFICIENT * (this->euler.pitch + gyro.x * delta_t) 49 | + (1 - this->COMPLEMENTARY_COEFFICIENT) * accel_pitch; 50 | 51 | this->euler.yaw += gyro.z * delta_t; 52 | 53 | if (this->euler.yaw > 180) 54 | this->euler.yaw -= 360; 55 | else if (this->euler.yaw < -180) 56 | this->euler.yaw += 360; 57 | 58 | euler = this->euler; 59 | } 60 | 61 | void Complementary_Filter::Reset() 62 | { 63 | this->last_time = 0; 64 | 65 | this->euler.roll = 0; 66 | this->euler.pitch = 0; 67 | this->euler.yaw = 0; 68 | } 69 | 70 | } /* namespace flyhero */ 71 | -------------------------------------------------------------------------------- /components/IMU/src/IMU_Detector.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 22.10.17. 3 | // 4 | 5 | #include "IMU_Detector.h" 6 | 7 | 8 | namespace flyhero 9 | { 10 | 11 | spi_device_handle_t IMU_Detector::spi = NULL; 12 | IMU *IMU_Detector::imu = NULL; 13 | bool IMU_Detector::detected = false; 14 | 15 | bool IMU_Detector::i2c_init() 16 | { 17 | // init i2c peripheral 18 | i2c_config_t conf; 19 | conf.mode = I2C_MODE_MASTER; 20 | conf.sda_io_num = GPIO_NUM_5; 21 | conf.sda_pullup_en = GPIO_PULLUP_DISABLE; 22 | conf.scl_io_num = GPIO_NUM_18; 23 | conf.scl_pullup_en = GPIO_PULLUP_DISABLE; 24 | conf.master.clk_speed = 400000; 25 | 26 | if (i2c_param_config(I2C_NUM_0, &conf) != ESP_OK) 27 | return false; 28 | if (i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0) != ESP_OK) 29 | return false; 30 | 31 | return true; 32 | } 33 | 34 | void IMU_Detector::i2c_deinit() 35 | { 36 | i2c_driver_delete(I2C_NUM_0); 37 | } 38 | 39 | bool IMU_Detector::spi_init() 40 | { 41 | //Initialize the SPI bus 42 | spi_bus_config_t buscfg; 43 | buscfg.miso_io_num = GPIO_NUM_16; 44 | buscfg.mosi_io_num = GPIO_NUM_5; 45 | buscfg.sclk_io_num = GPIO_NUM_18; 46 | buscfg.quadwp_io_num = -1; 47 | buscfg.quadhd_io_num = -1; 48 | buscfg.max_transfer_sz = 0; 49 | buscfg.flags = 0; 50 | 51 | if (spi_bus_initialize(HSPI_HOST, &buscfg, 0) != ESP_OK) 52 | return false; 53 | 54 | return true; 55 | } 56 | 57 | void IMU_Detector::spi_deinit() 58 | { 59 | spi_bus_free(HSPI_HOST); 60 | } 61 | 62 | bool IMU_Detector::try_i2c_imu(const uint8_t DEVICE_ADDRESS_WRITE, const uint8_t DEVICE_ADDRESS_READ, 63 | const uint8_t WHO_AM_I_REGISTER, const uint8_t EXPECTED_VALUE) 64 | { 65 | // read WHO_AM_I register 66 | uint8_t who_am_i; 67 | 68 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 69 | 70 | if (i2c_master_start(cmd) != ESP_OK) 71 | goto i2c_error; 72 | if (i2c_master_write_byte(cmd, DEVICE_ADDRESS_WRITE, true) != ESP_OK) 73 | goto i2c_error; 74 | if (i2c_master_write_byte(cmd, WHO_AM_I_REGISTER, true) != ESP_OK) 75 | goto i2c_error; 76 | 77 | if (i2c_master_start(cmd) != ESP_OK) 78 | goto i2c_error; 79 | if (i2c_master_write_byte(cmd, DEVICE_ADDRESS_READ, true) != ESP_OK) 80 | goto i2c_error; 81 | if (i2c_master_read_byte(cmd, &who_am_i, I2C_MASTER_NACK) != ESP_OK) 82 | goto i2c_error; 83 | if (i2c_master_stop(cmd) != ESP_OK) 84 | goto i2c_error; 85 | 86 | if (i2c_master_cmd_begin(I2C_NUM_0, cmd, 500 / portTICK_RATE_MS) != ESP_OK) 87 | goto i2c_error; 88 | 89 | i2c_cmd_link_delete(cmd); 90 | 91 | return who_am_i == EXPECTED_VALUE; 92 | 93 | i2c_error: 94 | 95 | i2c_cmd_link_delete(cmd); 96 | return false; 97 | } 98 | 99 | bool IMU_Detector::try_spi_imu(const uint8_t WHO_AM_I_REGISTER, const uint8_t EXPECTED_VALUE, const gpio_num_t CS_NUM) 100 | { 101 | spi_device_interface_config_t devcfg; 102 | devcfg.command_bits = 0; 103 | devcfg.address_bits = 8; 104 | devcfg.dummy_bits = 0; 105 | devcfg.mode = 0; 106 | devcfg.duty_cycle_pos = 128; 107 | devcfg.cs_ena_pretrans = 0; 108 | devcfg.cs_ena_posttrans = 0; 109 | devcfg.clock_speed_hz = 1000000; 110 | devcfg.spics_io_num = CS_NUM; 111 | devcfg.flags = 0; 112 | devcfg.queue_size = 7; 113 | devcfg.pre_cb = 0; 114 | devcfg.post_cb = 0; 115 | devcfg.input_delay_ns = 0; 116 | 117 | if (spi_bus_add_device(HSPI_HOST, &devcfg, &IMU_Detector::spi) != ESP_OK) 118 | return false; 119 | 120 | // read WHO_AM_I register 121 | uint8_t who_am_i; 122 | 123 | spi_transaction_t trans; 124 | trans.flags = SPI_TRANS_USE_RXDATA | SPI_TRANS_USE_TXDATA; 125 | trans.cmd = 0; 126 | trans.addr = WHO_AM_I_REGISTER | 0x80; 127 | trans.length = 8; 128 | trans.rxlength = 8; 129 | trans.user = 0; 130 | trans.tx_data[0] = 0x00; 131 | 132 | if (spi_device_transmit(spi, &trans) != ESP_OK) 133 | { 134 | spi_bus_remove_device(IMU_Detector::spi); 135 | return false; 136 | } 137 | 138 | who_am_i = trans.rx_data[0]; 139 | 140 | spi_bus_remove_device(IMU_Detector::spi); 141 | 142 | return who_am_i == EXPECTED_VALUE; 143 | } 144 | 145 | IMU &IMU_Detector::Detect_IMU() 146 | { 147 | if (IMU_Detector::detected) 148 | return *IMU_Detector::imu; 149 | 150 | if (IMU_Detector::i2c_init()) 151 | { 152 | 153 | if (IMU_Detector::try_i2c_imu(0x68 << 1, (0x68 << 1) | 1, 0x75, 0x68)) 154 | { 155 | IMU_Detector::i2c_deinit(); 156 | 157 | IMU_Detector::imu = &MPU6050::Instance(); 158 | IMU_Detector::detected = true; 159 | 160 | return *IMU_Detector::imu; 161 | } 162 | 163 | IMU_Detector::i2c_deinit(); 164 | } 165 | 166 | if (IMU_Detector::spi_init()) 167 | { 168 | if (IMU_Detector::try_spi_imu(0x75, 0x71, GPIO_NUM_13)) 169 | { 170 | IMU_Detector::spi_deinit(); 171 | 172 | IMU_Detector::imu = &MPU9250::Instance(); 173 | IMU_Detector::detected = true; 174 | 175 | return *IMU_Detector::imu; 176 | } else if (IMU_Detector::try_spi_imu(0x75, 0x73, GPIO_NUM_13)) 177 | { 178 | IMU_Detector::spi_deinit(); 179 | 180 | IMU_Detector::imu = &MPU9250::Instance(); 181 | IMU_Detector::detected = true; 182 | 183 | return *IMU_Detector::imu; 184 | } else if (IMU_Detector::try_spi_imu(0x75, 0x68, GPIO_NUM_17)) 185 | { 186 | IMU_Detector::spi_deinit(); 187 | 188 | IMU_Detector::imu = &MPU6000::Instance(); 189 | IMU_Detector::detected = true; 190 | 191 | return *IMU_Detector::imu; 192 | } 193 | 194 | IMU_Detector::spi_deinit(); 195 | } 196 | 197 | assert(0); 198 | } 199 | 200 | } -------------------------------------------------------------------------------- /components/IMU/src/MPU6000.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6000.cpp 3 | * 4 | * Created on: 18. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "MPU6000.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | static void int_handler(void *arg) 15 | { 16 | MPU6000::Instance().Data_Ready_Callback(); 17 | } 18 | 19 | MPU6000 &MPU6000::Instance() 20 | { 21 | static MPU6000 instance; 22 | 23 | return instance; 24 | } 25 | 26 | MPU6000::MPU6000() 27 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 28 | : accel_x_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 29 | accel_y_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 30 | accel_z_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 31 | gyro_x_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF), 32 | gyro_y_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF), 33 | gyro_z_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF) 34 | #endif 35 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 36 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 37 | , 38 | #endif 39 | gyro_x_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH), 40 | gyro_y_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH), 41 | gyro_z_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH) 42 | #endif 43 | { 44 | this->g_fsr = GYRO_FSR_NOT_SET; 45 | this->g_mult = 0; 46 | this->a_mult = 0; 47 | this->a_fsr = ACCEL_FSR_NOT_SET; 48 | this->lpf = LPF_NOT_SET; 49 | this->sample_rate_divider_set = false; 50 | this->sample_rate_divider = 0; 51 | this->accel_offsets[0] = 0; 52 | this->accel_offsets[1] = 0; 53 | this->accel_offsets[2] = 0; 54 | this->gyro_offsets[0] = 0; 55 | this->gyro_offsets[1] = 0; 56 | this->gyro_offsets[2] = 0; 57 | this->data_ready = false; 58 | this->spi = NULL; 59 | this->readings_counter = 0; 60 | } 61 | 62 | esp_err_t MPU6000::spi_init() 63 | { 64 | esp_err_t ret; 65 | 66 | spi_bus_config_t buscfg; 67 | buscfg.miso_io_num = GPIO_NUM_16; 68 | buscfg.mosi_io_num = GPIO_NUM_5; 69 | buscfg.sclk_io_num = GPIO_NUM_18; 70 | buscfg.quadwp_io_num = -1; 71 | buscfg.quadhd_io_num = -1; 72 | buscfg.max_transfer_sz = 0; 73 | buscfg.flags = 0; 74 | 75 | spi_device_interface_config_t devcfg; 76 | devcfg.command_bits = 0; 77 | devcfg.address_bits = 8; 78 | devcfg.dummy_bits = 0; 79 | devcfg.mode = 0; 80 | devcfg.duty_cycle_pos = 128; 81 | devcfg.cs_ena_pretrans = 0; 82 | devcfg.cs_ena_posttrans = 0; 83 | devcfg.clock_speed_hz = 1000000; 84 | devcfg.spics_io_num = GPIO_NUM_17; 85 | devcfg.flags = 0; 86 | devcfg.queue_size = 7; 87 | devcfg.pre_cb = 0; 88 | devcfg.post_cb = 0; 89 | devcfg.input_delay_ns = 0; 90 | 91 | //Initialize the SPI bus 92 | if ((ret = spi_bus_initialize(HSPI_HOST, &buscfg, 0))) 93 | return ret; 94 | 95 | if ((ret = spi_bus_add_device(HSPI_HOST, &devcfg, &this->spi))) 96 | return ret; 97 | 98 | return ESP_OK; 99 | } 100 | 101 | esp_err_t MPU6000::int_init() 102 | { 103 | esp_err_t state; 104 | 105 | gpio_config_t conf; 106 | conf.pin_bit_mask = GPIO_SEL_4; 107 | conf.mode = GPIO_MODE_INPUT; 108 | conf.pull_up_en = GPIO_PULLUP_DISABLE; 109 | conf.pull_down_en = GPIO_PULLDOWN_DISABLE; 110 | conf.intr_type = GPIO_INTR_POSEDGE; 111 | 112 | if ((state = gpio_config(&conf))) 113 | return state; 114 | 115 | return ESP_OK; 116 | } 117 | 118 | esp_err_t MPU6000::spi_reg_write(uint8_t reg, uint8_t data) 119 | { 120 | esp_err_t ret; 121 | 122 | spi_transaction_t trans; 123 | trans.flags = SPI_TRANS_USE_TXDATA; 124 | trans.cmd = 0; 125 | trans.addr = reg & 0x7F; 126 | trans.length = 8; 127 | trans.rxlength = 0; 128 | trans.user = 0; 129 | trans.tx_data[0] = data; 130 | trans.rx_buffer = NULL; 131 | 132 | if ((ret = spi_device_transmit(this->spi, &trans))) 133 | return ret; 134 | 135 | return ESP_OK; 136 | } 137 | 138 | esp_err_t MPU6000::spi_reg_read(uint8_t reg, uint8_t &data) 139 | { 140 | esp_err_t ret; 141 | 142 | spi_transaction_t trans; 143 | trans.flags = SPI_TRANS_USE_RXDATA | SPI_TRANS_USE_TXDATA; 144 | trans.cmd = 0; 145 | trans.addr = reg | 0x80; 146 | trans.length = 8; 147 | trans.rxlength = 8; 148 | trans.user = 0; 149 | trans.tx_data[0] = 0x00; 150 | 151 | if ((ret = spi_device_transmit(this->spi, &trans))) 152 | return ret; 153 | 154 | data = trans.rx_data[0]; 155 | 156 | return ESP_OK; 157 | } 158 | 159 | esp_err_t MPU6000::set_gyro_fsr(gyro_fsr fsr) 160 | { 161 | if (fsr == GYRO_FSR_NOT_SET) 162 | return ESP_FAIL; 163 | 164 | if (this->g_fsr == fsr) 165 | return ESP_OK; 166 | 167 | if (this->spi_reg_write(this->REGISTERS.GYRO_CONFIG, fsr) == ESP_OK) 168 | { 169 | this->g_fsr = fsr; 170 | 171 | switch (this->g_fsr) 172 | { 173 | case GYRO_FSR_250: 174 | this->g_mult = 250; 175 | break; 176 | case GYRO_FSR_500: 177 | this->g_mult = 500; 178 | break; 179 | case GYRO_FSR_1000: 180 | this->g_mult = 1000; 181 | break; 182 | case GYRO_FSR_2000: 183 | this->g_mult = 2000; 184 | break; 185 | case GYRO_FSR_NOT_SET: 186 | return ESP_FAIL; 187 | } 188 | 189 | this->g_mult /= std::pow(2, this->ADC_BITS - 1); 190 | 191 | return ESP_OK; 192 | } 193 | 194 | return ESP_FAIL; 195 | } 196 | 197 | esp_err_t MPU6000::set_accel_fsr(accel_fsr fsr) 198 | { 199 | if (fsr == ACCEL_FSR_NOT_SET) 200 | return ESP_FAIL; 201 | 202 | if (this->a_fsr == fsr) 203 | return ESP_OK; 204 | 205 | if (this->spi_reg_write(this->REGISTERS.ACCEL_CONFIG, fsr) == ESP_OK) 206 | { 207 | this->a_fsr = fsr; 208 | 209 | switch (this->a_fsr) 210 | { 211 | case ACCEL_FSR_2: 212 | this->a_mult = 2; 213 | break; 214 | case ACCEL_FSR_4: 215 | this->a_mult = 4; 216 | break; 217 | case ACCEL_FSR_8: 218 | this->a_mult = 8; 219 | break; 220 | case ACCEL_FSR_16: 221 | this->a_mult = 16; 222 | break; 223 | case ACCEL_FSR_NOT_SET: 224 | return ESP_FAIL; 225 | } 226 | 227 | this->a_mult /= std::pow(2, this->ADC_BITS - 1); 228 | 229 | return ESP_OK; 230 | } 231 | 232 | return ESP_FAIL; 233 | } 234 | 235 | esp_err_t MPU6000::set_lpf(lpf_bandwidth lpf) 236 | { 237 | if (lpf == LPF_NOT_SET) 238 | return ESP_FAIL; 239 | 240 | if (this->lpf == lpf) 241 | return ESP_OK; 242 | 243 | if (this->spi_reg_write(this->REGISTERS.CONFIG, lpf) == ESP_OK) 244 | { 245 | this->lpf = lpf; 246 | return ESP_OK; 247 | } 248 | 249 | return ESP_FAIL; 250 | } 251 | 252 | esp_err_t MPU6000::set_sample_rate_divider(uint8_t divider) 253 | { 254 | if (this->sample_rate_divider == divider && this->sample_rate_divider_set) 255 | return ESP_OK; 256 | 257 | if (this->spi_reg_write(this->REGISTERS.SMPRT_DIV, divider) == ESP_OK) 258 | { 259 | this->sample_rate_divider_set = true; 260 | this->sample_rate_divider = divider; 261 | return ESP_OK; 262 | } 263 | 264 | return ESP_FAIL; 265 | } 266 | 267 | esp_err_t MPU6000::set_interrupt(bool enable) 268 | { 269 | if (enable) 270 | { 271 | if (this->spi_reg_write(this->REGISTERS.INT_ENABLE, 0x01) != ESP_OK) 272 | return ESP_FAIL; 273 | 274 | if (gpio_isr_handler_add(GPIO_NUM_4, int_handler, NULL) != ESP_OK) 275 | return ESP_FAIL; 276 | } else 277 | { 278 | if (this->spi_reg_write(this->REGISTERS.INT_ENABLE, 0x00) != ESP_OK) 279 | return ESP_FAIL; 280 | 281 | if (gpio_isr_handler_remove(GPIO_NUM_4) != ESP_OK) 282 | return ESP_FAIL; 283 | } 284 | 285 | return ESP_OK; 286 | } 287 | 288 | esp_err_t MPU6000::load_accel_offsets() 289 | { 290 | esp_err_t ret; 291 | nvs_handle handle; 292 | 293 | if ((ret = nvs_open("MPU6000", NVS_READONLY, &handle)) != ESP_OK) 294 | return ret; 295 | 296 | if ((ret = nvs_get_i16(handle, "accel_x_offset", this->accel_offsets)) != ESP_OK) 297 | goto nvs_error; 298 | 299 | if ((ret = nvs_get_i16(handle, "accel_y_offset", this->accel_offsets + 1)) != ESP_OK) 300 | goto nvs_error; 301 | 302 | if ((ret = nvs_get_i16(handle, "accel_z_offset", this->accel_offsets + 2)) != ESP_OK) 303 | goto nvs_error; 304 | 305 | nvs_error: 306 | 307 | nvs_close(handle); 308 | return ret; 309 | } 310 | 311 | void MPU6000::Init() 312 | { 313 | // init SPI bus 314 | ESP_ERROR_CHECK(this->spi_init()); 315 | 316 | // init INT pin on ESP32 317 | ESP_ERROR_CHECK(this->int_init()); 318 | 319 | // reset device 320 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.PWR_MGMT_1, 0x80)); 321 | vTaskDelay(100 / portTICK_PERIOD_MS); 322 | 323 | // reset analog devices - should not be needed 324 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.SIGNAL_PATH_RESET, 0x07)); 325 | vTaskDelay(100 / portTICK_RATE_MS); 326 | 327 | // wake up, set clock source PLL with Z gyro axis 328 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.PWR_MGMT_1, 0x03)); 329 | 330 | // do not disable any sensor 331 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.PWR_MGMT_2, 0x00)); 332 | 333 | // check I2C connection 334 | uint8_t who_am_i; 335 | ESP_ERROR_CHECK(this->spi_reg_read(this->REGISTERS.WHO_AM_I, who_am_i)); 336 | 337 | if (who_am_i != 0x68) 338 | ESP_ERROR_CHECK(ESP_FAIL); 339 | 340 | // disable interrupt 341 | ESP_ERROR_CHECK(this->set_interrupt(false)); 342 | 343 | // set INT pin active high, push-pull; don't use latched mode, fsync nor I2C master aux 344 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.INT_PIN_CFG, 0x00)); 345 | 346 | // disable I2C master aux, enable SPI only 347 | ESP_ERROR_CHECK(this->spi_reg_write(this->REGISTERS.USER_CTRL, 0x30)); 348 | 349 | // set gyro full scale range 350 | ESP_ERROR_CHECK(this->set_gyro_fsr(this->TARGET_GYRO_FSR)); 351 | 352 | // set accel full scale range 353 | ESP_ERROR_CHECK(this->set_accel_fsr(this->TARGET_ACCEL_FSR)); 354 | 355 | // set low pass filter 356 | ESP_ERROR_CHECK(this->set_lpf(this->TARGET_LPF)); 357 | 358 | // set sample rate 359 | ESP_ERROR_CHECK(this->set_sample_rate_divider(0)); 360 | } 361 | 362 | bool MPU6000::Start() 363 | { 364 | if (this->load_accel_offsets() != ESP_OK) 365 | return false; 366 | 367 | ESP_ERROR_CHECK(this->set_interrupt(true)); 368 | 369 | // set SPI speed to 20 MHz 370 | ESP_ERROR_CHECK(spi_bus_remove_device(this->spi)); 371 | 372 | spi_device_interface_config_t devcfg; 373 | devcfg.command_bits = 0; 374 | devcfg.address_bits = 8; 375 | devcfg.dummy_bits = 0; 376 | devcfg.mode = 0; 377 | devcfg.duty_cycle_pos = 128; 378 | devcfg.cs_ena_pretrans = 0; 379 | devcfg.cs_ena_posttrans = 0; 380 | devcfg.clock_speed_hz = 20000000; 381 | devcfg.spics_io_num = GPIO_NUM_17; 382 | devcfg.flags = 0; 383 | devcfg.queue_size = 7; 384 | devcfg.pre_cb = 0; 385 | devcfg.post_cb = 0; 386 | devcfg.input_delay_ns = 0; 387 | 388 | ESP_ERROR_CHECK(spi_bus_add_device(HSPI_HOST, &devcfg, &this->spi)); 389 | 390 | return true; 391 | } 392 | 393 | void MPU6000::Stop() 394 | { 395 | // wait for last SPI transcation 396 | spi_transaction_t * dummy; 397 | spi_device_get_trans_result(this->spi, &dummy, 10); 398 | 399 | // set SPI speed to 1 MHz 400 | ESP_ERROR_CHECK(spi_bus_remove_device(this->spi)); 401 | 402 | spi_device_interface_config_t devcfg; 403 | devcfg.command_bits = 0; 404 | devcfg.address_bits = 8; 405 | devcfg.dummy_bits = 0; 406 | devcfg.mode = 0; 407 | devcfg.duty_cycle_pos = 128; 408 | devcfg.cs_ena_pretrans = 0; 409 | devcfg.cs_ena_posttrans = 0; 410 | devcfg.clock_speed_hz = 1000000; 411 | devcfg.spics_io_num = GPIO_NUM_17; 412 | devcfg.flags = 0; 413 | devcfg.queue_size = 7; 414 | devcfg.pre_cb = 0; 415 | devcfg.post_cb = 0; 416 | devcfg.input_delay_ns = 0; 417 | 418 | ESP_ERROR_CHECK(spi_bus_add_device(HSPI_HOST, &devcfg, &this->spi)); 419 | 420 | ESP_ERROR_CHECK(this->set_interrupt(false)); 421 | } 422 | 423 | void MPU6000::Accel_Calibrate() 424 | { 425 | Raw_Data accel, gyro; 426 | int16_t accel_z_reference; 427 | 428 | switch (this->a_fsr) 429 | { 430 | case ACCEL_FSR_2: 431 | accel_z_reference = 16384; 432 | break; 433 | case ACCEL_FSR_4: 434 | accel_z_reference = 8192; 435 | break; 436 | case ACCEL_FSR_8: 437 | accel_z_reference = 4096; 438 | break; 439 | case ACCEL_FSR_16: 440 | accel_z_reference = 2048; 441 | break; 442 | default: 443 | return; 444 | } 445 | 446 | ESP_ERROR_CHECK(this->set_interrupt(true)); 447 | 448 | uint16_t i = 0; 449 | Counting_Median_Finder accel_median_finder[3]; 450 | 451 | while (i < 1000) 452 | { 453 | if (this->Data_Ready()) 454 | { 455 | this->Read_Raw(accel, gyro); 456 | 457 | accel_median_finder[0].Push_Value(accel.x); 458 | accel_median_finder[1].Push_Value(accel.y); 459 | accel_median_finder[2].Push_Value(accel.z - accel_z_reference); 460 | 461 | i++; 462 | } 463 | } 464 | 465 | ESP_ERROR_CHECK(this->set_interrupt(false)); 466 | 467 | this->accel_offsets[0] = -accel_median_finder[0].Get_Median(); 468 | this->accel_offsets[1] = -accel_median_finder[1].Get_Median(); 469 | this->accel_offsets[2] = -accel_median_finder[2].Get_Median(); 470 | 471 | nvs_handle handle; 472 | 473 | if (nvs_open("MPU6000", NVS_READWRITE, &handle) != ESP_OK) 474 | return; 475 | 476 | nvs_set_i16(handle, "accel_x_offset", this->accel_offsets[0]); 477 | nvs_set_i16(handle, "accel_y_offset", this->accel_offsets[1]); 478 | nvs_set_i16(handle, "accel_z_offset", this->accel_offsets[2]); 479 | 480 | nvs_close(handle); 481 | } 482 | 483 | void MPU6000::Gyro_Calibrate() 484 | { 485 | Raw_Data accel, gyro; 486 | 487 | ESP_ERROR_CHECK(this->set_interrupt(true)); 488 | 489 | uint16_t i = 0; 490 | Counting_Median_Finder gyro_median_finder[3]; 491 | 492 | while (i < 1000) 493 | { 494 | if (this->Data_Ready()) 495 | { 496 | this->Read_Raw(accel, gyro); 497 | 498 | gyro_median_finder[0].Push_Value(gyro.x); 499 | gyro_median_finder[1].Push_Value(gyro.y); 500 | gyro_median_finder[2].Push_Value(gyro.z); 501 | 502 | i++; 503 | } 504 | } 505 | 506 | ESP_ERROR_CHECK(this->set_interrupt(false)); 507 | 508 | this->gyro_offsets[0] = -gyro_median_finder[0].Get_Median(); 509 | this->gyro_offsets[1] = -gyro_median_finder[1].Get_Median(); 510 | this->gyro_offsets[2] = -gyro_median_finder[2].Get_Median(); 511 | } 512 | 513 | float MPU6000::Get_Accel_Sample_Rate() 514 | { 515 | return this->ACCEL_SAMPLE_RATE; 516 | } 517 | 518 | float MPU6000::Get_Gyro_Sample_Rate() 519 | { 520 | return this->GYRO_SAMPLE_RATE; 521 | } 522 | 523 | uint8_t MPU6000::Get_Sample_Rates_Ratio() 524 | { 525 | return this->SAMPLE_RATES_RATIO; 526 | } 527 | 528 | void MPU6000::Read_Raw(Raw_Data &accel, Raw_Data &gyro) 529 | { 530 | const uint8_t tx_data[14] = { 0x00 }; 531 | uint8_t rx_data[14]; 532 | 533 | spi_transaction_t trans; 534 | trans.flags = 0; 535 | trans.cmd = 0; 536 | trans.addr = this->REGISTERS.ACCEL_XOUT_H | 0x80; 537 | trans.length = 14 * 8; 538 | trans.rxlength = 14 * 8; 539 | trans.user = 0; 540 | trans.tx_buffer = tx_data; 541 | trans.rx_buffer = rx_data; 542 | 543 | ESP_ERROR_CHECK(spi_device_transmit(this->spi, &trans)); 544 | 545 | accel.x = (rx_data[2] << 8) | rx_data[3]; 546 | accel.y = (rx_data[0] << 8) | rx_data[1]; 547 | accel.z = (rx_data[4] << 8) | rx_data[5]; 548 | 549 | gyro.x = -((rx_data[8] << 8) | rx_data[9]); 550 | gyro.y = -((rx_data[10] << 8) | rx_data[11]); 551 | gyro.z = (rx_data[12] << 8) | rx_data[13]; 552 | } 553 | 554 | IMU::Read_Data_Type MPU6000::Read_Data(Sensor_Data &accel, Sensor_Data &gyro) 555 | { 556 | IMU::Read_Data_Type return_type; 557 | 558 | if (this->readings_counter == 0) 559 | { 560 | const uint8_t tx_data[14] = { 0x00 }; 561 | uint8_t rx_data[14]; 562 | 563 | spi_transaction_t trans; 564 | trans.flags = 0; 565 | trans.cmd = 0; 566 | trans.addr = this->REGISTERS.ACCEL_XOUT_H | 0x80; 567 | trans.length = 14 * 8; 568 | trans.rxlength = 14 * 8; 569 | trans.user = 0; 570 | trans.tx_buffer = tx_data; 571 | trans.rx_buffer = rx_data; 572 | 573 | ESP_ERROR_CHECK(spi_device_transmit(this->spi, &trans)); 574 | 575 | Raw_Data raw_accel, raw_gyro; 576 | 577 | raw_accel.x = (rx_data[2] << 8) | rx_data[3]; 578 | raw_accel.y = (rx_data[0] << 8) | rx_data[1]; 579 | raw_accel.z = (rx_data[4] << 8) | rx_data[5]; 580 | 581 | raw_gyro.x = -((rx_data[8] << 8) | rx_data[9]); 582 | raw_gyro.y = -((rx_data[10] << 8) | rx_data[11]); 583 | raw_gyro.z = (rx_data[12] << 8) | rx_data[13]; 584 | 585 | accel.x = (raw_accel.x + this->accel_offsets[0]) * this->a_mult; 586 | accel.y = (raw_accel.y + this->accel_offsets[1]) * this->a_mult; 587 | accel.z = (raw_accel.z + this->accel_offsets[2]) * this->a_mult; 588 | 589 | gyro.x = (raw_gyro.x + this->gyro_offsets[0]) * this->g_mult; 590 | gyro.y = (raw_gyro.y + this->gyro_offsets[1]) * this->g_mult; 591 | gyro.z = (raw_gyro.z + this->gyro_offsets[2]) * this->g_mult; 592 | 593 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 594 | gyro.x = this->gyro_x_notch_filter.Apply_Filter(gyro.x); 595 | gyro.y = this->gyro_y_notch_filter.Apply_Filter(gyro.y); 596 | gyro.z = this->gyro_z_notch_filter.Apply_Filter(gyro.z); 597 | #endif 598 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 599 | accel.x = this->accel_x_filter.Apply_Filter(accel.x); 600 | accel.y = this->accel_y_filter.Apply_Filter(accel.y); 601 | accel.z = this->accel_z_filter.Apply_Filter(accel.z); 602 | 603 | gyro.x = this->gyro_x_filter.Apply_Filter(gyro.x); 604 | gyro.y = this->gyro_y_filter.Apply_Filter(gyro.y); 605 | gyro.z = this->gyro_z_filter.Apply_Filter(gyro.z); 606 | #endif 607 | this->last_accel = accel; 608 | return_type = IMU::Read_Data_Type::ACCEL_GYRO; 609 | } else 610 | { 611 | const uint8_t tx_data[6] = { 0x00 }; 612 | uint8_t rx_data[6]; 613 | 614 | spi_transaction_t trans; 615 | trans.flags = 0; 616 | trans.cmd = 0; 617 | trans.addr = this->REGISTERS.GYRO_XOUT_H | 0x80; 618 | trans.length = 6 * 8; 619 | trans.rxlength = 6 * 8; 620 | trans.user = 0; 621 | trans.tx_buffer = tx_data; 622 | trans.rx_buffer = rx_data; 623 | 624 | ESP_ERROR_CHECK(spi_device_transmit(this->spi, &trans)); 625 | 626 | Raw_Data raw_gyro; 627 | 628 | raw_gyro.x = -((rx_data[0] << 8) | rx_data[1]); 629 | raw_gyro.y = -((rx_data[2] << 8) | rx_data[3]); 630 | raw_gyro.z = (rx_data[4] << 8) | rx_data[5]; 631 | 632 | accel = this->last_accel; 633 | 634 | gyro.x = (raw_gyro.x + this->gyro_offsets[0]) * this->g_mult; 635 | gyro.y = (raw_gyro.y + this->gyro_offsets[1]) * this->g_mult; 636 | gyro.z = (raw_gyro.z + this->gyro_offsets[2]) * this->g_mult; 637 | 638 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 639 | gyro.x = this->gyro_x_notch_filter.Apply_Filter(gyro.x); 640 | gyro.y = this->gyro_y_notch_filter.Apply_Filter(gyro.y); 641 | gyro.z = this->gyro_z_notch_filter.Apply_Filter(gyro.z); 642 | #endif 643 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 644 | gyro.x = this->gyro_x_filter.Apply_Filter(gyro.x); 645 | gyro.y = this->gyro_y_filter.Apply_Filter(gyro.y); 646 | gyro.z = this->gyro_z_filter.Apply_Filter(gyro.z); 647 | #endif 648 | return_type = IMU::Read_Data_Type::GYRO_ONLY; 649 | } 650 | 651 | this->readings_counter++; 652 | 653 | if (this->readings_counter == this->SAMPLE_RATES_RATIO) 654 | this->readings_counter = 0; 655 | 656 | return return_type; 657 | } 658 | 659 | void MPU6000::Data_Ready_Callback() 660 | { 661 | this->data_ready = true; 662 | } 663 | 664 | bool MPU6000::Data_Ready() 665 | { 666 | portDISABLE_INTERRUPTS(); 667 | 668 | bool tmp = this->data_ready; 669 | 670 | if (this->data_ready) 671 | this->data_ready = false; 672 | 673 | portENABLE_INTERRUPTS(); 674 | 675 | return tmp; 676 | } 677 | 678 | } /* namespace flyhero */ 679 | -------------------------------------------------------------------------------- /components/IMU/src/MPU6050.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MPU6050.cpp 3 | * 4 | * Created on: 24. 4. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "MPU6050.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | static void int_handler(void *arg) 15 | { 16 | MPU6050::Instance().Data_Ready_Callback(); 17 | } 18 | 19 | MPU6050 &MPU6050::Instance() 20 | { 21 | static MPU6050 instance; 22 | 23 | return instance; 24 | } 25 | 26 | MPU6050::MPU6050() 27 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 28 | : accel_x_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 29 | accel_y_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 30 | accel_z_filter(Biquad_Filter::FILTER_LOW_PASS, this->ACCEL_SAMPLE_RATE, CONFIG_FLYHERO_IMU_ACCEL_SOFT_LPF), 31 | gyro_x_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF), 32 | gyro_y_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF), 33 | gyro_z_filter(Biquad_Filter::FILTER_LOW_PASS, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_SOFT_LPF) 34 | #endif 35 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 36 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 37 | , 38 | #endif 39 | gyro_x_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH), 40 | gyro_y_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH), 41 | gyro_z_notch_filter(Biquad_Filter::FILTER_NOTCH, this->GYRO_SAMPLE_RATE, CONFIG_FLYHERO_IMU_GYRO_NOTCH) 42 | #endif 43 | { 44 | this->g_fsr = GYRO_FSR_NOT_SET; 45 | this->g_mult = 0; 46 | this->a_mult = 0; 47 | this->a_fsr = ACCEL_FSR_NOT_SET; 48 | this->lpf = LPF_NOT_SET; 49 | this->sample_rate_divider_set = false; 50 | this->sample_rate_divider = 0; 51 | this->accel_offsets[0] = 0; 52 | this->accel_offsets[1] = 0; 53 | this->accel_offsets[2] = 0; 54 | this->gyro_offsets[0] = 0; 55 | this->gyro_offsets[1] = 0; 56 | this->gyro_offsets[2] = 0; 57 | this->data_ready = false; 58 | } 59 | 60 | esp_err_t MPU6050::i2c_init() 61 | { 62 | esp_err_t state; 63 | 64 | i2c_config_t conf; 65 | conf.mode = I2C_MODE_MASTER; 66 | conf.sda_io_num = GPIO_NUM_5; 67 | conf.sda_pullup_en = GPIO_PULLUP_DISABLE; 68 | conf.scl_io_num = GPIO_NUM_18; 69 | conf.scl_pullup_en = GPIO_PULLUP_DISABLE; 70 | conf.master.clk_speed = 400000; 71 | 72 | if ((state = i2c_param_config(I2C_NUM_0, &conf))) 73 | return state; 74 | if ((state = i2c_driver_install(I2C_NUM_0, conf.mode, 0, 0, 0))) 75 | return state; 76 | 77 | return ESP_OK; 78 | } 79 | 80 | esp_err_t MPU6050::int_init() 81 | { 82 | esp_err_t state; 83 | 84 | gpio_config_t conf; 85 | conf.pin_bit_mask = GPIO_SEL_4; 86 | conf.mode = GPIO_MODE_INPUT; 87 | conf.pull_up_en = GPIO_PULLUP_DISABLE; 88 | conf.pull_down_en = GPIO_PULLDOWN_DISABLE; 89 | conf.intr_type = GPIO_INTR_POSEDGE; 90 | 91 | if ((state = gpio_config(&conf))) 92 | return state; 93 | 94 | return ESP_OK; 95 | } 96 | 97 | esp_err_t MPU6050::i2c_write(uint8_t reg, uint8_t data) 98 | { 99 | esp_err_t state; 100 | 101 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 102 | 103 | if ((state = i2c_master_start(cmd))) 104 | goto i2c_error; 105 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_WRITE, true))) 106 | goto i2c_error; 107 | if ((state = i2c_master_write_byte(cmd, reg, true))) 108 | goto i2c_error; 109 | if ((state = i2c_master_write_byte(cmd, data, true))) 110 | goto i2c_error; 111 | if ((state = i2c_master_stop(cmd))) 112 | goto i2c_error; 113 | 114 | if ((state = i2c_master_cmd_begin(I2C_NUM_0, cmd, this->I2C_TIMEOUT / portTICK_RATE_MS))) 115 | goto i2c_error; 116 | 117 | i2c_cmd_link_delete(cmd); 118 | 119 | return ESP_OK; 120 | 121 | i2c_error: 122 | 123 | i2c_cmd_link_delete(cmd); 124 | return state; 125 | } 126 | 127 | esp_err_t MPU6050::i2c_write(uint8_t reg, uint8_t *data, uint8_t data_size) 128 | { 129 | esp_err_t state; 130 | 131 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 132 | 133 | if ((state = i2c_master_start(cmd))) 134 | goto i2c_error; 135 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_WRITE, true))) 136 | goto i2c_error; 137 | if ((state = i2c_master_write_byte(cmd, reg, true))) 138 | goto i2c_error; 139 | if ((state = i2c_master_write(cmd, data, data_size, true))) 140 | goto i2c_error; 141 | if ((state = i2c_master_stop(cmd))) 142 | goto i2c_error; 143 | 144 | if ((state = i2c_master_cmd_begin(I2C_NUM_0, cmd, this->I2C_TIMEOUT / portTICK_RATE_MS))) 145 | goto i2c_error; 146 | 147 | i2c_cmd_link_delete(cmd); 148 | 149 | return ESP_OK; 150 | 151 | i2c_error: 152 | 153 | i2c_cmd_link_delete(cmd); 154 | return state; 155 | } 156 | 157 | esp_err_t MPU6050::i2c_read(uint8_t reg, uint8_t *data) 158 | { 159 | esp_err_t state; 160 | 161 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 162 | 163 | if ((state = i2c_master_start(cmd))) 164 | goto i2c_error; 165 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_WRITE, true))) 166 | goto i2c_error; 167 | if ((state = i2c_master_write_byte(cmd, reg, true))) 168 | goto i2c_error; 169 | 170 | if ((state = i2c_master_start(cmd))) 171 | goto i2c_error; 172 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_READ, true))) 173 | goto i2c_error; 174 | if ((state = i2c_master_read_byte(cmd, data, I2C_MASTER_NACK))) 175 | goto i2c_error; 176 | if ((state = i2c_master_stop(cmd))) 177 | goto i2c_error; 178 | 179 | if ((state = i2c_master_cmd_begin(I2C_NUM_0, cmd, this->I2C_TIMEOUT / portTICK_RATE_MS))) 180 | goto i2c_error; 181 | 182 | i2c_cmd_link_delete(cmd); 183 | 184 | return ESP_OK; 185 | 186 | i2c_error: 187 | 188 | i2c_cmd_link_delete(cmd); 189 | return state; 190 | } 191 | 192 | esp_err_t MPU6050::i2c_read(uint8_t reg, uint8_t *data, uint8_t data_size) 193 | { 194 | esp_err_t state; 195 | 196 | i2c_cmd_handle_t cmd = i2c_cmd_link_create(); 197 | 198 | if ((state = i2c_master_start(cmd))) 199 | goto i2c_error; 200 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_WRITE, true))) 201 | goto i2c_error; 202 | if ((state = i2c_master_write_byte(cmd, reg, true))) 203 | goto i2c_error; 204 | 205 | if ((state = i2c_master_start(cmd))) 206 | goto i2c_error; 207 | if ((state = i2c_master_write_byte(cmd, this->I2C_ADDRESS_READ, true))) 208 | goto i2c_error; 209 | 210 | if ((state = i2c_master_read(cmd, data, data_size, I2C_MASTER_LAST_NACK))) 211 | goto i2c_error; 212 | 213 | if ((state = i2c_master_stop(cmd))) 214 | goto i2c_error; 215 | 216 | if ((state = i2c_master_cmd_begin(I2C_NUM_0, cmd, this->I2C_TIMEOUT / portTICK_RATE_MS))) 217 | goto i2c_error; 218 | 219 | i2c_cmd_link_delete(cmd); 220 | 221 | return ESP_OK; 222 | 223 | i2c_error: 224 | 225 | i2c_cmd_link_delete(cmd); 226 | return state; 227 | } 228 | 229 | esp_err_t MPU6050::set_gyro_fsr(gyro_fsr fsr) 230 | { 231 | if (fsr == GYRO_FSR_NOT_SET) 232 | return ESP_FAIL; 233 | 234 | if (this->g_fsr == fsr) 235 | return ESP_OK; 236 | 237 | if (this->i2c_write(this->REGISTERS.GYRO_CONFIG, fsr) == ESP_OK) 238 | { 239 | this->g_fsr = fsr; 240 | 241 | switch (this->g_fsr) 242 | { 243 | case GYRO_FSR_250: 244 | this->g_mult = 250; 245 | break; 246 | case GYRO_FSR_500: 247 | this->g_mult = 500; 248 | break; 249 | case GYRO_FSR_1000: 250 | this->g_mult = 1000; 251 | break; 252 | case GYRO_FSR_2000: 253 | this->g_mult = 2000; 254 | break; 255 | case GYRO_FSR_NOT_SET: 256 | return ESP_FAIL; 257 | } 258 | 259 | this->g_mult /= std::pow(2, this->ADC_BITS - 1); 260 | 261 | return ESP_OK; 262 | } 263 | 264 | return ESP_FAIL; 265 | } 266 | 267 | esp_err_t MPU6050::set_accel_fsr(accel_fsr fsr) 268 | { 269 | if (fsr == ACCEL_FSR_NOT_SET) 270 | return ESP_FAIL; 271 | 272 | if (this->a_fsr == fsr) 273 | return ESP_OK; 274 | 275 | if (this->i2c_write(this->REGISTERS.ACCEL_CONFIG, fsr) == ESP_OK) 276 | { 277 | this->a_fsr = fsr; 278 | 279 | switch (this->a_fsr) 280 | { 281 | case ACCEL_FSR_2: 282 | this->a_mult = 2; 283 | break; 284 | case ACCEL_FSR_4: 285 | this->a_mult = 4; 286 | break; 287 | case ACCEL_FSR_8: 288 | this->a_mult = 8; 289 | break; 290 | case ACCEL_FSR_16: 291 | this->a_mult = 16; 292 | break; 293 | case ACCEL_FSR_NOT_SET: 294 | return ESP_FAIL; 295 | } 296 | 297 | this->a_mult /= std::pow(2, this->ADC_BITS - 1); 298 | 299 | return ESP_OK; 300 | } 301 | 302 | return ESP_FAIL; 303 | } 304 | 305 | esp_err_t MPU6050::set_lpf(lpf_bandwidth lpf) 306 | { 307 | if (lpf == LPF_NOT_SET) 308 | return ESP_FAIL; 309 | 310 | if (this->lpf == lpf) 311 | return ESP_OK; 312 | 313 | if (this->i2c_write(this->REGISTERS.CONFIG, lpf) == ESP_OK) 314 | { 315 | this->lpf = lpf; 316 | return ESP_OK; 317 | } 318 | 319 | return ESP_FAIL; 320 | } 321 | 322 | esp_err_t MPU6050::set_sample_rate_divider(uint8_t divider) 323 | { 324 | if (this->sample_rate_divider == divider && this->sample_rate_divider_set) 325 | return ESP_OK; 326 | 327 | if (this->i2c_write(this->REGISTERS.SMPRT_DIV, divider) == ESP_OK) 328 | { 329 | this->sample_rate_divider_set = true; 330 | this->sample_rate_divider = divider; 331 | return ESP_OK; 332 | } 333 | 334 | return ESP_FAIL; 335 | } 336 | 337 | esp_err_t MPU6050::set_interrupt(bool enable) 338 | { 339 | if (enable) 340 | { 341 | if (this->i2c_write(this->REGISTERS.INT_ENABLE, 0x01) != ESP_OK) 342 | return ESP_FAIL; 343 | 344 | if (gpio_isr_handler_add(GPIO_NUM_4, int_handler, NULL) != ESP_OK) 345 | return ESP_FAIL; 346 | } else 347 | { 348 | if (this->i2c_write(this->REGISTERS.INT_ENABLE, 0x00) != ESP_OK) 349 | return ESP_FAIL; 350 | 351 | if (gpio_isr_handler_remove(GPIO_NUM_4) != ESP_OK) 352 | return ESP_FAIL; 353 | } 354 | 355 | return ESP_OK; 356 | } 357 | 358 | esp_err_t MPU6050::load_accel_offsets() 359 | { 360 | esp_err_t ret; 361 | nvs_handle handle; 362 | 363 | if ((ret = nvs_open("MPU6050", NVS_READONLY, &handle)) != ESP_OK) 364 | return ret; 365 | 366 | if ((ret = nvs_get_i16(handle, "accel_x_offset", this->accel_offsets)) != ESP_OK) 367 | goto nvs_error; 368 | 369 | if ((ret = nvs_get_i16(handle, "accel_y_offset", this->accel_offsets + 1)) != ESP_OK) 370 | goto nvs_error; 371 | 372 | if ((ret = nvs_get_i16(handle, "accel_z_offset", this->accel_offsets + 2)) != ESP_OK) 373 | goto nvs_error; 374 | 375 | nvs_error: 376 | 377 | nvs_close(handle); 378 | return ret; 379 | } 380 | 381 | void MPU6050::Init() 382 | { 383 | // init I2C bus including DMA peripheral 384 | ESP_ERROR_CHECK(this->i2c_init()); 385 | 386 | // init INT pin on ESP32 387 | ESP_ERROR_CHECK(this->int_init()); 388 | 389 | // reset device 390 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.PWR_MGMT_1, 0x80)); 391 | vTaskDelay(100 / portTICK_PERIOD_MS); 392 | 393 | // reset analog devices - should not be needed 394 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.SIGNAL_PATH_RESET, 0x07)); 395 | vTaskDelay(100 / portTICK_PERIOD_MS); 396 | 397 | // wake up, set clock source PLL with Z gyro axis 398 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.PWR_MGMT_1, 0x03)); 399 | 400 | // do not disable any sensor 401 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.PWR_MGMT_2, 0x00)); 402 | 403 | // check I2C connection 404 | uint8_t who_am_i; 405 | ESP_ERROR_CHECK(this->i2c_read(this->REGISTERS.WHO_AM_I, &who_am_i)); 406 | 407 | if (who_am_i != 0x68) 408 | ESP_ERROR_CHECK(ESP_FAIL); 409 | 410 | // disable interrupt 411 | ESP_ERROR_CHECK(this->set_interrupt(false)); 412 | 413 | // set INT pin active high, push-pull; don't use latched mode, fsync nor I2C master aux 414 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.INT_PIN_CFG, 0x00)); 415 | 416 | // disable I2C master aux 417 | ESP_ERROR_CHECK(this->i2c_write(this->REGISTERS.USER_CTRL, 0x20)); 418 | 419 | // set gyro full scale range 420 | ESP_ERROR_CHECK(this->set_gyro_fsr(this->TARGET_GYRO_FSR)); 421 | 422 | // set accel full scale range 423 | ESP_ERROR_CHECK(this->set_accel_fsr(this->TARGET_ACCEL_FSR)); 424 | 425 | // set low pass filter 426 | ESP_ERROR_CHECK(this->set_lpf(this->TARGET_LPF)); 427 | 428 | // set sample rate 429 | #if CONFIG_FLYHERO_IMU_HARD_LPF_256HZ 430 | ESP_ERROR_CHECK(this->set_sample_rate_divider(7)); 431 | #else 432 | ESP_ERROR_CHECK(this->set_sample_rate_divider(0)); 433 | #endif 434 | 435 | vTaskDelay(100 / portTICK_PERIOD_MS); 436 | } 437 | 438 | bool MPU6050::Start() 439 | { 440 | if (this->load_accel_offsets() != ESP_OK) 441 | return false; 442 | 443 | ESP_ERROR_CHECK(this->set_interrupt(true)); 444 | 445 | return true; 446 | } 447 | 448 | void MPU6050::Stop() 449 | { 450 | ESP_ERROR_CHECK(this->set_interrupt(false)); 451 | } 452 | 453 | void MPU6050::Accel_Calibrate() 454 | { 455 | Raw_Data accel, gyro; 456 | int16_t accel_z_reference; 457 | 458 | switch (this->a_fsr) 459 | { 460 | case ACCEL_FSR_2: 461 | accel_z_reference = 16384; 462 | break; 463 | case ACCEL_FSR_4: 464 | accel_z_reference = 8192; 465 | break; 466 | case ACCEL_FSR_8: 467 | accel_z_reference = 4096; 468 | break; 469 | case ACCEL_FSR_16: 470 | accel_z_reference = 2048; 471 | break; 472 | default: 473 | return; 474 | } 475 | 476 | ESP_ERROR_CHECK(this->set_interrupt(true)); 477 | 478 | uint16_t i = 0; 479 | Counting_Median_Finder accel_median_finder[3]; 480 | 481 | while (i < 1000) 482 | { 483 | if (this->Data_Ready()) 484 | { 485 | this->Read_Raw(accel, gyro); 486 | 487 | accel_median_finder[0].Push_Value(accel.x); 488 | accel_median_finder[1].Push_Value(accel.y); 489 | accel_median_finder[2].Push_Value(accel.z - accel_z_reference); 490 | 491 | i++; 492 | } 493 | } 494 | 495 | ESP_ERROR_CHECK(this->set_interrupt(false)); 496 | 497 | this->accel_offsets[0] = -accel_median_finder[0].Get_Median(); 498 | this->accel_offsets[1] = -accel_median_finder[1].Get_Median(); 499 | this->accel_offsets[2] = -accel_median_finder[2].Get_Median(); 500 | 501 | nvs_handle handle; 502 | 503 | if (nvs_open("MPU6050", NVS_READWRITE, &handle) != ESP_OK) 504 | return; 505 | 506 | nvs_set_i16(handle, "accel_x_offset", this->accel_offsets[0]); 507 | nvs_set_i16(handle, "accel_y_offset", this->accel_offsets[1]); 508 | nvs_set_i16(handle, "accel_z_offset", this->accel_offsets[2]); 509 | 510 | nvs_close(handle); 511 | } 512 | 513 | void MPU6050::Gyro_Calibrate() 514 | { 515 | Raw_Data accel, gyro; 516 | 517 | ESP_ERROR_CHECK(this->set_interrupt(true)); 518 | 519 | uint16_t i = 0; 520 | Counting_Median_Finder gyro_median_finder[3]; 521 | 522 | while (i < 1000) 523 | { 524 | if (this->Data_Ready()) 525 | { 526 | this->Read_Raw(accel, gyro); 527 | 528 | gyro_median_finder[0].Push_Value(gyro.x); 529 | gyro_median_finder[1].Push_Value(gyro.y); 530 | gyro_median_finder[2].Push_Value(gyro.z); 531 | 532 | i++; 533 | } 534 | } 535 | 536 | ESP_ERROR_CHECK(this->set_interrupt(false)); 537 | 538 | this->gyro_offsets[0] = -gyro_median_finder[0].Get_Median(); 539 | this->gyro_offsets[1] = -gyro_median_finder[1].Get_Median(); 540 | this->gyro_offsets[2] = -gyro_median_finder[2].Get_Median(); 541 | } 542 | 543 | float MPU6050::Get_Accel_Sample_Rate() 544 | { 545 | return this->ACCEL_SAMPLE_RATE; 546 | } 547 | 548 | float MPU6050::Get_Gyro_Sample_Rate() 549 | { 550 | return this->GYRO_SAMPLE_RATE; 551 | } 552 | 553 | uint8_t MPU6050::Get_Sample_Rates_Ratio() 554 | { 555 | return this->SAMPLE_RATES_RATIO; 556 | } 557 | 558 | void MPU6050::Read_Raw(Raw_Data &accel, Raw_Data &gyro) 559 | { 560 | uint8_t data[14]; 561 | 562 | ESP_ERROR_CHECK(this->i2c_read(this->REGISTERS.ACCEL_XOUT_H, data, 14)); 563 | 564 | accel.x = (data[2] << 8) | data[3]; 565 | accel.y = (data[0] << 8) | data[1]; 566 | accel.z = (data[4] << 8) | data[5]; 567 | 568 | gyro.x = -((data[8] << 8) | data[9]); 569 | gyro.y = -((data[10] << 8) | data[11]); 570 | gyro.z = (data[12] << 8) | data[13]; 571 | } 572 | 573 | IMU::Read_Data_Type MPU6050::Read_Data(Sensor_Data &accel, Sensor_Data &gyro) 574 | { 575 | uint8_t data[14]; 576 | 577 | ESP_ERROR_CHECK(this->i2c_read(this->REGISTERS.ACCEL_XOUT_H, data, 14)); 578 | 579 | Raw_Data raw_accel, raw_gyro; 580 | 581 | raw_accel.x = (data[2] << 8) | data[3]; 582 | raw_accel.y = (data[0] << 8) | data[1]; 583 | raw_accel.z = (data[4] << 8) | data[5]; 584 | 585 | raw_gyro.x = -((data[8] << 8) | data[9]); 586 | raw_gyro.y = -((data[10] << 8) | data[11]); 587 | raw_gyro.z = (data[12] << 8) | data[13]; 588 | 589 | accel.x = (raw_accel.x + this->accel_offsets[0]) * this->a_mult; 590 | accel.y = (raw_accel.y + this->accel_offsets[1]) * this->a_mult; 591 | accel.z = (raw_accel.z + this->accel_offsets[2]) * this->a_mult; 592 | 593 | gyro.x = (raw_gyro.x + this->gyro_offsets[0]) * this->g_mult; 594 | gyro.y = (raw_gyro.y + this->gyro_offsets[1]) * this->g_mult; 595 | gyro.z = (raw_gyro.z + this->gyro_offsets[2]) * this->g_mult; 596 | 597 | #if CONFIG_FLYHERO_IMU_USE_NOTCH 598 | gyro.x = this->gyro_x_notch_filter.Apply_Filter(gyro.x); 599 | gyro.y = this->gyro_y_notch_filter.Apply_Filter(gyro.y); 600 | gyro.z = this->gyro_z_notch_filter.Apply_Filter(gyro.z); 601 | #endif 602 | #if CONFIG_FLYHERO_IMU_USE_SOFT_LPF 603 | accel.x = this->accel_x_filter.Apply_Filter(accel.x); 604 | accel.y = this->accel_y_filter.Apply_Filter(accel.y); 605 | accel.z = this->accel_z_filter.Apply_Filter(accel.z); 606 | 607 | gyro.x = this->gyro_x_filter.Apply_Filter(gyro.x); 608 | gyro.y = this->gyro_y_filter.Apply_Filter(gyro.y); 609 | gyro.z = this->gyro_z_filter.Apply_Filter(gyro.z); 610 | #endif 611 | 612 | return IMU::Read_Data_Type::ACCEL_GYRO; 613 | } 614 | 615 | void MPU6050::Data_Ready_Callback() 616 | { 617 | this->data_ready = true; 618 | } 619 | 620 | bool MPU6050::Data_Ready() 621 | { 622 | portDISABLE_INTERRUPTS(); 623 | 624 | bool tmp = this->data_ready; 625 | 626 | if (this->data_ready) 627 | this->data_ready = false; 628 | 629 | portENABLE_INTERRUPTS(); 630 | 631 | return tmp; 632 | } 633 | 634 | } /* namespace The_Eye */ 635 | -------------------------------------------------------------------------------- /components/IMU/src/Mahony_Filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Mahony_Filter.cpp 3 | * 4 | * Created on: 9. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "Mahony_Filter.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | Mahony_Filter::Mahony_Filter(float kp, float ki) 15 | : TWO_KP(2 * kp), TWO_KI(2 * ki) 16 | { 17 | this->last_time = 0; 18 | this->quaternion.q0 = 1; 19 | this->quaternion.q1 = 0; 20 | this->quaternion.q2 = 0; 21 | this->quaternion.q3 = 0; 22 | this->error_integral.x = 0; 23 | this->error_integral.y = 0; 24 | this->error_integral.z = 0; 25 | } 26 | 27 | // expects gyro data in deg/s, accel data in multiples of g 28 | void Mahony_Filter::Compute(IMU::Sensor_Data accel, IMU::Sensor_Data gyro, IMU::Euler_Angles &euler) 29 | { 30 | float delta_t; 31 | 32 | if (this->last_time == 0) 33 | { 34 | delta_t = 0; 35 | 36 | this->first_time = esp_timer_get_time(); 37 | this->last_time = this->first_time; 38 | } else 39 | { 40 | int64_t tmp = esp_timer_get_time(); 41 | delta_t = (tmp - this->last_time) * 0.000001f; 42 | 43 | this->last_time = tmp; 44 | } 45 | 46 | float recip_norm; 47 | 48 | // swap x with y value 49 | IMU::Sensor_Data gyro_rad; 50 | gyro_rad.x = gyro.y * Math::DEG_TO_RAD; 51 | gyro_rad.y = gyro.x * Math::DEG_TO_RAD; 52 | gyro_rad.z = gyro.z * Math::DEG_TO_RAD; 53 | 54 | IMU::Sensor_Data half_v, half_error; 55 | 56 | // normalise accelerometer measurement 57 | recip_norm = 1 / sqrtf(accel.x * accel.x + accel.y * accel.y + accel.z * accel.z); 58 | accel.x *= recip_norm; 59 | accel.y *= recip_norm; 60 | accel.z *= recip_norm; 61 | 62 | // estimated direction of gravity and vector perpendicular to magnetic flux 63 | half_v.x = this->quaternion.q1 * this->quaternion.q3 64 | - this->quaternion.q0 * this->quaternion.q2; 65 | half_v.y = this->quaternion.q0 * this->quaternion.q1 66 | + this->quaternion.q2 * this->quaternion.q3; 67 | half_v.z = this->quaternion.q0 * this->quaternion.q0 - 0.5f 68 | + this->quaternion.q3 * this->quaternion.q3; 69 | 70 | // error is sum of cross product between estimated and measured direction of gravity 71 | half_error.x = accel.y * half_v.z - accel.z * half_v.y; 72 | half_error.y = accel.z * half_v.x - accel.x * half_v.z; 73 | half_error.z = accel.x * half_v.y - accel.y * half_v.x; 74 | 75 | if (this->TWO_KI > 0) 76 | { 77 | this->error_integral.x += this->TWO_KI * half_error.x * delta_t; 78 | this->error_integral.y += this->TWO_KI * half_error.y * delta_t; 79 | this->error_integral.z += this->TWO_KI * half_error.z * delta_t; 80 | } else 81 | { 82 | this->error_integral.x = 0; 83 | this->error_integral.y = 0; 84 | this->error_integral.z = 0; 85 | } 86 | 87 | if (this->last_time - this->first_time < 20000000) 88 | { 89 | gyro_rad.x += 10 * this->TWO_KP * half_error.x + error_integral.x; 90 | gyro_rad.y += 10 * this->TWO_KP * half_error.y + error_integral.y; 91 | gyro_rad.z += 10 * this->TWO_KP * half_error.z + error_integral.z; 92 | } else 93 | { 94 | gyro_rad.x += this->TWO_KP * half_error.x + error_integral.x; 95 | gyro_rad.y += this->TWO_KP * half_error.y + error_integral.y; 96 | gyro_rad.z += this->TWO_KP * half_error.z + error_integral.z; 97 | } 98 | 99 | gyro_rad.x *= 0.5f * delta_t; 100 | gyro_rad.y *= 0.5f * delta_t; 101 | gyro_rad.z *= 0.5f * delta_t; 102 | 103 | float qa = this->quaternion.q0; 104 | float qb = this->quaternion.q1; 105 | float qc = this->quaternion.q2; 106 | 107 | this->quaternion.q0 += -qb * gyro_rad.x - qc * gyro_rad.y 108 | - this->quaternion.q3 * gyro_rad.z; 109 | this->quaternion.q1 += qa * gyro_rad.x + qc * gyro_rad.z 110 | - this->quaternion.q3 * gyro_rad.y; 111 | this->quaternion.q2 += qa * gyro_rad.y - qb * gyro_rad.z 112 | + this->quaternion.q3 * gyro_rad.x; 113 | this->quaternion.q3 += qa * gyro_rad.z + qb * gyro_rad.y 114 | - qc * gyro_rad.x; 115 | 116 | // normalise quaternion 117 | recip_norm = 1 / sqrtf(this->quaternion.q0 * this->quaternion.q0 + 118 | this->quaternion.q1 * this->quaternion.q1 + 119 | this->quaternion.q2 * this->quaternion.q2 + 120 | this->quaternion.q3 * this->quaternion.q3); 121 | 122 | this->quaternion.q0 *= recip_norm; 123 | this->quaternion.q1 *= recip_norm; 124 | this->quaternion.q2 *= recip_norm; 125 | this->quaternion.q3 *= recip_norm; 126 | 127 | // convert quaternion to euler 128 | // https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Quaternion_to_Euler_Angles_Conversion 129 | 130 | float q2_sqr = this->quaternion.q2 * this->quaternion.q2; 131 | 132 | euler.roll = atan2f(2 * (this->quaternion.q0 * this->quaternion.q1 + this->quaternion.q2 * this->quaternion.q3), 133 | 1 - 2 * (this->quaternion.q1 * this->quaternion.q1 + q2_sqr)) * Math::RAD_TO_DEG; 134 | 135 | float sinp = 2 * (this->quaternion.q0 * this->quaternion.q2 - this->quaternion.q3 * this->quaternion.q1); 136 | 137 | if (fabs(sinp) >= 1) 138 | euler.pitch = copysignf(Math::PI / 2, sinp); 139 | else 140 | euler.pitch = asinf(sinp); 141 | euler.pitch *= Math::RAD_TO_DEG; 142 | 143 | euler.yaw = atan2f(2 * (this->quaternion.q0 * this->quaternion.q3 + this->quaternion.q1 * this->quaternion.q2), 144 | 1 - 2 * (q2_sqr + this->quaternion.q3 * this->quaternion.q3)) * Math::RAD_TO_DEG; 145 | } 146 | 147 | void Mahony_Filter::Reset() 148 | { 149 | this->last_time = 0; 150 | 151 | this->quaternion.q0 = 1; 152 | this->quaternion.q1 = 0; 153 | this->quaternion.q2 = 0; 154 | this->quaternion.q3 = 0; 155 | 156 | this->error_integral.x = 0; 157 | this->error_integral.y = 0; 158 | this->error_integral.z = 0; 159 | } 160 | 161 | } /* namespace flyhero */ 162 | -------------------------------------------------------------------------------- /components/LEDs/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/LEDs/include/LEDs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LEDs.h 3 | * 4 | * Created on: 17. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | 14 | namespace flyhero 15 | { 16 | 17 | class LEDs 18 | { 19 | private: 20 | LEDs() 21 | {}; 22 | 23 | public: 24 | enum Color 25 | { 26 | ONBOARD = 1 << 0, WARNING = 1 << 1 27 | }; 28 | 29 | static void Init(); 30 | 31 | static void Turn_On(Color color); 32 | 33 | static void Turn_Off(Color color); 34 | 35 | }; 36 | 37 | inline LEDs::Color operator|(LEDs::Color a, LEDs::Color b) 38 | { 39 | return static_cast(static_cast(a) | static_cast(b)); 40 | } 41 | 42 | } /* namespace flyhero */ 43 | -------------------------------------------------------------------------------- /components/LEDs/src/LEDs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * LEDs.cpp 3 | * 4 | * Created on: 17. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "LEDs.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | void LEDs::Init() 15 | { 16 | gpio_config_t gpio_conf; 17 | gpio_conf.intr_type = GPIO_INTR_DISABLE; 18 | gpio_conf.mode = GPIO_MODE_OUTPUT; 19 | gpio_conf.pin_bit_mask = GPIO_SEL_2 | GPIO_SEL_33; 20 | gpio_conf.pull_down_en = GPIO_PULLDOWN_DISABLE; 21 | gpio_conf.pull_up_en = GPIO_PULLUP_DISABLE; 22 | 23 | ESP_ERROR_CHECK(gpio_config(&gpio_conf)); 24 | 25 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_2, 0)); 26 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_33, 0)); 27 | } 28 | 29 | void LEDs::Turn_On(Color color) 30 | { 31 | if (color & ONBOARD) 32 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_2, 1)); 33 | if (color & WARNING) 34 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_33, 1)); 35 | } 36 | 37 | void LEDs::Turn_Off(Color color) 38 | { 39 | if (color & ONBOARD) 40 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_2, 0)); 41 | if (color & WARNING) 42 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_33, 0)); 43 | } 44 | 45 | } /* namespace flyhero */ 46 | -------------------------------------------------------------------------------- /components/Logging/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/Logging/include/Logger.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 20.12.17. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace flyhero 10 | { 11 | 12 | class Logger 13 | { 14 | private: 15 | Logger(); 16 | 17 | Logger(Logger const &); 18 | 19 | Logger &operator=(Logger const &); 20 | 21 | const esp_partition_t *partition; 22 | size_t write_offset, read_offset; 23 | bool log; 24 | 25 | public: 26 | static Logger &Instance(); 27 | 28 | bool Init(); 29 | 30 | void Enable_Writes(); 31 | 32 | bool Erase(); 33 | 34 | bool Log_Next(const void *data, size_t size); 35 | 36 | void Reset_Read_Pointer(); 37 | 38 | bool Read_Next(void *data, size_t size); 39 | }; 40 | 41 | } -------------------------------------------------------------------------------- /components/Logging/src/Logger.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 20.12.17. 3 | // 4 | 5 | #include 6 | #include "Logger.h" 7 | 8 | namespace flyhero 9 | { 10 | 11 | Logger &Logger::Instance() 12 | { 13 | static Logger instance; 14 | 15 | return instance; 16 | } 17 | 18 | Logger::Logger() 19 | { 20 | this->partition = NULL; 21 | } 22 | 23 | bool Logger::Init() 24 | { 25 | this->partition = esp_partition_find_first((esp_partition_type_t) 0x40, (esp_partition_subtype_t) 0x40, NULL); 26 | this->write_offset = 0; 27 | this->read_offset = 0; 28 | this->log = false; 29 | 30 | return esp_partition_verify(this->partition) != NULL; 31 | } 32 | 33 | void Logger::Enable_Writes() 34 | { 35 | this->log = true; 36 | } 37 | 38 | bool Logger::Erase() 39 | { 40 | return esp_partition_erase_range(this->partition, 0, this->partition->size) == ESP_OK; 41 | } 42 | 43 | bool Logger::Log_Next(const void *data, size_t size) 44 | { 45 | if (!this->log || esp_partition_write(this->partition, this->write_offset, data, size) != ESP_OK) 46 | return false; 47 | 48 | this->write_offset += size; 49 | return true; 50 | } 51 | 52 | void Logger::Reset_Read_Pointer() 53 | { 54 | this->read_offset = 0; 55 | } 56 | 57 | bool Logger::Read_Next(void *data, size_t size) 58 | { 59 | if (esp_partition_read(this->partition, this->read_offset, data, size) != ESP_OK) 60 | return false; 61 | 62 | this->read_offset += size; 63 | return true; 64 | } 65 | 66 | } -------------------------------------------------------------------------------- /components/Logging/test/Logger_test.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 21.12.17. 3 | // 4 | 5 | #include 6 | #include 7 | #include "Logger.h" 8 | 9 | using namespace flyhero; 10 | 11 | TEST_CASE("Basic logger test", "[logger]") 12 | { 13 | Logger &logger = Logger::Instance(); 14 | 15 | TEST_ASSERT_TRUE(logger.Init()); 16 | logger.Enable_Writes(); 17 | TEST_ASSERT_TRUE(logger.Erase()); 18 | 19 | bool var = true; 20 | 21 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 22 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 23 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 24 | var = false; 25 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 26 | 27 | 28 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var)) && var); 29 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var)) && var); 30 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var)) && var); 31 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var)) && !var); 32 | } 33 | 34 | TEST_CASE("Array data type logger test", "[logger]") 35 | { 36 | Logger &logger = Logger::Instance(); 37 | 38 | TEST_ASSERT_TRUE(logger.Init()); 39 | logger.Enable_Writes(); 40 | TEST_ASSERT_TRUE(logger.Erase()); 41 | 42 | int var[4] = { 1, 2, 3, 4 }; 43 | 44 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 45 | var[3] = -8; 46 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 47 | var[1] = 0; 48 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 49 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 50 | 51 | 52 | int expected[4] = { 1, 2, 3, 4 }; 53 | 54 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 55 | TEST_ASSERT_EQUAL_INT_ARRAY(expected, var, 4); 56 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 57 | expected[3] = -8; 58 | TEST_ASSERT_EQUAL_INT_ARRAY(expected, var, 4); 59 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 60 | expected[1] = 0; 61 | TEST_ASSERT_EQUAL_INT_ARRAY(expected, var, 4); 62 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 63 | } 64 | 65 | struct test_struct 66 | { 67 | char *str; 68 | int xy; 69 | }; 70 | 71 | TEST_CASE("Struct data type logger test", "[logger]") 72 | { 73 | Logger &logger = Logger::Instance(); 74 | 75 | TEST_ASSERT_TRUE(logger.Init()); 76 | logger.Enable_Writes(); 77 | TEST_ASSERT_TRUE(logger.Erase()); 78 | 79 | test_struct var; 80 | var.str = strdup("Hello!"); 81 | var.xy = -5; 82 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 83 | 84 | var.str = strdup("Test string"); 85 | var.xy = 454578; 86 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 87 | 88 | var.str = strdup("Final test!"); 89 | var.xy = 0; 90 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 91 | 92 | 93 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 94 | TEST_ASSERT_EQUAL_STRING("Hello!", var.str); 95 | TEST_ASSERT_EQUAL_INT(-5, var.xy); 96 | delete var.str; 97 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 98 | TEST_ASSERT_EQUAL_STRING("Test string", var.str); 99 | TEST_ASSERT_EQUAL_INT(454578, var.xy); 100 | delete var.str; 101 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 102 | TEST_ASSERT_EQUAL_STRING("Final test!", var.str); 103 | TEST_ASSERT_EQUAL_INT(0, var.xy); 104 | delete var.str; 105 | } 106 | 107 | class test_class 108 | { 109 | private: 110 | int x; 111 | 112 | public: 113 | test_class() 114 | { 115 | this->x = INT_MIN; 116 | } 117 | 118 | void Set_X(int x) 119 | { 120 | this->x = x; 121 | } 122 | 123 | int Get_X() 124 | { 125 | return this->x; 126 | } 127 | }; 128 | 129 | TEST_CASE("Class data type logger test", "[logger]") 130 | { 131 | Logger &logger = Logger::Instance(); 132 | 133 | TEST_ASSERT_TRUE(logger.Init()); 134 | logger.Enable_Writes(); 135 | TEST_ASSERT_TRUE(logger.Erase()); 136 | 137 | test_class var; 138 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 139 | 140 | var.Set_X(55); 141 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 142 | 143 | var.Set_X(-9); 144 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 145 | 146 | 147 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 148 | TEST_ASSERT_EQUAL_INT(INT_MIN, var.Get_X()); 149 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 150 | TEST_ASSERT_EQUAL_INT(55, var.Get_X()); 151 | TEST_ASSERT_TRUE(logger.Read_Next(&var, sizeof(var))); 152 | TEST_ASSERT_EQUAL_INT(-9, var.Get_X()); 153 | } 154 | 155 | TEST_CASE("Run out of space test", "[logger]") 156 | { 157 | Logger &logger = Logger::Instance(); 158 | 159 | TEST_ASSERT_TRUE(logger.Init()); 160 | logger.Enable_Writes(); 161 | TEST_ASSERT_TRUE(logger.Erase()); 162 | 163 | char var = 'a'; 164 | 165 | for (int i = 0; i < 8 * 1024; i++) 166 | TEST_ASSERT_TRUE(logger.Log_Next(&var, sizeof(var))); 167 | 168 | TEST_ASSERT_FALSE(logger.Log_Next(&var, sizeof(var))); 169 | TEST_ASSERT_FALSE(logger.Log_Next(&var, sizeof(var))); 170 | } 171 | 172 | TEST_CASE("Not enabled test", "[logger]") 173 | { 174 | Logger &logger = Logger::Instance(); 175 | 176 | TEST_ASSERT_TRUE(logger.Init()); 177 | TEST_ASSERT_TRUE(logger.Erase()); 178 | 179 | uint8_t x = 77; 180 | 181 | TEST_ASSERT_FALSE(logger.Log_Next(&x, sizeof(x))); 182 | } -------------------------------------------------------------------------------- /components/Logging/test/component.mk: -------------------------------------------------------------------------------- 1 | # 2 | #Component Makefile 3 | # 4 | 5 | COMPONENT_ADD_LDFLAGS = -Wl,--whole-archive -l$(COMPONENT_NAME) -Wl,--no-whole-archive 6 | -------------------------------------------------------------------------------- /components/Motors/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/Motors/include/Motors_Controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Motors_Controller.h 3 | * 4 | * Created on: 18. 7. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | #include "PID.h" 14 | #include "IMU.h" 15 | #include "Motors_Protocol.h" 16 | #include "IMU_Detector.h" 17 | 18 | 19 | namespace flyhero 20 | { 21 | 22 | class Motors_Controller 23 | { 24 | private: 25 | Motors_Controller(); 26 | 27 | Motors_Controller(Motors_Controller const &); 28 | 29 | Motors_Controller &operator=(Motors_Controller const &); 30 | 31 | enum axis 32 | { 33 | ROLL = 0, 34 | PITCH = 1, 35 | YAW = 2 36 | }; 37 | 38 | Motors_Protocol &motors_protocol; 39 | PID **stab_PIDs, **rate_PIDs; 40 | int16_t motor_FL, motor_FR, motor_BL, motor_BR; 41 | uint16_t throttle; 42 | float rate_setpoints[3]; 43 | bool running; 44 | 45 | portMUX_TYPE stab_PIDs_mutex, rate_PIDs_mutex; 46 | 47 | public: 48 | static Motors_Controller &Instance(); 49 | 50 | enum PID_Type 51 | { 52 | STABILIZE, RATE 53 | }; 54 | 55 | // to be called from CORE 0 56 | void Init(); 57 | 58 | void Set_PID_Constants(PID_Type type, float parameters[3][3]); 59 | 60 | void Set_Throttle(uint16_t throttle); 61 | 62 | // to be called from CORE 1 63 | void Start(); 64 | 65 | void Stop(); 66 | 67 | void Feed_Stab_PIDs(IMU::Euler_Angles euler); 68 | void Feed_Rate_PIDs(IMU::Sensor_Data gyro); 69 | }; 70 | 71 | } /* namespace flyhero */ 72 | -------------------------------------------------------------------------------- /components/Motors/include/Motors_Protocol.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 7.3.18. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | 9 | namespace flyhero 10 | { 11 | 12 | class Motors_Protocol 13 | { 14 | public: 15 | virtual void Init() = 0; 16 | 17 | virtual void Arm() = 0; 18 | 19 | virtual void Disarm() = 0; 20 | 21 | virtual void Update(uint16_t motor_fl, uint16_t motor_bl, uint16_t motor_fr, uint16_t motor_br) = 0; 22 | }; 23 | 24 | } -------------------------------------------------------------------------------- /components/Motors/include/OneShot125.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 7.3.18. 3 | // 4 | 5 | #pragma once 6 | 7 | #include "Motors_Protocol.h" 8 | 9 | namespace flyhero 10 | { 11 | 12 | class OneShot125 : Motors_Protocol 13 | { 14 | private: 15 | OneShot125() = default; 16 | 17 | OneShot125(OneShot125 const &); 18 | 19 | OneShot125 &operator=(OneShot125 const &); 20 | 21 | public: 22 | static Motors_Protocol &Instance(); 23 | 24 | void Init() override; 25 | 26 | void Arm() override; 27 | 28 | void Disarm() override; 29 | 30 | void Update(uint16_t motor_fl, uint16_t motor_bl, uint16_t motor_fr, uint16_t motor_br) override; 31 | }; 32 | 33 | } -------------------------------------------------------------------------------- /components/Motors/src/Motors_Controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Motors_Controller.cpp 3 | * 4 | * Created on: 18. 7. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "OneShot125.h" 9 | #include "Motors_Controller.h" 10 | 11 | 12 | namespace flyhero 13 | { 14 | 15 | Motors_Controller &Motors_Controller::Instance() 16 | { 17 | static Motors_Controller instance; 18 | 19 | return instance; 20 | } 21 | 22 | Motors_Controller::Motors_Controller() : motors_protocol(OneShot125::Instance()) 23 | { 24 | this->motors_protocol.Disarm(); 25 | 26 | this->motor_FR = 0; 27 | this->motor_FL = 0; 28 | this->motor_BR = 0; 29 | this->motor_BL = 0; 30 | 31 | this->rate_setpoints[0] = 0; 32 | this->rate_setpoints[1] = 0; 33 | this->rate_setpoints[2] = 0; 34 | 35 | this->throttle = 0; 36 | 37 | this->running = false; 38 | 39 | this->stab_PIDs_mutex = portMUX_INITIALIZER_UNLOCKED; 40 | this->rate_PIDs_mutex = portMUX_INITIALIZER_UNLOCKED; 41 | } 42 | 43 | void Motors_Controller::Init() 44 | { 45 | float sample_rate = IMU_Detector::Detect_IMU().Get_Gyro_Sample_Rate(); 46 | 47 | vPortCPUAcquireMutex(&this->stab_PIDs_mutex); 48 | vPortCPUAcquireMutex(&this->rate_PIDs_mutex); 49 | 50 | this->stab_PIDs = new PID *[3]; 51 | this->rate_PIDs = new PID *[3]; 52 | 53 | for (uint8_t i = ROLL; i <= YAW; i++) 54 | { 55 | this->stab_PIDs[i] = new PID(sample_rate); 56 | this->stab_PIDs[i]->Set_I_Max(50); 57 | 58 | this->rate_PIDs[i] = new PID(sample_rate); 59 | this->rate_PIDs[i]->Set_I_Max(50); 60 | } 61 | 62 | this->stab_PIDs[ROLL]->Set_Kp(4.5f); 63 | this->stab_PIDs[PITCH]->Set_Kp(4.5f); 64 | 65 | vPortCPUReleaseMutex(&this->stab_PIDs_mutex); 66 | vPortCPUReleaseMutex(&this->rate_PIDs_mutex); 67 | 68 | this->motors_protocol.Init(); 69 | } 70 | 71 | void Motors_Controller::Set_PID_Constants(PID_Type type, float parameters[3][3]) 72 | { 73 | portDISABLE_INTERRUPTS(); 74 | 75 | switch (type) 76 | { 77 | case STABILIZE: 78 | vPortCPUAcquireMutex(&this->stab_PIDs_mutex); 79 | 80 | for (uint8_t i = ROLL; i <= YAW; i++) 81 | { 82 | this->stab_PIDs[i]->Set_Kp(parameters[i][0]); 83 | this->stab_PIDs[i]->Set_Ki(parameters[i][1]); 84 | this->stab_PIDs[i]->Set_Kd(parameters[i][2]); 85 | } 86 | 87 | vPortCPUReleaseMutex(&this->stab_PIDs_mutex); 88 | break; 89 | case RATE: 90 | vPortCPUAcquireMutex(&this->rate_PIDs_mutex); 91 | 92 | for (uint8_t i = ROLL; i <= YAW; i++) 93 | { 94 | this->rate_PIDs[i]->Set_Kp(parameters[i][0]); 95 | this->rate_PIDs[i]->Set_Ki(parameters[i][1]); 96 | this->rate_PIDs[i]->Set_Kd(parameters[i][2]); 97 | } 98 | 99 | vPortCPUReleaseMutex(&this->rate_PIDs_mutex); 100 | break; 101 | } 102 | 103 | portENABLE_INTERRUPTS(); 104 | } 105 | 106 | void Motors_Controller::Set_Throttle(uint16_t throttle) 107 | { 108 | if (throttle > 1000) 109 | return; 110 | 111 | this->throttle = throttle; 112 | } 113 | 114 | void Motors_Controller::Feed_Stab_PIDs(IMU::Euler_Angles euler) 115 | { 116 | if (!this->running) 117 | return; 118 | 119 | // consider more than 60 deg unsafe 120 | if (std::fabs(euler.roll) > 60 || std::fabs(euler.pitch) > 60) 121 | esp_restart(); 122 | 123 | uint16_t throttle = this->throttle; 124 | 125 | if (throttle > 180) 126 | { 127 | vPortCPUAcquireMutex(&this->stab_PIDs_mutex); 128 | 129 | this->rate_setpoints[ROLL] = this->stab_PIDs[ROLL]->Get_PID(0 - euler.roll); 130 | this->rate_setpoints[PITCH] = this->stab_PIDs[PITCH]->Get_PID(0 - euler.pitch); 131 | this->rate_setpoints[YAW] = 0; 132 | 133 | vPortCPUReleaseMutex(&this->stab_PIDs_mutex); 134 | } 135 | } 136 | 137 | void Motors_Controller::Feed_Rate_PIDs(IMU::Sensor_Data gyro) 138 | { 139 | if (!this->running) 140 | return; 141 | 142 | uint16_t throttle = this->throttle; 143 | 144 | if (throttle > 180) 145 | { 146 | float rate_corrections[3]; 147 | 148 | vPortCPUAcquireMutex(&this->rate_PIDs_mutex); 149 | 150 | rate_corrections[ROLL] = 151 | this->rate_PIDs[ROLL]->Get_PID(this->rate_setpoints[ROLL] - gyro.y); 152 | rate_corrections[PITCH] = 153 | this->rate_PIDs[PITCH]->Get_PID(this->rate_setpoints[PITCH] - gyro.x); 154 | rate_corrections[YAW] = 155 | this->rate_PIDs[YAW]->Get_PID(this->rate_setpoints[YAW] - gyro.z); 156 | 157 | vPortCPUReleaseMutex(&this->rate_PIDs_mutex); 158 | 159 | this->motor_FL = throttle - rate_corrections[ROLL] - rate_corrections[PITCH] 160 | + rate_corrections[YAW]; 161 | this->motor_BL = throttle - rate_corrections[ROLL] + rate_corrections[PITCH] 162 | - rate_corrections[YAW]; 163 | this->motor_FR = throttle + rate_corrections[ROLL] - rate_corrections[PITCH] 164 | - rate_corrections[YAW]; 165 | this->motor_BR = throttle + rate_corrections[ROLL] + rate_corrections[PITCH] 166 | + rate_corrections[YAW]; 167 | 168 | if (this->motor_FL > 1000) 169 | this->motor_FL = 1000; 170 | else if (this->motor_FL < 0) 171 | this->motor_FL = 0; 172 | 173 | if (this->motor_BL > 1000) 174 | this->motor_BL = 1000; 175 | else if (this->motor_BL < 0) 176 | this->motor_BL = 0; 177 | 178 | if (this->motor_FR > 1000) 179 | this->motor_FR = 1000; 180 | else if (this->motor_FR < 0) 181 | this->motor_FR = 0; 182 | 183 | if (this->motor_BR > 1000) 184 | this->motor_BR = 1000; 185 | else if (this->motor_BR < 0) 186 | this->motor_BR = 0; 187 | 188 | this->motors_protocol.Update(this->motor_FL, this->motor_BL, this->motor_FR, this->motor_BR); 189 | } else 190 | this->motors_protocol.Update(0, 0, 0, 0); 191 | } 192 | 193 | void Motors_Controller::Start() 194 | { 195 | this->motors_protocol.Arm(); 196 | this->running = true; 197 | } 198 | 199 | void Motors_Controller::Stop() 200 | { 201 | this->motors_protocol.Disarm(); 202 | this->running = false; 203 | } 204 | 205 | } /* namespace flyhero */ 206 | -------------------------------------------------------------------------------- /components/Motors/src/OneShot125.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 7.3.18. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "OneShot125.h" 12 | 13 | namespace flyhero 14 | { 15 | 16 | Motors_Protocol &OneShot125::Instance() 17 | { 18 | static OneShot125 instance; 19 | 20 | return instance; 21 | } 22 | 23 | void OneShot125::Init() 24 | { 25 | mcpwm_pin_config_t mcpwm_pin_config; 26 | mcpwm_pin_config.mcpwm0a_out_num = GPIO_NUM_25; 27 | mcpwm_pin_config.mcpwm0b_out_num = GPIO_NUM_26; 28 | mcpwm_pin_config.mcpwm1a_out_num = GPIO_NUM_27; 29 | mcpwm_pin_config.mcpwm1b_out_num = GPIO_NUM_14; 30 | mcpwm_pin_config.mcpwm2a_out_num = -1; 31 | mcpwm_pin_config.mcpwm2b_out_num = -1; 32 | mcpwm_pin_config.mcpwm_sync0_in_num = -1; 33 | mcpwm_pin_config.mcpwm_sync1_in_num = -1; 34 | mcpwm_pin_config.mcpwm_sync2_in_num = -1; 35 | mcpwm_pin_config.mcpwm_fault0_in_num = -1; 36 | mcpwm_pin_config.mcpwm_fault1_in_num = -1; 37 | mcpwm_pin_config.mcpwm_fault2_in_num = -1; 38 | mcpwm_pin_config.mcpwm_cap0_in_num = -1; 39 | mcpwm_pin_config.mcpwm_cap1_in_num = -1; 40 | mcpwm_pin_config.mcpwm_cap2_in_num = -1; 41 | 42 | ESP_ERROR_CHECK(mcpwm_set_pin(MCPWM_UNIT_0, &mcpwm_pin_config)); 43 | 44 | // set unit clock to 16 MHz 45 | MCPWM0.clk_cfg.prescale = 9; 46 | 47 | // set timer clock to 8 MHz 48 | MCPWM0.timer[0].period.prescale = 1; 49 | 50 | // set timer period to 2000 ticks 51 | MCPWM0.timer[0].period.period = 2000; 52 | 53 | // set timer update method to immediate 54 | MCPWM0.timer[0].period.upmethod = 0; 55 | 56 | // set timer mode to count up 57 | MCPWM0.timer[0].mode.mode = 1; 58 | 59 | // stop timer 60 | MCPWM0.timer[0].mode.start = 0; 61 | 62 | MCPWM0.timer[0].sync.timer_phase = 0; 63 | 64 | // set both operators reference timer 0 65 | MCPWM0.timer_sel.operator0_sel = 0; 66 | MCPWM0.timer_sel.operator1_sel = 0; 67 | 68 | MCPWM0.channel[0].cmpr_cfg.a_upmethod = 0; 69 | MCPWM0.channel[0].cmpr_cfg.b_upmethod = 0; 70 | MCPWM0.channel[1].cmpr_cfg.a_upmethod = 0; 71 | MCPWM0.channel[1].cmpr_cfg.b_upmethod = 0; 72 | 73 | 74 | // 0 = no action, 1 = low, 2 = high 75 | // utep ticks = period 76 | // utez ticks = 0 77 | // utea ticks = a_cmpr 78 | // uteb ticks = b_cmpr 79 | for (uint8_t i = 0; i < 2; i++) 80 | for (uint8_t j = 0; j < 2; j++) 81 | { 82 | MCPWM0.channel[i].generator[j].utep = 0; 83 | MCPWM0.channel[i].generator[j].utez = 2; 84 | if (j == 0) 85 | MCPWM0.channel[i].generator[j].utea = 1; 86 | else 87 | MCPWM0.channel[i].generator[j].uteb = 1; 88 | } 89 | 90 | MCPWM0.update_cfg.global_up_en = 1; 91 | MCPWM0.update_cfg.global_force_up = 1; 92 | MCPWM0.update_cfg.global_force_up = 0; 93 | } 94 | 95 | void OneShot125::Arm() 96 | { 97 | // set duty to 250 us = 100 % 98 | MCPWM0.channel[0].cmpr_value[0].cmpr_val = 2000; 99 | MCPWM0.channel[0].cmpr_value[1].cmpr_val = 2000; 100 | MCPWM0.channel[1].cmpr_value[0].cmpr_val = 2000; 101 | MCPWM0.channel[1].cmpr_value[1].cmpr_val = 2000; 102 | 103 | // free run mode 104 | MCPWM0.timer[0].mode.start = 2; 105 | 106 | vTaskDelay(1000 / portTICK_RATE_MS); 107 | 108 | // set duty to 125 us = 0 % 109 | MCPWM0.channel[0].cmpr_value[0].cmpr_val = 1000; 110 | MCPWM0.channel[0].cmpr_value[1].cmpr_val = 1000; 111 | MCPWM0.channel[1].cmpr_value[0].cmpr_val = 1000; 112 | MCPWM0.channel[1].cmpr_value[1].cmpr_val = 1000; 113 | 114 | vTaskDelay(1000 / portTICK_RATE_MS); 115 | } 116 | 117 | void OneShot125::Disarm() 118 | { 119 | // stop timer 120 | MCPWM0.timer[0].mode.start = 0; 121 | 122 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_25, 0)); 123 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_26, 0)); 124 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_27, 0)); 125 | ESP_ERROR_CHECK(gpio_set_level(GPIO_NUM_14, 0)); 126 | } 127 | 128 | void OneShot125::Update(uint16_t motor_fl, uint16_t motor_bl, uint16_t motor_fr, uint16_t motor_br) 129 | { 130 | MCPWM0.timer[0].mode.start = 4; 131 | MCPWM0.timer[0].sync.sync_sw = !MCPWM0.timer[0].sync.sync_sw; 132 | 133 | if (motor_fl <= 1000) 134 | MCPWM0.channel[0].cmpr_value[0].cmpr_val = motor_fl + 1000; 135 | if (motor_bl <= 1000) 136 | MCPWM0.channel[0].cmpr_value[1].cmpr_val = motor_bl + 1000; 137 | if (motor_fr <= 1000) 138 | MCPWM0.channel[1].cmpr_value[0].cmpr_val = motor_fr + 1000; 139 | if (motor_br <= 1000) 140 | MCPWM0.channel[1].cmpr_value[1].cmpr_val = motor_br + 1000; 141 | } 142 | 143 | } 144 | -------------------------------------------------------------------------------- /components/PID/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/PID/include/PID.h: -------------------------------------------------------------------------------- 1 | /* 2 | * PID.h 3 | * 4 | * Created on: 14. 7. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | 13 | #include "Biquad_Filter.h" 14 | 15 | 16 | namespace flyhero 17 | { 18 | 19 | class PID 20 | { 21 | private: 22 | Biquad_Filter d_term_lpf; 23 | int64_t last_t; 24 | float Kp, Ki, Kd; 25 | float integrator; 26 | float i_max; 27 | float last_d; 28 | float last_error; 29 | 30 | public: 31 | PID(float update_rate, float i_max = 0, float Kp = 0, float Ki = 0, float Kd = 0); 32 | 33 | float Get_PID(float error); 34 | 35 | void Set_Kp(float Kp); 36 | 37 | void Set_Ki(float Ki); 38 | 39 | void Set_Kd(float Kd); 40 | 41 | void Set_I_Max(float i_max); 42 | }; 43 | 44 | } /* namespace flyhero */ 45 | -------------------------------------------------------------------------------- /components/PID/src/PID.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * PID.cpp 3 | * 4 | * Created on: 14. 7. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include 9 | #include "PID.h" 10 | 11 | 12 | namespace flyhero 13 | { 14 | 15 | PID::PID(float update_rate, float i_max, float Kp, float Ki, float Kd) 16 | : d_term_lpf(Biquad_Filter::FILTER_LOW_PASS, update_rate, 20) 17 | { 18 | this->last_t = 0; 19 | this->integrator = 0; 20 | this->Kp = Kp; 21 | this->Ki = Ki; 22 | this->Kd = Kd; 23 | this->i_max = i_max; 24 | this->last_d = NAN; 25 | this->last_error = NAN; 26 | } 27 | 28 | float PID::Get_PID(float error) 29 | { 30 | int64_t now = esp_timer_get_time(); 31 | 32 | float delta_t = (now - this->last_t) * 0.000001f; 33 | 34 | if (this->last_t == 0 || delta_t > 1) 35 | { 36 | this->integrator = 0; 37 | delta_t = 0; 38 | } 39 | 40 | this->last_t = now; 41 | 42 | float output = 0; 43 | 44 | // proportional component 45 | output += error * this->Kp; 46 | 47 | // integral component 48 | if (this->Ki != 0 && delta_t > 0) 49 | { 50 | this->integrator += error * this->Ki * delta_t; 51 | 52 | if (this->integrator < -this->i_max) 53 | this->integrator = -this->i_max; 54 | if (this->integrator > this->i_max) 55 | this->integrator = this->i_max; 56 | 57 | output += this->integrator; 58 | } 59 | 60 | // derivative component 61 | if (this->Kd != 0 && delta_t > 0) 62 | { 63 | float derivative; 64 | 65 | if (std::isnan(this->last_d)) 66 | { 67 | derivative = 0; 68 | this->last_d = 0; 69 | } else 70 | derivative = (error - this->last_error) / delta_t; 71 | 72 | // apply 20 Hz biquad LPF 73 | derivative = this->d_term_lpf.Apply_Filter(derivative); 74 | 75 | this->last_error = error; 76 | this->last_d = derivative; 77 | 78 | output += derivative * this->Kd; 79 | } 80 | 81 | return output; 82 | } 83 | 84 | void PID::Set_Kp(float Kp) 85 | { 86 | this->Kp = Kp; 87 | } 88 | 89 | void PID::Set_Ki(float Ki) 90 | { 91 | this->Ki = Ki; 92 | } 93 | 94 | void PID::Set_Kd(float Kd) 95 | { 96 | this->Kd = Kd; 97 | } 98 | 99 | void PID::Set_I_Max(float i_max) 100 | { 101 | this->i_max = i_max; 102 | } 103 | 104 | } /* namespace flyhero */ 105 | -------------------------------------------------------------------------------- /components/Ultrasonic/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/Ultrasonic/include/HC_SR04.h: -------------------------------------------------------------------------------- 1 | /* 2 | * HC_SR04.h 3 | * 4 | * Created on: 19. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace flyhero 18 | { 19 | 20 | class HC_SR04 21 | { 22 | private: 23 | const gpio_num_t trigg_pin; 24 | const gpio_num_t echo_pin; 25 | const gpio_isr_t echo_handler; 26 | 27 | int64_t start; 28 | bool level_high; 29 | float distance; 30 | SemaphoreHandle_t distance_semaphore; 31 | 32 | esp_err_t trigg_init(); 33 | 34 | esp_err_t echo_init(); 35 | 36 | public: 37 | HC_SR04(gpio_num_t trigg, gpio_num_t echo, gpio_isr_t isr_handler); 38 | 39 | void Init(); 40 | 41 | void Trigger(); 42 | 43 | float Get_Distance(); 44 | 45 | void Echo_Callback(); 46 | }; 47 | 48 | } /* namespace flyhero */ 49 | -------------------------------------------------------------------------------- /components/Ultrasonic/src/HC_SR04.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * HC_SR04.cpp 3 | * 4 | * Created on: 19. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "HC_SR04.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | HC_SR04::HC_SR04(gpio_num_t trigg, gpio_num_t echo, gpio_isr_t isr_handler) : 15 | trigg_pin(trigg), echo_pin(echo), echo_handler(isr_handler) 16 | { 17 | this->level_high = false; 18 | this->distance = -1; 19 | this->distance_semaphore = xSemaphoreCreateBinary(); 20 | } 21 | 22 | esp_err_t HC_SR04::trigg_init() 23 | { 24 | gpio_config_t conf; 25 | conf.pin_bit_mask = (uint64_t) (((uint64_t) 1) << this->trigg_pin); 26 | conf.mode = GPIO_MODE_OUTPUT; 27 | conf.pull_up_en = GPIO_PULLUP_DISABLE; 28 | conf.pull_down_en = GPIO_PULLDOWN_DISABLE; 29 | conf.intr_type = GPIO_INTR_DISABLE; 30 | 31 | return gpio_config(&conf); 32 | } 33 | 34 | esp_err_t HC_SR04::echo_init() 35 | { 36 | esp_err_t state; 37 | 38 | gpio_config_t conf; 39 | conf.pin_bit_mask = (uint64_t) (((uint64_t) 1) << this->echo_pin); 40 | conf.mode = GPIO_MODE_INPUT; 41 | conf.pull_up_en = GPIO_PULLUP_DISABLE; 42 | conf.pull_down_en = GPIO_PULLDOWN_ENABLE; 43 | conf.intr_type = GPIO_INTR_ANYEDGE; 44 | 45 | if ((state = gpio_config(&conf))) 46 | return state; 47 | 48 | if ((state = gpio_install_isr_service(0))) 49 | return state; 50 | 51 | if ((state = gpio_isr_handler_add(this->echo_pin, this->echo_handler, (void *) this->echo_pin))) 52 | return state; 53 | 54 | return ESP_OK; 55 | } 56 | 57 | void HC_SR04::Init() 58 | { 59 | xSemaphoreGive(this->distance_semaphore); 60 | ESP_ERROR_CHECK(this->trigg_init()); 61 | ESP_ERROR_CHECK(this->echo_init()); 62 | } 63 | 64 | void HC_SR04::Trigger() 65 | { 66 | int64_t start, end; 67 | 68 | // we should not get errors here so no need to check 69 | gpio_set_level(this->trigg_pin, 1); 70 | start = esp_timer_get_time(); 71 | 72 | do 73 | { 74 | end = esp_timer_get_time(); 75 | } while (end - start < 10); 76 | 77 | gpio_set_level(this->trigg_pin, 0); 78 | } 79 | 80 | float HC_SR04::Get_Distance() 81 | { 82 | while (xSemaphoreTake(this->distance_semaphore, 0) != pdTRUE); 83 | 84 | float ret = this->distance; 85 | 86 | xSemaphoreGive(this->distance_semaphore); 87 | 88 | return ret; 89 | } 90 | 91 | // TODO use float: https://www.esp32.com/viewtopic.php?t=1292 92 | void HC_SR04::Echo_Callback() 93 | { 94 | this->level_high = !this->level_high; 95 | 96 | if (this->level_high) 97 | this->start = esp_timer_get_time(); 98 | else 99 | { 100 | int64_t end = esp_timer_get_time(); 101 | 102 | while (xSemaphoreTake(this->distance_semaphore, 0) != pdTRUE); 103 | 104 | this->distance = (end - this->start) * 0.000001f * 334 * 0.5f; 105 | 106 | xSemaphoreGive(this->distance_semaphore); 107 | } 108 | } 109 | 110 | } /* namespace flyhero */ 111 | -------------------------------------------------------------------------------- /components/Utility/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/Utility/include/Counting_Median_Finder.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by michal on 28.2.18. 3 | // 4 | 5 | #pragma once 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | template 12 | class Counting_Median_Finder 13 | { 14 | private: 15 | std::map map; 16 | std::vector keys; 17 | 18 | public: 19 | void Push_Value(T value); 20 | 21 | T Get_Median(); 22 | 23 | }; 24 | 25 | template 26 | void Counting_Median_Finder::Push_Value(T value) 27 | { 28 | if (this->map.find(value) != this->map.end()) 29 | this->map[value]++; 30 | else 31 | { 32 | this->keys.push_back(value); 33 | this->map[value] = 1; 34 | } 35 | } 36 | 37 | template 38 | T Counting_Median_Finder::Get_Median() 39 | { 40 | std::sort(this->keys.begin(), this->keys.end()); 41 | 42 | auto begin = this->keys.begin(); 43 | auto end = this->keys.end() - 1; 44 | 45 | if (begin == end) 46 | return *begin; 47 | 48 | int sum = this->map[*begin] - this->map[*end]; 49 | 50 | while (begin + 1 != end) 51 | { 52 | if (sum <= 0) 53 | { 54 | begin++; 55 | sum += this->map[*begin]; 56 | } else if (sum > 0) 57 | { 58 | end--; 59 | sum -= this->map[*end]; 60 | } 61 | } 62 | 63 | if (sum >= 0) 64 | return *begin; 65 | else 66 | return *end; 67 | } -------------------------------------------------------------------------------- /components/WiFi/Kconfig: -------------------------------------------------------------------------------- 1 | menu "Flyhero WiFi" 2 | 3 | config FLYHERO_WIFI_TCP_PORT 4 | int "TCP port" 5 | default 4821 6 | 7 | config FLYHERO_WIFI_UDP_PORT 8 | int "UDP port" 9 | default 4789 10 | 11 | config FLYHERO_WIFI_SSID 12 | string "SSID" 13 | default "DRONE WIFI" 14 | 15 | config FLYHERO_WIFI_SSID_VISIBLE 16 | bool "SSID visible" 17 | default y 18 | 19 | config FLYHERO_WIFI_WPA2_PASSPHRASE 20 | string "WPA2 passphrase" 21 | default flyhero00 22 | 23 | endmenu 24 | -------------------------------------------------------------------------------- /components/WiFi/component.mk: -------------------------------------------------------------------------------- 1 | COMPONENT_ADD_INCLUDEDIRS:= include 2 | COMPONENT_SRCDIRS:= src 3 | -------------------------------------------------------------------------------- /components/WiFi/include/CRC.h: -------------------------------------------------------------------------------- 1 | /* 2 | * CRC.h 3 | * 4 | * Created on: 16. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | 12 | 13 | namespace flyhero 14 | { 15 | 16 | class CRC 17 | { 18 | private: 19 | CRC() 20 | {}; 21 | 22 | public: 23 | static inline uint16_t CRC16(const uint8_t *data, uint8_t length); 24 | 25 | }; 26 | 27 | inline uint16_t CRC::CRC16(const uint8_t *data, uint8_t length) 28 | { 29 | const uint16_t CRC_POLYNOME = 0x1021; 30 | uint16_t crc = 0; 31 | 32 | for (uint8_t i = 0; i < length; i++) 33 | { 34 | crc ^= (int16_t) (data[i]) << 8; 35 | 36 | for (uint8_t j = 0; j < 8; j++) 37 | { 38 | if (crc & 0x8000) 39 | crc = (crc << 1) ^ CRC_POLYNOME; 40 | else 41 | crc = (crc << 1); 42 | } 43 | } 44 | 45 | return crc; 46 | } 47 | 48 | } /* namespace flyhero */ 49 | -------------------------------------------------------------------------------- /components/WiFi/include/WiFi_Controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * WiFi_Controller.h 3 | * 4 | * Created on: 13. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #pragma once 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | #include "LEDs.h" 20 | #include "CRC.h" 21 | #include "IMU.h" 22 | 23 | 24 | #define FUSION_ALGORITHMS_USED 1 25 | 26 | namespace flyhero 27 | { 28 | 29 | class WiFi_Controller 30 | { 31 | public: 32 | struct __attribute__((__packed__)) In_Datagram_Data 33 | { 34 | uint16_t throttle; 35 | uint16_t rate_roll_kp; 36 | uint16_t stab_roll_kp; 37 | uint16_t stab_roll_ki; 38 | uint16_t rate_pitch_kp; 39 | uint16_t stab_pitch_kp; 40 | uint16_t stab_pitch_ki; 41 | uint16_t rate_yaw_kp; 42 | }; 43 | 44 | struct __attribute__((__packed__)) Out_Datagram_Data 45 | { 46 | uint16_t free_time; 47 | 48 | IMU::Euler_Angles euler[FUSION_ALGORITHMS_USED]; 49 | }; 50 | 51 | private: 52 | WiFi_Controller(); 53 | 54 | WiFi_Controller(const WiFi_Controller &); 55 | 56 | WiFi_Controller & operator=(const WiFi_Controller &); 57 | 58 | struct __attribute__((__packed__)) in_data 59 | { 60 | uint8_t datagram_length; 61 | In_Datagram_Data datagram_contents; 62 | uint16_t crc; 63 | }; 64 | 65 | struct __attribute__((__packed__)) out_data 66 | { 67 | uint8_t datagram_length; 68 | Out_Datagram_Data datagram_contents; 69 | uint16_t crc; 70 | }; 71 | 72 | static const uint8_t IN_DATAGRAM_LENGTH = sizeof(in_data); 73 | static const uint8_t OUT_DATAGRAM_LENGTH = sizeof(out_data); 74 | 75 | union in_datagram 76 | { 77 | uint8_t raw_data[IN_DATAGRAM_LENGTH]; 78 | in_data data; 79 | }; 80 | 81 | union out_datagram 82 | { 83 | uint8_t raw_data[OUT_DATAGRAM_LENGTH]; 84 | out_data data; 85 | }; 86 | 87 | static const size_t TCP_BUFFER_LENGTH = 256; 88 | uint8_t tcp_buffer[TCP_BUFFER_LENGTH]; 89 | 90 | const uint16_t UDP_PORT = CONFIG_FLYHERO_WIFI_UDP_PORT; 91 | const uint16_t TCP_PORT = CONFIG_FLYHERO_WIFI_TCP_PORT; 92 | 93 | int udp_server_fd; 94 | int tcp_server_fd; 95 | int tcp_client_fd; 96 | 97 | sockaddr_in udp_client_socket; 98 | sockaddr_in tcp_client_address; 99 | 100 | uint32_t udp_client_socket_length; 101 | uint32_t tcp_client_address_length; 102 | 103 | bool client_connected; 104 | 105 | void ap_init(); 106 | 107 | public: 108 | static WiFi_Controller & Instance(); 109 | 110 | void Init(); 111 | 112 | esp_err_t UDP_Server_Start(); 113 | 114 | esp_err_t UDP_Server_Stop(); 115 | 116 | esp_err_t TCP_Server_Start(); 117 | 118 | esp_err_t TCP_Server_Stop(); 119 | 120 | esp_err_t TCP_Wait_For_Client(); 121 | 122 | bool UDP_Receive(In_Datagram_Data & datagram_data); 123 | 124 | bool UDP_Send(const Out_Datagram_Data & datagram_data); 125 | 126 | bool TCP_Receive(const char *& data, size_t & received_length); 127 | 128 | bool TCP_Receive(const uint8_t *& data, size_t & received_length); 129 | 130 | bool TCP_Send(const char * data, size_t data_length); 131 | 132 | bool TCP_Send(const uint8_t * data, size_t data_length); 133 | 134 | void Client_Connected_Callback(); 135 | 136 | void Client_Disconnected_Callback(); 137 | 138 | }; 139 | 140 | } /* namespace flyhero */ 141 | -------------------------------------------------------------------------------- /components/WiFi/src/WiFi_Controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * WiFi_Controller.cpp 3 | * 4 | * Created on: 13. 9. 2017 5 | * Author: michp 6 | */ 7 | 8 | #include "WiFi_Controller.h" 9 | 10 | 11 | namespace flyhero 12 | { 13 | 14 | static esp_err_t event_handler(void * ctx, system_event_t * event) 15 | { 16 | switch (event->event_id) 17 | { 18 | case SYSTEM_EVENT_AP_STACONNECTED: 19 | WiFi_Controller::Instance().Client_Connected_Callback(); 20 | 21 | LEDs::Turn_On(LEDs::WARNING); 22 | vTaskDelay(250 / portTICK_RATE_MS); 23 | LEDs::Turn_Off(LEDs::WARNING); 24 | break; 25 | case SYSTEM_EVENT_AP_STADISCONNECTED: 26 | WiFi_Controller::Instance().Client_Disconnected_Callback(); 27 | 28 | LEDs::Turn_On(LEDs::WARNING); 29 | vTaskDelay(1000 / portTICK_RATE_MS); 30 | LEDs::Turn_Off(LEDs::WARNING); 31 | break; 32 | default: 33 | break; 34 | } 35 | 36 | return ESP_OK; 37 | } 38 | 39 | WiFi_Controller & WiFi_Controller::Instance() 40 | { 41 | static WiFi_Controller instance; 42 | 43 | return instance; 44 | } 45 | 46 | WiFi_Controller::WiFi_Controller() 47 | { 48 | this->udp_server_fd = -1; 49 | this->udp_client_socket_length = sizeof(this->udp_client_socket); 50 | 51 | this->tcp_server_fd = -1; 52 | this->tcp_client_fd = -1; 53 | this->tcp_client_address_length = sizeof(this->tcp_client_address); 54 | 55 | this->client_connected = false; 56 | } 57 | 58 | void WiFi_Controller::ap_init() 59 | { 60 | tcpip_adapter_init(); 61 | 62 | ESP_ERROR_CHECK(esp_event_loop_init(event_handler, NULL)); 63 | 64 | wifi_init_config_t config = WIFI_INIT_CONFIG_DEFAULT(); 65 | 66 | ESP_ERROR_CHECK(esp_wifi_init(&config)); 67 | ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP)); 68 | 69 | wifi_config_t wifi_config; 70 | wifi_config.ap.authmode = WIFI_AUTH_WPA_WPA2_PSK; 71 | wifi_config.ap.beacon_interval = 100; 72 | wifi_config.ap.channel = 7; 73 | wifi_config.ap.max_connection = 1; 74 | 75 | assert(strlen(CONFIG_FLYHERO_WIFI_SSID) >= 1); 76 | assert(strlen(CONFIG_FLYHERO_WIFI_WPA2_PASSPHRASE) >= 8); 77 | 78 | std::strcpy((char *) wifi_config.ap.password, CONFIG_FLYHERO_WIFI_WPA2_PASSPHRASE); 79 | std::strcpy((char *) wifi_config.ap.ssid, CONFIG_FLYHERO_WIFI_SSID); 80 | wifi_config.ap.ssid_hidden = !CONFIG_FLYHERO_WIFI_SSID_VISIBLE; 81 | wifi_config.ap.ssid_len = 0; 82 | 83 | ESP_ERROR_CHECK(esp_wifi_set_config(ESP_IF_WIFI_AP, &wifi_config)); 84 | ESP_ERROR_CHECK(esp_wifi_start()); 85 | } 86 | 87 | void WiFi_Controller::Init() 88 | { 89 | this->ap_init(); 90 | } 91 | 92 | esp_err_t WiFi_Controller::UDP_Server_Start() 93 | { 94 | this->udp_server_fd = socket(AF_INET, SOCK_DGRAM, 0); 95 | 96 | if (this->udp_server_fd < 0) 97 | return ESP_FAIL; 98 | 99 | sockaddr_in udp_server_address; 100 | udp_server_address.sin_addr.s_addr = htonl(INADDR_ANY); 101 | udp_server_address.sin_port = htons(this->UDP_PORT); 102 | udp_server_address.sin_family = AF_INET; 103 | 104 | if (fcntl(this->udp_server_fd, F_SETFL, O_NONBLOCK) != 0) 105 | { 106 | closesocket(this->udp_server_fd); 107 | this->udp_server_fd = -1; 108 | return ESP_FAIL; 109 | } 110 | 111 | if (bind(this->udp_server_fd, (sockaddr *) &udp_server_address, sizeof(udp_server_address)) < 0) 112 | { 113 | closesocket(this->udp_server_fd); 114 | this->udp_server_fd = -1; 115 | return ESP_FAIL; 116 | } 117 | 118 | return ESP_OK; 119 | } 120 | 121 | esp_err_t WiFi_Controller::UDP_Server_Stop() 122 | { 123 | if (this->udp_server_fd < 0) 124 | return ESP_FAIL; 125 | 126 | closesocket(this->udp_server_fd); 127 | this->udp_server_fd = -1; 128 | return ESP_OK; 129 | } 130 | 131 | esp_err_t WiFi_Controller::TCP_Server_Start() 132 | { 133 | this->tcp_server_fd = socket(AF_INET, SOCK_STREAM, 0); 134 | 135 | if (this->tcp_server_fd < 0) 136 | return ESP_FAIL; 137 | 138 | sockaddr_in tcp_server_address; 139 | tcp_server_address.sin_addr.s_addr = htonl(INADDR_ANY); 140 | tcp_server_address.sin_port = htons(this->TCP_PORT); 141 | tcp_server_address.sin_family = AF_INET; 142 | 143 | if (fcntl(this->tcp_server_fd, F_SETFL, O_NONBLOCK) != 0) 144 | { 145 | closesocket(this->tcp_server_fd); 146 | this->tcp_server_fd = -1; 147 | return ESP_FAIL; 148 | } 149 | 150 | if (bind(this->tcp_server_fd, (sockaddr *) &tcp_server_address, sizeof(tcp_server_address)) < 0) 151 | { 152 | closesocket(this->tcp_server_fd); 153 | this->tcp_server_fd = -1; 154 | return ESP_FAIL; 155 | } 156 | 157 | if (listen(this->tcp_server_fd, 1) < 0) 158 | { 159 | closesocket(this->tcp_server_fd); 160 | this->tcp_server_fd = -1; 161 | return ESP_FAIL; 162 | } 163 | 164 | return ESP_OK; 165 | } 166 | 167 | esp_err_t WiFi_Controller::TCP_Server_Stop() 168 | { 169 | if (this->tcp_server_fd < 0) 170 | return ESP_FAIL; 171 | 172 | closesocket(this->tcp_server_fd); 173 | this->tcp_server_fd = -1; 174 | return ESP_OK; 175 | } 176 | 177 | esp_err_t WiFi_Controller::TCP_Wait_For_Client() 178 | { 179 | if (this->tcp_server_fd < 0) 180 | return ESP_FAIL; 181 | 182 | do 183 | { 184 | this->tcp_client_fd = 185 | accept(this->tcp_server_fd, 186 | (sockaddr *) &this->tcp_client_address, 187 | &this->tcp_client_address_length); 188 | 189 | } while (this->tcp_client_fd < 0 && (errno == EAGAIN || errno == EWOULDBLOCK)); 190 | 191 | if (this->tcp_client_fd < 0) 192 | { 193 | closesocket(this->tcp_server_fd); 194 | this->tcp_server_fd = -1; 195 | 196 | return ESP_FAIL; 197 | } 198 | 199 | return ESP_OK; 200 | } 201 | 202 | bool WiFi_Controller::UDP_Receive(In_Datagram_Data & datagram_data) 203 | { 204 | in_datagram datagram; 205 | 206 | if (recvfrom(this->udp_server_fd, datagram.raw_data, IN_DATAGRAM_LENGTH, 0, 207 | (sockaddr *) (&this->udp_client_socket), &this->udp_client_socket_length) != IN_DATAGRAM_LENGTH) 208 | return false; 209 | 210 | if (datagram.data.datagram_length != IN_DATAGRAM_LENGTH) 211 | return false; 212 | 213 | if (datagram.data.crc != 214 | CRC::CRC16(datagram.raw_data, IN_DATAGRAM_LENGTH - 2)) 215 | return false; 216 | 217 | datagram_data = datagram.data.datagram_contents; 218 | 219 | return true; 220 | } 221 | 222 | bool WiFi_Controller::UDP_Send(const Out_Datagram_Data & datagram_data) 223 | { 224 | if (!this->client_connected) 225 | return false; 226 | 227 | out_datagram datagram; 228 | 229 | datagram.data.datagram_length = OUT_DATAGRAM_LENGTH; 230 | datagram.data.datagram_contents = datagram_data; 231 | datagram.data.crc = CRC::CRC16(datagram.raw_data, OUT_DATAGRAM_LENGTH - 2); 232 | 233 | return sendto(this->udp_server_fd, datagram.raw_data, OUT_DATAGRAM_LENGTH, 0, 234 | (sockaddr *) &this->udp_client_socket, this->udp_client_socket_length) == 235 | datagram.data.datagram_length; 236 | 237 | } 238 | 239 | bool WiFi_Controller::TCP_Receive(const char *& data, size_t & received_length) 240 | { 241 | // in case that client reconnected 242 | int tmp_fd = accept(this->tcp_server_fd, 243 | (sockaddr *) &this->tcp_client_address, 244 | &this->tcp_client_address_length); 245 | if (tmp_fd >= 0) 246 | this->tcp_client_fd = tmp_fd; 247 | 248 | ssize_t len; 249 | if ((len = recv(this->tcp_client_fd, this->tcp_buffer, this->TCP_BUFFER_LENGTH, MSG_DONTWAIT)) <= 0) 250 | { 251 | received_length = 0; 252 | return false; 253 | } 254 | 255 | data = (const char *) this->tcp_buffer; 256 | received_length = (size_t) len; 257 | 258 | return true; 259 | } 260 | 261 | bool WiFi_Controller::TCP_Receive(const uint8_t *& data, size_t & received_length) 262 | { 263 | // in case that client reconnected 264 | int tmp_fd = accept(this->tcp_server_fd, 265 | (sockaddr *) &this->tcp_client_address, 266 | &this->tcp_client_address_length); 267 | if (tmp_fd >= 0) 268 | this->tcp_client_fd = tmp_fd; 269 | 270 | ssize_t len; 271 | if ((len = recv(this->tcp_client_fd, this->tcp_buffer, this->TCP_BUFFER_LENGTH, MSG_DONTWAIT)) <= 0) 272 | { 273 | received_length = 0; 274 | return false; 275 | } 276 | 277 | data = this->tcp_buffer; 278 | received_length = (size_t) len; 279 | 280 | return true; 281 | } 282 | 283 | bool WiFi_Controller::TCP_Send(const char * data, size_t data_length) 284 | { 285 | if (!this->client_connected) 286 | return false; 287 | 288 | return send(this->tcp_client_fd, data, data_length, 0) == data_length; 289 | } 290 | 291 | bool WiFi_Controller::TCP_Send(const uint8_t * data, size_t data_length) 292 | { 293 | if (!this->client_connected) 294 | return false; 295 | 296 | return send(this->tcp_client_fd, data, data_length, 0) == data_length; 297 | } 298 | 299 | void WiFi_Controller::Client_Connected_Callback() 300 | { 301 | this->client_connected = true; 302 | } 303 | 304 | void WiFi_Controller::Client_Disconnected_Callback() 305 | { 306 | this->client_connected = false; 307 | } 308 | 309 | } /* namespace flyhero */ 310 | -------------------------------------------------------------------------------- /flyhero_crash_debug.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | "$IDF_PATH"/components/espcoredump/espcoredump.py -p /dev/ttyUSB0 dbg_corefile Flyhero_App/build/Flyhero_App.elf 4 | -------------------------------------------------------------------------------- /flyhero_crash_info.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | "$IDF_PATH"/components/espcoredump/espcoredump.py -p /dev/ttyUSB0 info_corefile Flyhero_App/build/Flyhero_App.elf 4 | -------------------------------------------------------------------------------- /flyhero_test.bash: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | if [ $# -eq 0 ]; then 4 | cd components 5 | set "$(echo *)" 6 | cd .. 7 | fi 8 | 9 | make flash monitor -C ${IDF_PATH}/tools/unit-test-app EXTRA_COMPONENT_DIRS="$PWD"/components/ TEST_COMPONENTS="$*" 10 | --------------------------------------------------------------------------------