├── .cproject ├── .gitattributes ├── .gitignore ├── .project ├── .settings ├── language.settings.xml ├── org.eclipse.cdt.core.prefs └── stm32cubeide.project.prefs ├── Clibrary └── include │ ├── config.h │ ├── define.h │ ├── gnss │ ├── gps.h │ ├── ublox.h │ └── um482.h │ ├── hal.h │ └── ringbuffer.h ├── Core ├── Inc │ ├── FreeRTOSConfig.h │ ├── adc.h │ ├── can.h │ ├── dma.h │ ├── gpio.h │ ├── i2c.h │ ├── main.h │ ├── sdio.h │ ├── spi.h │ ├── stm32f4xx_hal_conf.h │ ├── stm32f4xx_it.h │ ├── tim.h │ └── usart.h ├── Src │ ├── adc.c │ ├── can.c │ ├── dma.c │ ├── freertos.c │ ├── gpio.c │ ├── i2c.c │ ├── main.c │ ├── sdio.c │ ├── spi.c │ ├── stm32f4xx_hal_msp.c │ ├── stm32f4xx_hal_timebase_tim.c │ ├── stm32f4xx_it.c │ ├── syscalls.c │ ├── system_stm32f4xx.c │ ├── tim.c │ └── usart.c └── Startup │ └── startup_stm32f427vitx.s ├── Cpplibrary └── include │ ├── accel │ └── accelCalibrator.h │ ├── ahrs │ └── ahrs.h │ ├── attitude │ └── attitude_multi.h │ ├── compass │ ├── compassCalibrator.h │ └── declination.h │ ├── ekf │ ├── ekf_baro.h │ ├── ekf_gnss.h │ ├── ekf_odometry.h │ └── ekf_rangefinder.h │ ├── filter │ ├── DerivativeFilter.h │ ├── FilterClass.h │ ├── FilterWithBuffer.h │ ├── LowPassFilter.h │ └── LowPassFilter2p.h │ ├── flash │ └── flash.h │ ├── math │ ├── common.h │ ├── geodesicgrid.h │ ├── location.h │ ├── math_inc.h │ ├── matrix3.h │ ├── matrixN.h │ ├── quaternion.h │ ├── rotations.h │ ├── vector2.h │ ├── vector3.h │ └── vectorN.h │ ├── motors │ └── motors.h │ ├── pid │ ├── p.h │ ├── pid.h │ └── pid_2d.h │ ├── position │ └── position.h │ ├── sdlog │ └── sdlog.h │ └── uwb │ ├── deca_device_api.h │ ├── deca_param_types.h │ ├── deca_regs.h │ ├── deca_version.h │ ├── trilateration.h │ └── uwb.h ├── Drivers ├── CMSIS │ ├── Device │ │ └── ST │ │ │ └── STM32F4xx │ │ │ ├── Include │ │ │ ├── stm32f427xx.h │ │ │ ├── stm32f4xx.h │ │ │ └── system_stm32f4xx.h │ │ │ ├── LICENSE.txt │ │ │ └── License.md │ ├── Include │ │ ├── cmsis_armcc.h │ │ ├── cmsis_armclang.h │ │ ├── cmsis_compiler.h │ │ ├── cmsis_gcc.h │ │ ├── cmsis_iccarm.h │ │ ├── cmsis_version.h │ │ ├── core_armv8mbl.h │ │ ├── core_armv8mml.h │ │ ├── core_cm0.h │ │ ├── core_cm0plus.h │ │ ├── core_cm1.h │ │ ├── core_cm23.h │ │ ├── core_cm3.h │ │ ├── core_cm33.h │ │ ├── core_cm4.h │ │ ├── core_cm7.h │ │ ├── core_sc000.h │ │ ├── core_sc300.h │ │ ├── mpu_armv7.h │ │ ├── mpu_armv8.h │ │ └── tz_context.h │ └── LICENSE.txt └── STM32F4xx_HAL_Driver │ ├── Inc │ ├── Legacy │ │ └── stm32_hal_legacy.h │ ├── stm32f4xx_hal.h │ ├── stm32f4xx_hal_adc.h │ ├── stm32f4xx_hal_adc_ex.h │ ├── stm32f4xx_hal_can.h │ ├── stm32f4xx_hal_cortex.h │ ├── stm32f4xx_hal_def.h │ ├── stm32f4xx_hal_dma.h │ ├── stm32f4xx_hal_dma_ex.h │ ├── stm32f4xx_hal_exti.h │ ├── stm32f4xx_hal_flash.h │ ├── stm32f4xx_hal_flash_ex.h │ ├── stm32f4xx_hal_flash_ramfunc.h │ ├── stm32f4xx_hal_gpio.h │ ├── stm32f4xx_hal_gpio_ex.h │ ├── stm32f4xx_hal_i2c.h │ ├── stm32f4xx_hal_i2c_ex.h │ ├── stm32f4xx_hal_pcd.h │ ├── stm32f4xx_hal_pcd_ex.h │ ├── stm32f4xx_hal_pwr.h │ ├── stm32f4xx_hal_pwr_ex.h │ ├── stm32f4xx_hal_rcc.h │ ├── stm32f4xx_hal_rcc_ex.h │ ├── stm32f4xx_hal_sd.h │ ├── stm32f4xx_hal_spi.h │ ├── stm32f4xx_hal_tim.h │ ├── stm32f4xx_hal_tim_ex.h │ ├── stm32f4xx_hal_uart.h │ ├── stm32f4xx_ll_adc.h │ ├── stm32f4xx_ll_bus.h │ ├── stm32f4xx_ll_cortex.h │ ├── stm32f4xx_ll_dma.h │ ├── stm32f4xx_ll_exti.h │ ├── stm32f4xx_ll_gpio.h │ ├── stm32f4xx_ll_i2c.h │ ├── stm32f4xx_ll_pwr.h │ ├── stm32f4xx_ll_rcc.h │ ├── stm32f4xx_ll_sdmmc.h │ ├── stm32f4xx_ll_system.h │ ├── stm32f4xx_ll_tim.h │ ├── stm32f4xx_ll_usart.h │ ├── stm32f4xx_ll_usb.h │ └── stm32f4xx_ll_utils.h │ ├── LICENSE.txt │ ├── License.md │ └── Src │ ├── stm32f4xx_hal.c │ ├── stm32f4xx_hal_adc.c │ ├── stm32f4xx_hal_adc_ex.c │ ├── stm32f4xx_hal_can.c │ ├── stm32f4xx_hal_cortex.c │ ├── stm32f4xx_hal_dma.c │ ├── stm32f4xx_hal_dma_ex.c │ ├── stm32f4xx_hal_exti.c │ ├── stm32f4xx_hal_flash.c │ ├── stm32f4xx_hal_flash_ex.c │ ├── stm32f4xx_hal_flash_ramfunc.c │ ├── stm32f4xx_hal_gpio.c │ ├── stm32f4xx_hal_i2c.c │ ├── stm32f4xx_hal_i2c_ex.c │ ├── stm32f4xx_hal_pcd.c │ ├── stm32f4xx_hal_pcd_ex.c │ ├── stm32f4xx_hal_pwr.c │ ├── stm32f4xx_hal_pwr_ex.c │ ├── stm32f4xx_hal_rcc.c │ ├── stm32f4xx_hal_rcc_ex.c │ ├── stm32f4xx_hal_sd.c │ ├── stm32f4xx_hal_spi.c │ ├── stm32f4xx_hal_tim.c │ ├── stm32f4xx_hal_tim_ex.c │ ├── stm32f4xx_hal_uart.c │ ├── stm32f4xx_ll_adc.c │ ├── stm32f4xx_ll_sdmmc.c │ └── stm32f4xx_ll_usb.c ├── FATFS ├── App │ ├── fatfs.c │ └── fatfs.h └── Target │ ├── bsp_driver_sd.c │ ├── bsp_driver_sd.h │ ├── fatfs_platform.c │ ├── fatfs_platform.h │ ├── ffconf.h │ ├── sd_diskio.c │ └── sd_diskio.h ├── Maincontroller ├── demo │ ├── demo_can.cpp │ ├── demo_ekf.cpp │ ├── demo_filter.cpp │ └── demo_pid.cpp ├── inc │ ├── common.h │ └── maincontroller.h └── src │ ├── common.cpp │ ├── maincontroller.cpp │ ├── mode_althold.cpp │ ├── mode_autonav.cpp │ ├── mode_mecanum.cpp │ ├── mode_perch.cpp │ ├── mode_poshold.cpp │ ├── mode_segway.cpp │ ├── mode_stabilize.cpp │ ├── mode_ugv_a.cpp │ ├── mode_ugv_p.cpp │ ├── mode_ugv_v.cpp │ ├── mode_usv_a.cpp │ └── mode_vcopter.cpp ├── Middlewares ├── ST │ └── STM32_USB_Device_Library │ │ ├── Class │ │ └── CDC │ │ │ ├── Inc │ │ │ └── usbd_cdc.h │ │ │ └── Src │ │ │ └── usbd_cdc.c │ │ ├── Core │ │ ├── Inc │ │ │ ├── usbd_core.h │ │ │ ├── usbd_ctlreq.h │ │ │ ├── usbd_def.h │ │ │ └── usbd_ioreq.h │ │ └── Src │ │ │ ├── usbd_core.c │ │ │ ├── usbd_ctlreq.c │ │ │ └── usbd_ioreq.c │ │ └── LICENSE.txt └── Third_Party │ ├── FatFs │ └── src │ │ ├── diskio.c │ │ ├── diskio.h │ │ ├── ff.c │ │ ├── ff.h │ │ ├── ff_gen_drv.c │ │ ├── ff_gen_drv.h │ │ ├── integer.h │ │ └── option │ │ ├── cc936.c │ │ └── syscall.c │ ├── FreeRTOS │ └── Source │ │ ├── CMSIS_RTOS_V2 │ │ ├── cmsis_os.h │ │ ├── cmsis_os2.c │ │ ├── cmsis_os2.h │ │ ├── freertos_mpool.h │ │ └── freertos_os2.h │ │ ├── LICENSE │ │ ├── croutine.c │ │ ├── event_groups.c │ │ ├── include │ │ ├── FreeRTOS.h │ │ ├── StackMacros.h │ │ ├── atomic.h │ │ ├── croutine.h │ │ ├── deprecated_definitions.h │ │ ├── event_groups.h │ │ ├── list.h │ │ ├── message_buffer.h │ │ ├── mpu_prototypes.h │ │ ├── mpu_wrappers.h │ │ ├── portable.h │ │ ├── projdefs.h │ │ ├── queue.h │ │ ├── semphr.h │ │ ├── stack_macros.h │ │ ├── stream_buffer.h │ │ ├── task.h │ │ └── timers.h │ │ ├── list.c │ │ ├── portable │ │ ├── GCC │ │ │ └── ARM_CM4F │ │ │ │ ├── port.c │ │ │ │ └── portmacro.h │ │ └── MemMang │ │ │ └── heap_4.c │ │ ├── queue.c │ │ ├── stream_buffer.c │ │ ├── tasks.c │ │ └── timers.c │ └── mavlink │ ├── checksum.h │ ├── common │ ├── common.h │ ├── mavlink.h │ ├── mavlink_msg_actuator_control_target.h │ ├── mavlink_msg_adsb_vehicle.h │ ├── mavlink_msg_altitude.h │ ├── mavlink_msg_att_pos_mocap.h │ ├── mavlink_msg_attitude.h │ ├── mavlink_msg_attitude_quaternion.h │ ├── mavlink_msg_attitude_quaternion_cov.h │ ├── mavlink_msg_attitude_target.h │ ├── mavlink_msg_auth_key.h │ ├── mavlink_msg_autopilot_version.h │ ├── mavlink_msg_battery_status.h │ ├── mavlink_msg_camera_trigger.h │ ├── mavlink_msg_change_operator_control.h │ ├── mavlink_msg_change_operator_control_ack.h │ ├── mavlink_msg_collision.h │ ├── mavlink_msg_command_ack.h │ ├── mavlink_msg_command_int.h │ ├── mavlink_msg_command_long.h │ ├── mavlink_msg_control_system_state.h │ ├── mavlink_msg_data_stream.h │ ├── mavlink_msg_data_transmission_handshake.h │ ├── mavlink_msg_debug.h │ ├── mavlink_msg_debug_vect.h │ ├── mavlink_msg_distance_sensor.h │ ├── mavlink_msg_encapsulated_data.h │ ├── mavlink_msg_estimator_status.h │ ├── mavlink_msg_extended_sys_state.h │ ├── mavlink_msg_fence_status.h │ ├── mavlink_msg_file_transfer_protocol.h │ ├── mavlink_msg_follow_target.h │ ├── mavlink_msg_global_position_int.h │ ├── mavlink_msg_global_position_int_cov.h │ ├── mavlink_msg_global_vision_position_estimate.h │ ├── mavlink_msg_gps2_raw.h │ ├── mavlink_msg_gps2_rtk.h │ ├── mavlink_msg_gps_global_origin.h │ ├── mavlink_msg_gps_inject_data.h │ ├── mavlink_msg_gps_input.h │ ├── mavlink_msg_gps_raw_int.h │ ├── mavlink_msg_gps_rtcm_data.h │ ├── mavlink_msg_gps_rtk.h │ ├── mavlink_msg_gps_status.h │ ├── mavlink_msg_heartbeat.h │ ├── mavlink_msg_high_latency.h │ ├── mavlink_msg_high_latency2.h │ ├── mavlink_msg_highres_imu.h │ ├── mavlink_msg_hil_actuator_controls.h │ ├── mavlink_msg_hil_controls.h │ ├── mavlink_msg_hil_gps.h │ ├── mavlink_msg_hil_optical_flow.h │ ├── mavlink_msg_hil_rc_inputs_raw.h │ ├── mavlink_msg_hil_sensor.h │ ├── mavlink_msg_hil_state.h │ ├── mavlink_msg_hil_state_quaternion.h │ ├── mavlink_msg_home_position.h │ ├── mavlink_msg_landing_target.h │ ├── mavlink_msg_link_node_status.h │ ├── mavlink_msg_local_position_ned.h │ ├── mavlink_msg_local_position_ned_cov.h │ ├── mavlink_msg_local_position_ned_system_global_offset.h │ ├── mavlink_msg_log_data.h │ ├── mavlink_msg_log_entry.h │ ├── mavlink_msg_log_erase.h │ ├── mavlink_msg_log_request_data.h │ ├── mavlink_msg_log_request_end.h │ ├── mavlink_msg_log_request_list.h │ ├── mavlink_msg_manual_control.h │ ├── mavlink_msg_manual_setpoint.h │ ├── mavlink_msg_memory_vect.h │ ├── mavlink_msg_message_interval.h │ ├── mavlink_msg_mission_ack.h │ ├── mavlink_msg_mission_changed.h │ ├── mavlink_msg_mission_clear_all.h │ ├── mavlink_msg_mission_count.h │ ├── mavlink_msg_mission_current.h │ ├── mavlink_msg_mission_item.h │ ├── mavlink_msg_mission_item_int.h │ ├── mavlink_msg_mission_item_reached.h │ ├── mavlink_msg_mission_request.h │ ├── mavlink_msg_mission_request_int.h │ ├── mavlink_msg_mission_request_list.h │ ├── mavlink_msg_mission_request_partial_list.h │ ├── mavlink_msg_mission_set_current.h │ ├── mavlink_msg_mission_write_partial_list.h │ ├── mavlink_msg_named_value_float.h │ ├── mavlink_msg_named_value_int.h │ ├── mavlink_msg_nav_controller_output.h │ ├── mavlink_msg_optical_flow.h │ ├── mavlink_msg_optical_flow_rad.h │ ├── mavlink_msg_param_map_rc.h │ ├── mavlink_msg_param_request_list.h │ ├── mavlink_msg_param_request_read.h │ ├── mavlink_msg_param_set.h │ ├── mavlink_msg_param_value.h │ ├── mavlink_msg_ping.h │ ├── mavlink_msg_position_target_global_int.h │ ├── mavlink_msg_position_target_local_ned.h │ ├── mavlink_msg_power_status.h │ ├── mavlink_msg_radio_status.h │ ├── mavlink_msg_raw_imu.h │ ├── mavlink_msg_raw_pressure.h │ ├── mavlink_msg_rc_channels.h │ ├── mavlink_msg_rc_channels_override.h │ ├── mavlink_msg_rc_channels_raw.h │ ├── mavlink_msg_rc_channels_scaled.h │ ├── mavlink_msg_request_data_stream.h │ ├── mavlink_msg_resource_request.h │ ├── mavlink_msg_safety_allowed_area.h │ ├── mavlink_msg_safety_set_allowed_area.h │ ├── mavlink_msg_scaled_imu.h │ ├── mavlink_msg_scaled_imu2.h │ ├── mavlink_msg_scaled_imu3.h │ ├── mavlink_msg_scaled_pressure.h │ ├── mavlink_msg_scaled_pressure2.h │ ├── mavlink_msg_scaled_pressure3.h │ ├── mavlink_msg_serial_control.h │ ├── mavlink_msg_servo_output_raw.h │ ├── mavlink_msg_set_actuator_control_target.h │ ├── mavlink_msg_set_attitude_target.h │ ├── mavlink_msg_set_gps_global_origin.h │ ├── mavlink_msg_set_home_position.h │ ├── mavlink_msg_set_mode.h │ ├── mavlink_msg_set_position_target_global_int.h │ ├── mavlink_msg_set_position_target_local_ned.h │ ├── mavlink_msg_sim_state.h │ ├── mavlink_msg_statustext.h │ ├── mavlink_msg_sys_status.h │ ├── mavlink_msg_system_time.h │ ├── mavlink_msg_terrain_check.h │ ├── mavlink_msg_terrain_data.h │ ├── mavlink_msg_terrain_report.h │ ├── mavlink_msg_terrain_request.h │ ├── mavlink_msg_timesync.h │ ├── mavlink_msg_v2_extension.h │ ├── mavlink_msg_vfr_hud.h │ ├── mavlink_msg_vibration.h │ ├── mavlink_msg_vicon_position_estimate.h │ ├── mavlink_msg_vision_position_estimate.h │ ├── mavlink_msg_vision_speed_estimate.h │ ├── mavlink_msg_wind_cov.h │ ├── testsuite.h │ └── version.h │ ├── mavlink_conversions.h │ ├── mavlink_helpers.h │ ├── mavlink_types.h │ └── protocol.h ├── README.md ├── STM32F427VITX_FLASH.ld ├── STM32F427VITX_RAM.ld └── USB_DEVICE ├── App ├── usb_device.c ├── usb_device.h ├── usbd_cdc_if.c ├── usbd_cdc_if.h ├── usbd_desc.c └── usbd_desc.h └── Target ├── usbd_conf.c └── usbd_conf.h /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.a 3 | Debug/* 4 | -------------------------------------------------------------------------------- /.project: -------------------------------------------------------------------------------- 1 | 2 | 3 | Mcontroller-v5 4 | 5 | 6 | 7 | 8 | 9 | org.eclipse.cdt.managedbuilder.core.genmakebuilder 10 | clean,full,incremental, 11 | 12 | 13 | 14 | 15 | org.eclipse.cdt.managedbuilder.core.ScannerConfigBuilder 16 | full,incremental, 17 | 18 | 19 | 20 | 21 | 22 | com.st.stm32cube.ide.mcu.MCUProjectNature 23 | com.st.stm32cube.ide.mcu.MCUCubeProjectNature 24 | org.eclipse.cdt.core.cnature 25 | org.eclipse.cdt.core.ccnature 26 | com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature 27 | com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature 28 | com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature 29 | com.st.stm32cube.ide.mcu.MCURootProjectNature 30 | org.eclipse.cdt.managedbuilder.core.managedBuildNature 31 | org.eclipse.cdt.managedbuilder.core.ScannerConfigNature 32 | 33 | 34 | -------------------------------------------------------------------------------- /.settings/language.settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /.settings/org.eclipse.cdt.core.prefs: -------------------------------------------------------------------------------- 1 | doxygen/doxygen_new_line_after_brief=true 2 | doxygen/doxygen_use_brief_tag=false 3 | doxygen/doxygen_use_javadoc_tags=true 4 | doxygen/doxygen_use_pre_tag=false 5 | doxygen/doxygen_use_structural_commands=false 6 | eclipse.preferences.version=1 7 | -------------------------------------------------------------------------------- /.settings/stm32cubeide.project.prefs: -------------------------------------------------------------------------------- 1 | 2F62501ED4689FB349E356AB974DBE57=501C117985193D73CC1777B726211330 2 | 8DF89ED150041C4CBC7CB9A9CAA90856=501C117985193D73CC1777B726211330 3 | DC22A860405A8BF2F2C095E5B6529F12=F57592913AF09D744BEB7C61D67FE7CB 4 | eclipse.preferences.version=1 5 | -------------------------------------------------------------------------------- /Clibrary/include/config.h: -------------------------------------------------------------------------------- 1 | /* 2 | * config.h 3 | * 4 | * Created on: 2021年7月12日 5 | * Author: JackyPan 6 | */ 7 | 8 | #pragma once 9 | 10 | #ifndef INCLUDE_CONFIG_H_ 11 | #define INCLUDE_CONFIG_H_ 12 | 13 | #include "hal.h" 14 | 15 | /**** 数字舵机PWM频率为320HZ **** 16 | **** 模拟舵机PWM频率为50HZ **** 17 | **** 电机PWM频率为400HZ ****/ 18 | #define PWM_MOTOR_FREQ 400 19 | #define PWM_DIGITAL_SERVO_FREQ 320 20 | #define PWM_ANALOG_SERVO_FREQ 50 21 | 22 | #define PWM_SERVO_MIN 500 // 脉宽 500us 23 | #define PWM_SERVO_MAX 2500 // 脉宽 2500us 24 | 25 | #define PWM_ESC_MIN 1000 // 脉宽 1000us 26 | #define PWM_ESC_MAX 2000 // 脉宽 2000us 27 | #define PWM_ESC_SPIN_ARM 1100 28 | #define PWM_ESC_SPIN_MIN 1100 29 | #define PWM_ESC_SPIN_MAX 1950 30 | 31 | #define PWM_BRUSH_MIN 0 // 脉宽 0us 32 | #define PWM_BRUSH_MAX 2500 // 脉宽 2500us 33 | #define PWM_BRUSH_SPIN_ARM 100 34 | #define PWM_BRUSH_SPIN_MIN 100 35 | #define PWM_BRUSH_SPIN_MAX 2400 36 | 37 | /**************遥控信号输入**************/ 38 | #define RC_INPUT_PPM 0 //PPM信号 39 | #define RC_INPUT_SBUS 1 //SBUS信号 40 | 41 | //配置遥控通道数 42 | #define RC_INPUT_CHANNELS 8 //8 RC input channel (value:1000~2000) 43 | 44 | //遥控器输入脉宽有效值范围 45 | #define RC_INPUT_MIN 1000.0f // 脉宽 1000us 46 | #define RC_INPUT_MAX 2000.0f // 脉宽 2000us 47 | #define RC_INPUT_MID 1500.0f // 脉宽 1500us 48 | #define RC_INPUT_RANGE 1000.0f // 脉宽 1000us 49 | 50 | //串口模式 51 | #define DEV_COMM 0x01 //自定义模式 52 | #define MAV_COMM 0x02 //Mavlink模式 53 | #define GPS_COMM 0x03 //GPS模式 54 | #define TFMINI_COMM 0X04 //TFmini激光测距仪 55 | #define LC302_COMM 0X05 //LC302光流 56 | 57 | /***************usb+串口配置**************** 58 | * *************COMM_0:USB口*************** 59 | * *************COMM_1:Serial串口1********** 60 | * *************COMM_2:Serial串口2********** 61 | * *************COMM_3:Serial串口3********** 62 | * *************COMM_4:Serial串口4********** 63 | * @param: 64 | * COMM_0~COMM_4可以配置为下列可选参数,参数及其含义如下: 65 | * (1)DEV_COMM 自定义模式 66 | * (2)MAV_COMM Mavlink模式 67 | * (3)GPS_COMM GPS模式 68 | * (4)TFMINI_COMM TFmini激光测距仪 69 | * (5)LC302_COMM LC302光流模块 70 | * **************************************/ 71 | #define COMM_0 MAV_COMM 72 | #define COMM_1 GPS_COMM 73 | #define COMM_2 MAV_COMM 74 | #define COMM_3 TFMINI_COMM 75 | #define COMM_4 MAV_COMM 76 | 77 | //配置LED 78 | #define FMU_LED_CONTROLL_ENABLE 1 // if use system default led control, set 1; if you want to control led by yourself, set 0; 79 | 80 | //配置磁罗盘 81 | #define USE_MAG 1 // if use mag, set 1; if not use mag, set 0; 82 | 83 | //配置GPS 84 | #define USE_GPS 1 // if use gps, set 1; if not use gps, set 0; 85 | 86 | //配置UWB 87 | #define USE_UWB 0 // if use uwb, set 1; if don't use uwb, set 0; 88 | 89 | //配置光流 90 | #define USE_FLOW 0 // if use optical flow, set 1; if don't use optical flow, set 0; 91 | 92 | //配置里程计 93 | #define USE_ODOMETRY 0 // if use odometry, set 1; if don't use odometry, set 0; 94 | 95 | //配置flash 96 | #define USE_FRAM 2 //保持默认值,请勿更改 97 | 98 | #if USE_FRAM==1 99 | #define ADDR_FLASH ((uint16_t)0x0000) 100 | #define ADDR_FLASH_DATA ((uint16_t)0x0002) /* the beginning of the real data package */ 101 | #define DATA_FLASH_LENGTH ((uint16_t)0x0020) /* each data package takes 32 bytes */ 102 | #elif USE_FRAM==2 103 | #define ADDR_FLASH ((uint16_t)0x0000) 104 | #define ADDR_FLASH_DATA ((uint16_t)0x0020) /* the beginning of the real data package */ 105 | #define DATA_FLASH_LENGTH ((uint16_t)0x0020) /* each data package takes 32 bytes */ 106 | #else 107 | #define ADDR_FLASH ((uint32_t)0x081D0000) /* bank2, Sector22: 128KB | 0x081D0000 - 0x081EFFFF, 1地址表示1byte */ 108 | /* the Sector22 must be the number of all data package, 109 | * so we can get how many packages were saved in the flash by reading Sector22*/ 110 | #define ADDR_FLASH_DATA ((uint32_t)0x081E0000) /* bank2, Sector23: 128KB | 0x081E0000 - 0x081FFFFF, the beginning of the real data package */ 111 | #define DATA_FLASH_LENGTH ((uint32_t)0x00000020) /* each data package takes 32 bytes */ 112 | #endif 113 | 114 | #define VERSION_HARDWARE 573 115 | #define VERSION_FIRMWARE 2024010201 116 | 117 | #endif /* INCLUDE_CONFIG_H_ */ 118 | -------------------------------------------------------------------------------- /Clibrary/include/define.h: -------------------------------------------------------------------------------- 1 | /* 2 | * define.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #ifdef M_PI 8 | # undef M_PI 9 | #endif 10 | #define M_PI (3.141592653589793f) 11 | 12 | #ifdef M_PI_2 13 | # undef M_PI_2 14 | #endif 15 | #define M_PI_2 (M_PI / 2) 16 | 17 | #define M_GOLDEN 1.6180339f 18 | 19 | #define M_2PI (M_PI * 2) 20 | 21 | // MATH_CHECK_INDEXES modifies some objects (e.g. SoloGimbalEKF) to 22 | // include more debug information. It is also used by some functions 23 | // to add extra code for debugging purposes. If you wish to activate 24 | // this, do it here or as part of the top-level Makefile - 25 | // e.g. Tools/Replay/Makefile 26 | #ifndef MATH_CHECK_INDEXES 27 | #define MATH_CHECK_INDEXES 0 28 | #endif 29 | 30 | #define DEG_TO_RAD (M_PI / 180.0f) 31 | #define RAD_TO_DEG (180.0f / M_PI) 32 | 33 | // Centi-degrees to radians 34 | #define DEGX100 5729.57795f 35 | 36 | // GPS Specific double precision conversions 37 | // The precision here does matter when using the wsg* functions for converting 38 | // between LLH and ECEF coordinates. 39 | #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS 40 | static const double DEG_TO_RAD_DOUBLE = asin(1) / 90; 41 | static const double RAD_TO_DEG_DOUBLE = 1 / DEG_TO_RAD_DOUBLE; 42 | #endif 43 | 44 | #define RadiansToCentiDegrees(x) (static_cast(x) * RAD_TO_DEG * static_cast(100)) 45 | 46 | // acceleration due to gravity in m/s/s 47 | #define GRAVITY_MSS 9.80665f 48 | 49 | // radius of earth in meters 50 | #define RADIUS_OF_EARTH 6378100 51 | 52 | // convert a longitude or latitude point to meters or centimeters. 53 | // Note: this does not include the longitude scaling which is dependent upon location 54 | #define LATLON_TO_M 0.01113195f 55 | #define LATLON_TO_CM 1.113195f 56 | 57 | // Semi-major axis of the Earth, in meters. 58 | #define WGS84_A 6378137.0f 59 | 60 | //Inverse flattening of the Earth 61 | #define WGS84_IF 298.257223563f 62 | 63 | // The flattening of the Earth 64 | #define WGS84_F ((double)1.0 / WGS84_IF) 65 | 66 | // Semi-minor axis of the Earth in meters 67 | #define WGS84_B (WGS84_A * (1 - WGS84_F)) 68 | 69 | // Eccentricity of the Earth 70 | #ifdef ALLOW_DOUBLE_MATH_FUNCTIONS 71 | static const double WGS84_E = (sqrt(2 * WGS84_F - WGS84_F * WGS84_F)); 72 | #endif 73 | 74 | #define C_TO_KELVIN 273.15f 75 | 76 | // Gas Constant is from Aerodynamics for Engineering Students, Third Edition, E.L.Houghton and N.B.Carruthers 77 | #define ISA_GAS_CONSTANT 287.26f 78 | #define ISA_LAPSE_RATE 0.0065f 79 | 80 | // Standard Sea Level values 81 | // Ref: https://en.wikipedia.org/wiki/Standard_sea_level 82 | #define SSL_AIR_DENSITY 1.225f // kg/m^3 83 | #define SSL_AIR_PRESSURE 101325.01576f // Pascal 84 | #define SSL_AIR_TEMPERATURE 288.15f // K 85 | 86 | #define INCH_OF_H2O_TO_PASCAL 248.84f 87 | 88 | /* 89 | use AP_ prefix to prevent conflict with OS headers, such as NuttX 90 | clock.h 91 | */ 92 | #define NSEC_PER_SEC 1000000000ULL 93 | #define NSEC_PER_USEC 1000ULL 94 | #define USEC_PER_SEC 1000000ULL 95 | #define USEC_PER_MSEC 1000ULL 96 | #define MSEC_PER_SEC 1000ULL 97 | #define SEC_PER_WEEK (7ULL * 86400ULL) 98 | #define MSEC_PER_WEEK (SEC_PER_WEEK * MSEC_PER_SEC) 99 | -------------------------------------------------------------------------------- /Clibrary/include/gnss/gps.h: -------------------------------------------------------------------------------- 1 | /* 2 | * gps.h 3 | * 4 | * Created on: 2020.01.18 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | /* Define to prevent recursive inclusion -------------------------------------*/ 9 | #ifndef __GPS_H 10 | #define __GPS_H 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | #include "hal.h" 17 | 18 | #define GPS_READ_BUFFER_SIZE 128 19 | #define DEBUG_GPS_ENABLE 0 20 | 21 | #if DEBUG_GPS_ENABLE==1 22 | #define GPS_DEBUG(format, ...) usb_printf (format, ##__VA_ARGS__) 23 | #else 24 | #define GPS_DEBUG(format,...) 25 | #endif 26 | 27 | typedef enum { 28 | GPS = 0, ///< normal GPS output 29 | RTCM ///< request RTCM output. This is used for (fixed position) base stations 30 | }OutputMode; 31 | 32 | typedef enum { 33 | UBLOX = 0, ///< UBLOX模组 34 | UM482 ///< UM482模组 35 | }GnssType; 36 | 37 | typedef enum { 38 | gnss_comm1 = 0, //串口1 39 | gnss_comm2, //串口2 40 | gnss_comm3, //串口3 41 | gnss_comm4 //串口4 42 | }GnssComm; 43 | 44 | typedef struct 45 | { 46 | uint64_t timestamp; // required for logger 47 | uint8_t count; 48 | uint8_t svid[20]; 49 | uint8_t used[20]; 50 | uint8_t elevation[20]; 51 | uint8_t azimuth[20]; 52 | uint8_t snr[20]; 53 | uint8_t _padding0[3]; // required for logger 54 | //const uint8_t SAT_INFO_MAX_SATELLITES = 20; 55 | }satellite_info_s; 56 | 57 | typedef struct 58 | { 59 | uint64_t timestamp; // required for logger 60 | uint64_t time_utc_usec; 61 | int32_t lat; 62 | int32_t lon; 63 | int32_t alt; 64 | int32_t alt_ellipsoid; 65 | float s_variance_m_s; 66 | float c_variance_rad; 67 | float eph; 68 | float epv; 69 | float hdop; 70 | float vdop; 71 | int32_t noise_per_ms; 72 | int32_t jamming_indicator; 73 | float vel_m_s; 74 | float vel_n_m_s; 75 | float vel_e_m_s; 76 | float vel_d_m_s; 77 | float vel_n_std; 78 | float vel_e_std; 79 | float vel_d_std; 80 | float cog_rad; 81 | int32_t timestamp_time_relative; 82 | uint8_t fix_type; 83 | uint8_t heading_status;//0:无效解;4:固定解;5:浮动解; 84 | uint8_t vel_ned_valid; 85 | uint8_t satellites_used; 86 | uint8_t gps_used; 87 | uint8_t bds_used; 88 | uint8_t glo_used; 89 | float baseline_n;//基线 90 | float baseline_e; 91 | float baseline_u; 92 | float heading; //双天线定向, 单位:度 93 | float lat_noise; 94 | float lon_noise; 95 | float alt_noise; 96 | uint8_t _padding0[5]; // required for logger 97 | }vehicle_gps_position_s; 98 | 99 | struct tm 100 | { 101 | int tm_sec; /* second (0-61, allows for leap seconds) */ 102 | int tm_min; /* minute (0-59) */ 103 | int tm_hour; /* hour (0-23) */ 104 | int tm_mday; /* day of the month (1-31) */ 105 | int tm_mon; /* month (0-11) */ 106 | int tm_year; /* years since 1900 */ 107 | int tm_wday; /* day of the week (0-6) */ /*not supported by NuttX*/ 108 | int tm_yday; /* day of the year (0-365) */ /*not supported by NuttX*/ 109 | int tm_isdst; /* non-0 if daylight savings time is in effect */ /*not supported by NuttX*/ 110 | }; 111 | 112 | struct SurveyInStatus 113 | { 114 | uint32_t mean_accuracy; /**< [mm] */ 115 | uint32_t duration; /**< [s] */ 116 | uint8_t flags; /**< bit 0: valid, bit 1: active */ 117 | }; 118 | 119 | #define SAT_INFO_MAX_SATELLITES 20 120 | 121 | extern vehicle_gps_position_s *gps_position; 122 | extern GnssComm gnss_comm; 123 | bool GPS_Init(GnssType type);// type:GNSS模组类型 124 | void GPS_Baud_Reset(uint32_t baud); 125 | void get_gps_data(uint8_t buf); 126 | bool get_gps_state(void); 127 | 128 | #ifdef __cplusplus 129 | } 130 | #endif 131 | 132 | #endif /* __GPS_H */ 133 | -------------------------------------------------------------------------------- /Clibrary/include/gnss/um482.h: -------------------------------------------------------------------------------- 1 | /* 2 | * um482.h 3 | * 4 | * Created on: 2022年6月16日 5 | * Author: 25053 6 | */ 7 | 8 | #ifndef INCLUDE_GNSS_UM482_H_ 9 | #define INCLUDE_GNSS_UM482_H_ 10 | 11 | #ifdef __cplusplus 12 | extern "C" { 13 | #endif 14 | 15 | #include "gnss/gps.h" 16 | 17 | void ParseUM482(uint8_t b); 18 | 19 | typedef enum { 20 | UM482_IDLE = 0, 21 | UM482_Header1_24, 22 | UM482_Header2_24, 23 | UM482_Header3_24, 24 | UM482_Gnss_4, 25 | UM482_Length_1, 26 | UM482_UTC_6,//年月日时分秒 27 | UM482_RTK_Status_1,//0:无效解;1:单点定位解;2:伪距差分;4:固定解; 5:浮动解; 28 | UM482_Heading_Status_1,//0:无效解;4:固定解;5:浮动解; 29 | UM482_GPS_Num_1, 30 | UM482_BDS_Num_1, 31 | UM482_GLO_Num_1, 32 | UM482_Baseline_N_4, 33 | UM482_Baseline_E_4, 34 | UM482_Baseline_U_4, 35 | UM482_Baseline_NStd_4, 36 | UM482_Baseline_EStd_4, 37 | UM482_Baseline_UStd_4, 38 | UM482_Heading_4, 39 | UM482_Pitch_4, 40 | UM482_Roll_4, 41 | UM482_Speed_4, 42 | UM482_Vel_N_4, 43 | UM482_Vel_E_4, 44 | UM482_Vel_U_4, 45 | UM482_Vel_NStd_4, 46 | UM482_Vel_EStd_4, 47 | UM482_Vel_UStd_4, 48 | UM482_Lat_8, 49 | UM482_Lon_8, 50 | UM482_Alt_8, 51 | UM482_ECEF_X_8, 52 | UM482_ECEF_Y_8, 53 | UM482_ECEF_Z_8, 54 | UM482_LatStd_4, 55 | UM482_LonStd_4, 56 | UM482_AltStd_4, 57 | UM482_ECEFStd_X_4, 58 | UM482_ECEFStd_Y_4, 59 | UM482_ECEFStd_Z_4, 60 | UM482_Base_Lat_8, 61 | UM482_Base_Lon_8, 62 | UM482_Base_Alt_8, 63 | UM482_SEC_Lat_8, 64 | UM482_SEC_Lon_8, 65 | UM482_SEC_Alt_8, 66 | UM482_Week_Second_4, 67 | UM482_Diffage_4, 68 | UM482_Speed_Heading_4, 69 | UM482_Undulation_4, 70 | Remain_float_8, 71 | UM482_Num_GAL_1, 72 | Remain_char_3, 73 | UM482_CRC_4 74 | }UM482_State; 75 | 76 | #ifdef __cplusplus 77 | } 78 | #endif 79 | 80 | #endif /* INCLUDE_GNSS_UM482_H_ */ 81 | -------------------------------------------------------------------------------- /Clibrary/include/ringbuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ringbuffer.h 3 | * 4 | * Created on: 2020.01.18 5 | * Author: JackyPan 6 | */ 7 | 8 | /* Define to prevent recursive inclusion -------------------------------------*/ 9 | #ifndef __RINGBUFFER_H 10 | #define __RINGBUFFER_H 11 | 12 | #ifdef __cplusplus 13 | extern "C" { 14 | #endif 15 | 16 | #include "hal.h" 17 | 18 | typedef struct { 19 | uint8_t* pBuff; 20 | uint8_t* pEnd; // pBuff + legnth 21 | uint8_t* wp; // Write Point 22 | uint8_t* rp; // Read Point 23 | uint16_t length; 24 | uint8_t flagOverflow; // set when buffer overflowed 25 | } RingBuffer; 26 | 27 | //初始化RingBuffer 28 | void rbInit(RingBuffer* pRingBuff, uint8_t* buff, uint16_t length); 29 | //清空RingBuffer 30 | void rbClear(RingBuffer* pRingBuff); 31 | //把一字节数据存进RingBuffer 32 | void rbPush(RingBuffer* pRingBuff, uint8_t value); 33 | //读取一个字节数据出来 34 | uint8_t rbPop(RingBuffer* pRingBuff); 35 | //当前还有多少数据没被读取 36 | uint16_t rbGetCount(const RingBuffer* pRingBuff); 37 | //当前RingBuffer是不是空的 38 | int8_t rbIsEmpty(const RingBuffer* pRingBuff); 39 | //当前RingBuffer是不是满的 40 | int8_t rbIsFull(const RingBuffer* pRingBuff); 41 | 42 | #ifdef __cplusplus 43 | } 44 | #endif 45 | 46 | #endif /* __RINGBUFFER_H */ 47 | -------------------------------------------------------------------------------- /Core/Inc/adc.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file adc.h 5 | * @brief This file contains all the function prototypes for 6 | * the adc.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __ADC_H__ 22 | #define __ADC_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern ADC_HandleTypeDef hadc1; 36 | 37 | /* USER CODE BEGIN Private defines */ 38 | 39 | /* USER CODE END Private defines */ 40 | 41 | void MX_ADC1_Init(void); 42 | 43 | /* USER CODE BEGIN Prototypes */ 44 | 45 | /* USER CODE END Prototypes */ 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* __ADC_H__ */ 52 | 53 | -------------------------------------------------------------------------------- /Core/Inc/can.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file can.h 5 | * @brief This file contains all the function prototypes for 6 | * the can.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __CAN_H__ 22 | #define __CAN_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern CAN_HandleTypeDef hcan2; 36 | 37 | /* USER CODE BEGIN Private defines */ 38 | 39 | /* USER CODE END Private defines */ 40 | 41 | void MX_CAN2_Init(void); 42 | 43 | /* USER CODE BEGIN Prototypes */ 44 | 45 | /* USER CODE END Prototypes */ 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* __CAN_H__ */ 52 | 53 | -------------------------------------------------------------------------------- /Core/Inc/dma.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file dma.h 5 | * @brief This file contains all the function prototypes for 6 | * the dma.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __DMA_H__ 22 | #define __DMA_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* DMA memory to memory transfer handles -------------------------------------*/ 32 | 33 | /* USER CODE BEGIN Includes */ 34 | 35 | /* USER CODE END Includes */ 36 | 37 | /* USER CODE BEGIN Private defines */ 38 | 39 | /* USER CODE END Private defines */ 40 | 41 | void MX_DMA_Init(void); 42 | 43 | /* USER CODE BEGIN Prototypes */ 44 | 45 | /* USER CODE END Prototypes */ 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* __DMA_H__ */ 52 | 53 | -------------------------------------------------------------------------------- /Core/Inc/gpio.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file gpio.h 5 | * @brief This file contains all the function prototypes for 6 | * the gpio.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __GPIO_H__ 22 | #define __GPIO_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | /* USER CODE BEGIN Private defines */ 36 | 37 | /* USER CODE END Private defines */ 38 | 39 | void MX_GPIO_Init(void); 40 | 41 | /* USER CODE BEGIN Prototypes */ 42 | 43 | /* USER CODE END Prototypes */ 44 | 45 | #ifdef __cplusplus 46 | } 47 | #endif 48 | #endif /*__ GPIO_H__ */ 49 | 50 | -------------------------------------------------------------------------------- /Core/Inc/i2c.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file i2c.h 5 | * @brief This file contains all the function prototypes for 6 | * the i2c.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __I2C_H__ 22 | #define __I2C_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern I2C_HandleTypeDef hi2c2; 36 | 37 | /* USER CODE BEGIN Private defines */ 38 | 39 | /* USER CODE END Private defines */ 40 | 41 | void MX_I2C2_Init(void); 42 | 43 | /* USER CODE BEGIN Prototypes */ 44 | 45 | /* USER CODE END Prototypes */ 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* __I2C_H__ */ 52 | 53 | -------------------------------------------------------------------------------- /Core/Inc/sdio.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file sdio.h 5 | * @brief This file contains all the function prototypes for 6 | * the sdio.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __SDIO_H__ 22 | #define __SDIO_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern SD_HandleTypeDef hsd; 36 | 37 | /* USER CODE BEGIN Private defines */ 38 | 39 | /* USER CODE END Private defines */ 40 | 41 | void MX_SDIO_SD_Init(void); 42 | 43 | /* USER CODE BEGIN Prototypes */ 44 | 45 | /* USER CODE END Prototypes */ 46 | 47 | #ifdef __cplusplus 48 | } 49 | #endif 50 | 51 | #endif /* __SDIO_H__ */ 52 | 53 | -------------------------------------------------------------------------------- /Core/Inc/spi.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file spi.h 5 | * @brief This file contains all the function prototypes for 6 | * the spi.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __SPI_H__ 22 | #define __SPI_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern SPI_HandleTypeDef hspi1; 36 | 37 | extern SPI_HandleTypeDef hspi2; 38 | 39 | extern SPI_HandleTypeDef hspi4; 40 | 41 | /* USER CODE BEGIN Private defines */ 42 | 43 | /* USER CODE END Private defines */ 44 | 45 | void MX_SPI1_Init(void); 46 | void MX_SPI2_Init(void); 47 | void MX_SPI4_Init(void); 48 | 49 | /* USER CODE BEGIN Prototypes */ 50 | 51 | /* USER CODE END Prototypes */ 52 | 53 | #ifdef __cplusplus 54 | } 55 | #endif 56 | 57 | #endif /* __SPI_H__ */ 58 | 59 | -------------------------------------------------------------------------------- /Core/Inc/stm32f4xx_it.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file stm32f4xx_it.h 5 | * @brief This file contains the headers of the interrupt handlers. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | *

© Copyright (c) 2020 STMicroelectronics. 10 | * All rights reserved.

11 | * 12 | * This software component is licensed by ST under BSD 3-Clause license, 13 | * the "License"; You may not use this file except in compliance with the 14 | * License. You may obtain a copy of the License at: 15 | * opensource.org/licenses/BSD-3-Clause 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | 21 | /* Define to prevent recursive inclusion -------------------------------------*/ 22 | #ifndef __STM32F4xx_IT_H 23 | #define __STM32F4xx_IT_H 24 | 25 | #ifdef __cplusplus 26 | extern "C" { 27 | #endif 28 | 29 | /* Private includes ----------------------------------------------------------*/ 30 | /* USER CODE BEGIN Includes */ 31 | 32 | /* USER CODE END Includes */ 33 | 34 | /* Exported types ------------------------------------------------------------*/ 35 | /* USER CODE BEGIN ET */ 36 | 37 | /* USER CODE END ET */ 38 | 39 | /* Exported constants --------------------------------------------------------*/ 40 | /* USER CODE BEGIN EC */ 41 | 42 | /* USER CODE END EC */ 43 | 44 | /* Exported macro ------------------------------------------------------------*/ 45 | /* USER CODE BEGIN EM */ 46 | 47 | /* USER CODE END EM */ 48 | 49 | /* Exported functions prototypes ---------------------------------------------*/ 50 | void NMI_Handler(void); 51 | void HardFault_Handler(void); 52 | void MemManage_Handler(void); 53 | void BusFault_Handler(void); 54 | void UsageFault_Handler(void); 55 | void DebugMon_Handler(void); 56 | void RCC_IRQHandler(void); 57 | void DMA1_Stream1_IRQHandler(void); 58 | void DMA1_Stream2_IRQHandler(void); 59 | void DMA1_Stream3_IRQHandler(void); 60 | void DMA1_Stream5_IRQHandler(void); 61 | void DMA1_Stream6_IRQHandler(void); 62 | void TIM1_UP_TIM10_IRQHandler(void); 63 | void TIM1_TRG_COM_TIM11_IRQHandler(void); 64 | void TIM2_IRQHandler(void); 65 | void TIM3_IRQHandler(void); 66 | void TIM4_IRQHandler(void); 67 | void I2C2_EV_IRQHandler(void); 68 | void SPI1_IRQHandler(void); 69 | void USART2_IRQHandler(void); 70 | void USART3_IRQHandler(void); 71 | void TIM8_UP_TIM13_IRQHandler(void); 72 | void TIM8_TRG_COM_TIM14_IRQHandler(void); 73 | void SDIO_IRQHandler(void); 74 | void TIM5_IRQHandler(void); 75 | void UART4_IRQHandler(void); 76 | void TIM7_IRQHandler(void); 77 | void DMA2_Stream0_IRQHandler(void); 78 | void DMA2_Stream3_IRQHandler(void); 79 | void DMA2_Stream4_IRQHandler(void); 80 | void CAN2_TX_IRQHandler(void); 81 | void CAN2_RX0_IRQHandler(void); 82 | void OTG_FS_IRQHandler(void); 83 | void DMA2_Stream5_IRQHandler(void); 84 | void DMA2_Stream6_IRQHandler(void); 85 | void UART7_IRQHandler(void); 86 | void UART8_IRQHandler(void); 87 | void SPI4_IRQHandler(void); 88 | /* USER CODE BEGIN EFP */ 89 | void EXTI1_IRQHandler(void); 90 | void EXTI3_IRQHandler(void); 91 | void EXTI4_IRQHandler(void); 92 | void EXTI9_5_IRQHandler(void); 93 | void EXTI15_10_IRQHandler(void); 94 | /* USER CODE END EFP */ 95 | 96 | #ifdef __cplusplus 97 | } 98 | #endif 99 | 100 | #endif /* __STM32F4xx_IT_H */ 101 | -------------------------------------------------------------------------------- /Core/Inc/tim.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file tim.h 5 | * @brief This file contains all the function prototypes for 6 | * the tim.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __TIM_H__ 22 | #define __TIM_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern TIM_HandleTypeDef htim1; 36 | 37 | extern TIM_HandleTypeDef htim2; 38 | 39 | extern TIM_HandleTypeDef htim3; 40 | 41 | extern TIM_HandleTypeDef htim4; 42 | 43 | extern TIM_HandleTypeDef htim7; 44 | 45 | extern TIM_HandleTypeDef htim8; 46 | 47 | extern TIM_HandleTypeDef htim10; 48 | 49 | extern TIM_HandleTypeDef htim11; 50 | 51 | extern TIM_HandleTypeDef htim13; 52 | 53 | extern TIM_HandleTypeDef htim14; 54 | 55 | /* USER CODE BEGIN Private defines */ 56 | 57 | /* USER CODE END Private defines */ 58 | 59 | void MX_TIM1_Init(void); 60 | void MX_TIM2_Init(void); 61 | void MX_TIM3_Init(void); 62 | void MX_TIM4_Init(void); 63 | void MX_TIM7_Init(void); 64 | void MX_TIM8_Init(void); 65 | void MX_TIM10_Init(void); 66 | void MX_TIM11_Init(void); 67 | void MX_TIM13_Init(void); 68 | void MX_TIM14_Init(void); 69 | 70 | void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim); 71 | 72 | /* USER CODE BEGIN Prototypes */ 73 | 74 | /* USER CODE END Prototypes */ 75 | 76 | #ifdef __cplusplus 77 | } 78 | #endif 79 | 80 | #endif /* __TIM_H__ */ 81 | 82 | -------------------------------------------------------------------------------- /Core/Inc/usart.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file usart.h 5 | * @brief This file contains all the function prototypes for 6 | * the usart.c file 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __USART_H__ 22 | #define __USART_H__ 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "main.h" 30 | 31 | /* USER CODE BEGIN Includes */ 32 | 33 | /* USER CODE END Includes */ 34 | 35 | extern UART_HandleTypeDef huart4; 36 | 37 | extern UART_HandleTypeDef huart7; 38 | 39 | extern UART_HandleTypeDef huart8; 40 | 41 | extern UART_HandleTypeDef huart2; 42 | 43 | extern UART_HandleTypeDef huart3; 44 | 45 | /* USER CODE BEGIN Private defines */ 46 | 47 | /* USER CODE END Private defines */ 48 | 49 | void MX_UART4_Init(void); 50 | void MX_UART7_Init(void); 51 | void MX_UART8_Init(void); 52 | void MX_USART2_UART_Init(void); 53 | void MX_USART3_UART_Init(void); 54 | 55 | /* USER CODE BEGIN Prototypes */ 56 | 57 | /* USER CODE END Prototypes */ 58 | 59 | #ifdef __cplusplus 60 | } 61 | #endif 62 | 63 | #endif /* __USART_H__ */ 64 | 65 | -------------------------------------------------------------------------------- /Core/Src/can.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file can.c 5 | * @brief This file provides code for the configuration 6 | * of the CAN instances. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Includes ------------------------------------------------------------------*/ 21 | #include "can.h" 22 | 23 | /* USER CODE BEGIN 0 */ 24 | 25 | /* USER CODE END 0 */ 26 | 27 | CAN_HandleTypeDef hcan2; 28 | 29 | /* CAN2 init function */ 30 | void MX_CAN2_Init(void) 31 | { 32 | 33 | /* USER CODE BEGIN CAN2_Init 0 */ 34 | 35 | /* USER CODE END CAN2_Init 0 */ 36 | 37 | /* USER CODE BEGIN CAN2_Init 1 */ 38 | 39 | /* USER CODE END CAN2_Init 1 */ 40 | hcan2.Instance = CAN2; 41 | hcan2.Init.Prescaler = 3; 42 | hcan2.Init.Mode = CAN_MODE_NORMAL; 43 | hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ; 44 | hcan2.Init.TimeSeg1 = CAN_BS1_8TQ; 45 | hcan2.Init.TimeSeg2 = CAN_BS2_5TQ; 46 | hcan2.Init.TimeTriggeredMode = DISABLE; 47 | hcan2.Init.AutoBusOff = DISABLE; 48 | hcan2.Init.AutoWakeUp = DISABLE; 49 | hcan2.Init.AutoRetransmission = DISABLE; 50 | hcan2.Init.ReceiveFifoLocked = DISABLE; 51 | hcan2.Init.TransmitFifoPriority = DISABLE; 52 | if (HAL_CAN_Init(&hcan2) != HAL_OK) 53 | { 54 | Error_Handler(); 55 | } 56 | /* USER CODE BEGIN CAN2_Init 2 */ 57 | 58 | /* USER CODE END CAN2_Init 2 */ 59 | 60 | } 61 | 62 | void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle) 63 | { 64 | 65 | GPIO_InitTypeDef GPIO_InitStruct = {0}; 66 | if(canHandle->Instance==CAN2) 67 | { 68 | /* USER CODE BEGIN CAN2_MspInit 0 */ 69 | 70 | /* USER CODE END CAN2_MspInit 0 */ 71 | /* CAN2 clock enable */ 72 | __HAL_RCC_CAN2_CLK_ENABLE(); 73 | __HAL_RCC_CAN1_CLK_ENABLE(); 74 | 75 | __HAL_RCC_GPIOB_CLK_ENABLE(); 76 | /**CAN2 GPIO Configuration 77 | PB12 ------> CAN2_RX 78 | PB6 ------> CAN2_TX 79 | */ 80 | GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_6; 81 | GPIO_InitStruct.Mode = GPIO_MODE_AF_PP; 82 | GPIO_InitStruct.Pull = GPIO_PULLUP; 83 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH; 84 | GPIO_InitStruct.Alternate = GPIO_AF9_CAN2; 85 | HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); 86 | 87 | /* CAN2 interrupt Init */ 88 | HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0); 89 | HAL_NVIC_EnableIRQ(CAN2_TX_IRQn); 90 | HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); 91 | HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); 92 | /* USER CODE BEGIN CAN2_MspInit 1 */ 93 | 94 | /* USER CODE END CAN2_MspInit 1 */ 95 | } 96 | } 97 | 98 | void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle) 99 | { 100 | 101 | if(canHandle->Instance==CAN2) 102 | { 103 | /* USER CODE BEGIN CAN2_MspDeInit 0 */ 104 | 105 | /* USER CODE END CAN2_MspDeInit 0 */ 106 | /* Peripheral clock disable */ 107 | __HAL_RCC_CAN2_CLK_DISABLE(); 108 | __HAL_RCC_CAN1_CLK_DISABLE(); 109 | 110 | /**CAN2 GPIO Configuration 111 | PB12 ------> CAN2_RX 112 | PB6 ------> CAN2_TX 113 | */ 114 | HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12|GPIO_PIN_6); 115 | 116 | /* CAN2 interrupt Deinit */ 117 | HAL_NVIC_DisableIRQ(CAN2_TX_IRQn); 118 | HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn); 119 | /* USER CODE BEGIN CAN2_MspDeInit 1 */ 120 | 121 | /* USER CODE END CAN2_MspDeInit 1 */ 122 | } 123 | } 124 | 125 | /* USER CODE BEGIN 1 */ 126 | 127 | /* USER CODE END 1 */ 128 | -------------------------------------------------------------------------------- /Core/Src/dma.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file dma.c 5 | * @brief This file provides code for the configuration 6 | * of all the requested memory to memory DMA transfers. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | 21 | /* Includes ------------------------------------------------------------------*/ 22 | #include "dma.h" 23 | 24 | /* USER CODE BEGIN 0 */ 25 | 26 | /* USER CODE END 0 */ 27 | 28 | /*----------------------------------------------------------------------------*/ 29 | /* Configure DMA */ 30 | /*----------------------------------------------------------------------------*/ 31 | 32 | /* USER CODE BEGIN 1 */ 33 | 34 | /* USER CODE END 1 */ 35 | 36 | /** 37 | * Enable DMA controller clock 38 | */ 39 | void MX_DMA_Init(void) 40 | { 41 | 42 | /* DMA controller clock enable */ 43 | __HAL_RCC_DMA1_CLK_ENABLE(); 44 | __HAL_RCC_DMA2_CLK_ENABLE(); 45 | 46 | /* DMA interrupt init */ 47 | /* DMA1_Stream1_IRQn interrupt configuration */ 48 | HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 7, 0); 49 | HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn); 50 | /* DMA1_Stream2_IRQn interrupt configuration */ 51 | HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 7, 0); 52 | HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn); 53 | /* DMA1_Stream3_IRQn interrupt configuration */ 54 | HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 7, 0); 55 | HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn); 56 | /* DMA1_Stream5_IRQn interrupt configuration */ 57 | HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 7, 0); 58 | HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn); 59 | /* DMA1_Stream6_IRQn interrupt configuration */ 60 | HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 7, 0); 61 | HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn); 62 | /* DMA2_Stream0_IRQn interrupt configuration */ 63 | HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 7, 0); 64 | HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn); 65 | /* DMA2_Stream3_IRQn interrupt configuration */ 66 | HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 7, 0); 67 | HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn); 68 | /* DMA2_Stream4_IRQn interrupt configuration */ 69 | HAL_NVIC_SetPriority(DMA2_Stream4_IRQn, 7, 0); 70 | HAL_NVIC_EnableIRQ(DMA2_Stream4_IRQn); 71 | /* DMA2_Stream5_IRQn interrupt configuration */ 72 | HAL_NVIC_SetPriority(DMA2_Stream5_IRQn, 7, 0); 73 | HAL_NVIC_EnableIRQ(DMA2_Stream5_IRQn); 74 | /* DMA2_Stream6_IRQn interrupt configuration */ 75 | HAL_NVIC_SetPriority(DMA2_Stream6_IRQn, 7, 0); 76 | HAL_NVIC_EnableIRQ(DMA2_Stream6_IRQn); 77 | 78 | } 79 | 80 | /* USER CODE BEGIN 2 */ 81 | 82 | /* USER CODE END 2 */ 83 | 84 | -------------------------------------------------------------------------------- /Core/Src/i2c.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file i2c.c 5 | * @brief This file provides code for the configuration 6 | * of the I2C instances. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Includes ------------------------------------------------------------------*/ 21 | #include "i2c.h" 22 | 23 | /* USER CODE BEGIN 0 */ 24 | 25 | /* USER CODE END 0 */ 26 | 27 | I2C_HandleTypeDef hi2c2; 28 | 29 | /* I2C2 init function */ 30 | void MX_I2C2_Init(void) 31 | { 32 | 33 | /* USER CODE BEGIN I2C2_Init 0 */ 34 | 35 | /* USER CODE END I2C2_Init 0 */ 36 | 37 | /* USER CODE BEGIN I2C2_Init 1 */ 38 | 39 | /* USER CODE END I2C2_Init 1 */ 40 | hi2c2.Instance = I2C2; 41 | hi2c2.Init.ClockSpeed = 400000; 42 | hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2; 43 | hi2c2.Init.OwnAddress1 = 0; 44 | hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT; 45 | hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE; 46 | hi2c2.Init.OwnAddress2 = 0; 47 | hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE; 48 | hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE; 49 | if (HAL_I2C_Init(&hi2c2) != HAL_OK) 50 | { 51 | Error_Handler(); 52 | } 53 | 54 | /** Configure Analogue filter 55 | */ 56 | if (HAL_I2CEx_ConfigAnalogFilter(&hi2c2, I2C_ANALOGFILTER_ENABLE) != HAL_OK) 57 | { 58 | Error_Handler(); 59 | } 60 | 61 | /** Configure Digital filter 62 | */ 63 | if (HAL_I2CEx_ConfigDigitalFilter(&hi2c2, 0) != HAL_OK) 64 | { 65 | Error_Handler(); 66 | } 67 | /* USER CODE BEGIN I2C2_Init 2 */ 68 | 69 | /* USER CODE END I2C2_Init 2 */ 70 | 71 | } 72 | 73 | void HAL_I2C_MspInit(I2C_HandleTypeDef* i2cHandle) 74 | { 75 | 76 | GPIO_InitTypeDef GPIO_InitStruct = {0}; 77 | if(i2cHandle->Instance==I2C2) 78 | { 79 | /* USER CODE BEGIN I2C2_MspInit 0 */ 80 | 81 | /* USER CODE END I2C2_MspInit 0 */ 82 | 83 | __HAL_RCC_GPIOB_CLK_ENABLE(); 84 | /**I2C2 GPIO Configuration 85 | PB10 ------> I2C2_SCL 86 | PB11 ------> I2C2_SDA 87 | */ 88 | GPIO_InitStruct.Pin = I2C_SCL_Pin|I2C_SDA_Pin; 89 | GPIO_InitStruct.Mode = GPIO_MODE_AF_OD; 90 | GPIO_InitStruct.Pull = GPIO_PULLUP; 91 | GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW; 92 | GPIO_InitStruct.Alternate = GPIO_AF4_I2C2; 93 | HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); 94 | 95 | /* I2C2 clock enable */ 96 | __HAL_RCC_I2C2_CLK_ENABLE(); 97 | 98 | /* I2C2 interrupt Init */ 99 | HAL_NVIC_SetPriority(I2C2_EV_IRQn, 5, 0); 100 | HAL_NVIC_EnableIRQ(I2C2_EV_IRQn); 101 | /* USER CODE BEGIN I2C2_MspInit 1 */ 102 | 103 | /* USER CODE END I2C2_MspInit 1 */ 104 | } 105 | } 106 | 107 | void HAL_I2C_MspDeInit(I2C_HandleTypeDef* i2cHandle) 108 | { 109 | 110 | if(i2cHandle->Instance==I2C2) 111 | { 112 | /* USER CODE BEGIN I2C2_MspDeInit 0 */ 113 | 114 | /* USER CODE END I2C2_MspDeInit 0 */ 115 | /* Peripheral clock disable */ 116 | __HAL_RCC_I2C2_CLK_DISABLE(); 117 | 118 | /**I2C2 GPIO Configuration 119 | PB10 ------> I2C2_SCL 120 | PB11 ------> I2C2_SDA 121 | */ 122 | HAL_GPIO_DeInit(I2C_SCL_GPIO_Port, I2C_SCL_Pin); 123 | 124 | HAL_GPIO_DeInit(I2C_SDA_GPIO_Port, I2C_SDA_Pin); 125 | 126 | /* I2C2 interrupt Deinit */ 127 | HAL_NVIC_DisableIRQ(I2C2_EV_IRQn); 128 | /* USER CODE BEGIN I2C2_MspDeInit 1 */ 129 | 130 | /* USER CODE END I2C2_MspDeInit 1 */ 131 | } 132 | } 133 | 134 | /* USER CODE BEGIN 1 */ 135 | 136 | /* USER CODE END 1 */ 137 | -------------------------------------------------------------------------------- /Core/Src/stm32f4xx_hal_msp.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * File Name : stm32f4xx_hal_msp.c 5 | * Description : This file provides code for the MSP Initialization 6 | * and de-Initialization codes. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under BSD 3-Clause license, 14 | * the "License"; You may not use this file except in compliance with the 15 | * License. You may obtain a copy of the License at: 16 | * opensource.org/licenses/BSD-3-Clause 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | #include "main.h" 24 | 25 | /* USER CODE BEGIN Includes */ 26 | 27 | /* USER CODE END Includes */ 28 | 29 | /* Private typedef -----------------------------------------------------------*/ 30 | /* USER CODE BEGIN TD */ 31 | 32 | /* USER CODE END TD */ 33 | 34 | /* Private define ------------------------------------------------------------*/ 35 | /* USER CODE BEGIN Define */ 36 | 37 | /* USER CODE END Define */ 38 | 39 | /* Private macro -------------------------------------------------------------*/ 40 | /* USER CODE BEGIN Macro */ 41 | 42 | /* USER CODE END Macro */ 43 | 44 | /* Private variables ---------------------------------------------------------*/ 45 | /* USER CODE BEGIN PV */ 46 | 47 | /* USER CODE END PV */ 48 | 49 | /* Private function prototypes -----------------------------------------------*/ 50 | /* USER CODE BEGIN PFP */ 51 | 52 | /* USER CODE END PFP */ 53 | 54 | /* External functions --------------------------------------------------------*/ 55 | /* USER CODE BEGIN ExternalFunctions */ 56 | 57 | /* USER CODE END ExternalFunctions */ 58 | 59 | /* USER CODE BEGIN 0 */ 60 | 61 | /* USER CODE END 0 */ 62 | /** 63 | * Initializes the Global MSP. 64 | */ 65 | void HAL_MspInit(void) 66 | { 67 | /* USER CODE BEGIN MspInit 0 */ 68 | 69 | /* USER CODE END MspInit 0 */ 70 | 71 | __HAL_RCC_SYSCFG_CLK_ENABLE(); 72 | __HAL_RCC_PWR_CLK_ENABLE(); 73 | 74 | /* System interrupt init*/ 75 | /* PendSV_IRQn interrupt configuration */ 76 | HAL_NVIC_SetPriority(PendSV_IRQn, 15, 0); 77 | 78 | /* Peripheral interrupt init */ 79 | /* RCC_IRQn interrupt configuration */ 80 | HAL_NVIC_SetPriority(RCC_IRQn, 5, 0); 81 | HAL_NVIC_EnableIRQ(RCC_IRQn); 82 | 83 | /* USER CODE BEGIN MspInit 1 */ 84 | 85 | /* USER CODE END MspInit 1 */ 86 | } 87 | 88 | /* USER CODE BEGIN 1 */ 89 | 90 | /* USER CODE END 1 */ 91 | -------------------------------------------------------------------------------- /Core/Src/syscalls.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file syscalls.c 4 | * @author Auto-generated by STM32CubeIDE 5 | * @brief STM32CubeIDE Minimal System calls file 6 | * 7 | * For more information about which c-functions 8 | * need which of these lowlevel functions 9 | * please consult the Newlib libc-manual 10 | ****************************************************************************** 11 | * @attention 12 | * 13 | *

© Copyright (c) 2020 STMicroelectronics. 14 | * All rights reserved.

15 | * 16 | * This software component is licensed by ST under BSD 3-Clause license, 17 | * the "License"; You may not use this file except in compliance with the 18 | * License. You may obtain a copy of the License at: 19 | * opensource.org/licenses/BSD-3-Clause 20 | * 21 | ****************************************************************************** 22 | */ 23 | 24 | /* Includes */ 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | 35 | /* Variables */ 36 | //#undef errno 37 | extern int errno; 38 | extern int __io_putchar(int ch) __attribute__((weak)); 39 | extern int __io_getchar(void) __attribute__((weak)); 40 | 41 | register char * stack_ptr asm("sp"); 42 | 43 | char *__env[1] = { 0 }; 44 | char **environ = __env; 45 | 46 | 47 | /* Functions */ 48 | void initialise_monitor_handles() 49 | { 50 | } 51 | 52 | int _getpid(void) 53 | { 54 | return 1; 55 | } 56 | 57 | int _kill(int pid, int sig) 58 | { 59 | errno = EINVAL; 60 | return -1; 61 | } 62 | 63 | void _exit (int status) 64 | { 65 | _kill(status, -1); 66 | while (1) {} /* Make sure we hang here */ 67 | } 68 | 69 | __attribute__((weak)) int _read(int file, char *ptr, int len) 70 | { 71 | int DataIdx; 72 | 73 | for (DataIdx = 0; DataIdx < len; DataIdx++) 74 | { 75 | *ptr++ = __io_getchar(); 76 | } 77 | 78 | return len; 79 | } 80 | 81 | __attribute__((weak)) int _write(int file, char *ptr, int len) 82 | { 83 | int DataIdx; 84 | 85 | for (DataIdx = 0; DataIdx < len; DataIdx++) 86 | { 87 | __io_putchar(*ptr++); 88 | } 89 | return len; 90 | } 91 | 92 | int _close(int file) 93 | { 94 | return -1; 95 | } 96 | 97 | 98 | int _fstat(int file, struct stat *st) 99 | { 100 | st->st_mode = S_IFCHR; 101 | return 0; 102 | } 103 | 104 | int _isatty(int file) 105 | { 106 | return 1; 107 | } 108 | 109 | int _lseek(int file, int ptr, int dir) 110 | { 111 | return 0; 112 | } 113 | 114 | int _open(char *path, int flags, ...) 115 | { 116 | /* Pretend like we always fail */ 117 | return -1; 118 | } 119 | 120 | int _wait(int *status) 121 | { 122 | errno = ECHILD; 123 | return -1; 124 | } 125 | 126 | int _unlink(char *name) 127 | { 128 | errno = ENOENT; 129 | return -1; 130 | } 131 | 132 | int _times(struct tms *buf) 133 | { 134 | return -1; 135 | } 136 | 137 | int _stat(char *file, struct stat *st) 138 | { 139 | st->st_mode = S_IFCHR; 140 | return 0; 141 | } 142 | 143 | int _link(char *old, char *new) 144 | { 145 | errno = EMLINK; 146 | return -1; 147 | } 148 | 149 | int _fork(void) 150 | { 151 | errno = EAGAIN; 152 | return -1; 153 | } 154 | 155 | int _execve(char *name, char **argv, char **env) 156 | { 157 | errno = ENOMEM; 158 | return -1; 159 | } 160 | -------------------------------------------------------------------------------- /Cpplibrary/include/ahrs/ahrs.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ahrs.h 3 | * 4 | * Created on: 2021年7月9日 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_AHRS_AHRS_H_ 10 | #define INCLUDE_AHRS_AHRS_H_ 11 | 12 | #include "common.h" 13 | 14 | #define var_length 20 //the length of variance window 15 | 16 | class AHRS{ 17 | 18 | public: 19 | AHRS(float dt); 20 | void update(bool &get_mag, bool &get_mav_yaw); 21 | bool is_initialed(void){return initialed;} 22 | void set_declination(float rad){declination=rad;} 23 | float get_declination(void){return declination;} 24 | void reset(void){ 25 | initialed=false; 26 | } 27 | //AHRS输出quaternion2 28 | Quaternion quaternion2=Quaternion(); 29 | void check_vibration(void); 30 | float vib_value=0, vib_angle=0; 31 | private: 32 | float T=0.0025; 33 | 34 | bool initialed=false; 35 | 36 | Quaternion quaternion=Quaternion(); 37 | 38 | float q0=1.0f, q1=0.0f, q2=0.0f, q3=0.0f; 39 | float wx=0.0f, wy=0.0f, wz=0.0f; 40 | Vector3f accel; 41 | float accel_length=0.0f; 42 | float accel_error_gain=1.0f; 43 | float ax=0.0f, ay=0.0f, az=0.0f; 44 | float Ak[4*4]={0}; 45 | float AkT[4*4]={0}; 46 | float temp33[3*3]={0}, temp33inv[3*3]={0}; 47 | float q_factor1[4]={0}; 48 | float Hk1[3*4]={0}; 49 | float Hk1T[4*3]={0}; 50 | float zk1_hk1[3*1]={0}; 51 | float mx=1.0f, my=0.0f, mz=0.0f; 52 | float q_factor2[4]={0}; 53 | float Hk2[3*4]={0}; 54 | float Hk2T[4*3]={0}; 55 | float zk2_hk2[3*1]={0}; 56 | 57 | float P_posteriori[4*4]={ 0.125, 0.0003, 0.0003, 0.0003, 58 | 0.0003, 0.125, 0.0003, 0.0003, 59 | 0.0003, 0.0003, 0.125, 0.0003, 60 | 0.0003, 0.0003, 0.0003, 0.125 }; 61 | float e6 = 1e6, P_priori[4*4], *tempP1, *tempP2, *KGain; 62 | float Q_PredNoise[4*4] = { e6, 0, 0, 0, 63 | 0, e6, 0, 0, 64 | 0, 0, e6, 0, 65 | 0, 0, 0, e6}; 66 | float q_priori[4]={0}, q_priori_length=1.0f; 67 | float Rk1[3*3] = { 0.8, 0, 0, 68 | 0, 0.8, 0, 69 | 0, 0, 0.8}; 70 | float Rk2[3*3] = { 0.4, 0, 0, 71 | 0, 0.4, 0, 72 | 0, 0, 0.4}; 73 | 74 | float ekf_gain; 75 | float declination=0.0f; 76 | Vector2f mag_ef_2d; 77 | Vector3f mag_bf; 78 | 79 | Vector3f accel_average, accel_variance; 80 | Vector3f accel_array[var_length]; 81 | }; 82 | 83 | #endif /* INCLUDE_AHRS_AHRS_H_ */ 84 | -------------------------------------------------------------------------------- /Cpplibrary/include/compass/declination.h: -------------------------------------------------------------------------------- 1 | /* 2 | * declination.h 3 | * 4 | * Created on: 2022年10月17日 5 | * Author: 25053 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_COMPASS_DECLINATION_H_ 10 | #define INCLUDE_COMPASS_DECLINATION_H_ 11 | 12 | #include "common.h" 13 | 14 | class Declination 15 | { 16 | public: 17 | /* 18 | * Calculates the magnetic intensity, declination and inclination at a given WGS-84 latitude and longitude. 19 | * Assumes a WGS-84 height of zero 20 | * latitude and longitude have units of degrees 21 | * declination and inclination are returned in degrees 22 | * intensity is returned in Gauss 23 | * Boolean returns false if latitude and longitude are outside the valid input range of +-60 latitude and +-180 longitude 24 | */ 25 | static bool get_mag_field_ef(float latitude_deg, float longitude_deg, float &intensity_gauss, float &declination_deg, float &inclination_deg); 26 | 27 | /* 28 | get declination in degrees for a given latitude_deg and longitude_deg 29 | */ 30 | static float get_declination(float latitude_deg, float longitude_deg); 31 | 32 | private: 33 | static const float SAMPLING_RES; 34 | static const float SAMPLING_MIN_LAT; 35 | static const float SAMPLING_MAX_LAT; 36 | static const float SAMPLING_MIN_LON; 37 | static const float SAMPLING_MAX_LON; 38 | 39 | static const float declination_table[19][37]; 40 | static const float inclination_table[19][37]; 41 | static const float intensity_table[19][37]; 42 | }; 43 | 44 | 45 | #endif /* INCLUDE_COMPASS_DECLINATION_H_ */ 46 | -------------------------------------------------------------------------------- /Cpplibrary/include/ekf/ekf_baro.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ekf_baro.h 3 | * 4 | * Created on: 2021年7月9日 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_EKF_EKF_BARO_H_ 10 | #define INCLUDE_EKF_EKF_BARO_H_ 11 | 12 | #include "common.h" 13 | 14 | class EKF_Baro{ 15 | 16 | public: 17 | EKF_Baro(float dt, float Q1, float Q2, float R1, float R2); 18 | void update(bool &get_baro_alt_filt, float baro_alt); 19 | void fusion(float baro_alt, float &baro_alt_correct); 20 | float pos_z=0, vel_z=0, vel_2d=0, accel_2d=0, accelz_ef=0; 21 | float get_vt(void){return vt_last;} 22 | bool is_initialed(void){return initialed;} 23 | void reset(void){ 24 | initialed=false; 25 | } 26 | 27 | private: 28 | float Qt[2*2]={0.0016, 0, 29 | 0, 0.0016};//观测数据的方差 30 | float _filt_alpha(float dt, float filt_hz); 31 | float _alpha=0; 32 | bool initialed=false; 33 | float delta_x=0, delta_v=0; 34 | float T_baro=0.0025; //2.5ms 35 | float G[2*2]={ 1, T_baro, 36 | 0, 1}; 37 | float GT[2*2]={1, 0, 38 | T_baro, 1}; 39 | float h_x=0, h_v=0, zt=0, zt_last=0, vt=0, vt_last=0, t_now=0, t_last=0; 40 | float error_x, error_v; 41 | float error_xv[2*2]={1,0, 42 | 0,1}; 43 | float inv[4]; 44 | float H[2*2]={1,0, 45 | 0,1}; 46 | float HT[2*2]={1,0, 47 | 0,1}; 48 | float Rt[2*2]={ 0.000016, 0, //预测数据x方差 49 | 0, 0.000016}; //预测数据v方差 50 | float error[2*2]={ 1.0, 0, 51 | 0, 1.0}; 52 | float error_p[2*2]; 53 | float* error1; 54 | float* error2; 55 | float* Kal; 56 | DerivativeFilterFloat_Size7 _climb_rate_filter; 57 | float alt_last=0; 58 | float baro_alt_last=0,gnss_alt_last=0; 59 | float rf_alt_delta=0, rf_alt_last=0, gnss_alt_delta=0, baro_alt_delta=0, baro_alt_offset=0; 60 | float K_gain=0.0f; 61 | bool rf_correct=false; 62 | uint8_t rf_correct_flag=0; 63 | }; 64 | #endif /* INCLUDE_EKF_EKF_BARO_H_ */ 65 | -------------------------------------------------------------------------------- /Cpplibrary/include/ekf/ekf_gnss.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ekf_gnss.h 3 | * 4 | * Created on: 2021年8月20日 5 | * Author: 25053 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_EKF_EKF_GNSS_H_ 10 | #define INCLUDE_EKF_EKF_GNSS_H_ 11 | 12 | #include "common.h" 13 | 14 | class EKF_GNSS{ 15 | public: 16 | EKF_GNSS(float dt, float Q1, float Q2, float Q3, float Q4, float R1, float R2, float R3, float R4); 17 | void update(bool &get_gnss, float gnss_pos_x, float gnss_pos_y, float gnss_vel_x, float gnss_vel_y); 18 | bool is_initialed(void){return initialed;} 19 | void reset(void){ 20 | initialed=false; 21 | } 22 | float pos_x=0, pos_y=0, vel_x=0, vel_y=0; 23 | 24 | private: 25 | float _filt_alpha(float dt, float filt_hz); 26 | float _alpha=0; 27 | float Qt[4]={1.0f,1.0f,1.0f,1.0f};//观测数据的方差 28 | float T_odom=0.0025; //2.5ms 29 | bool initialed=false; 30 | /*****************x-axis******************/ 31 | float delta_x_odomx=0, delta_v_velx=0; 32 | float G_odomx[2*2]={ 1, T_odom, 33 | 0, 1}; 34 | float GT_odomx[2*2]={1, 0, 35 | T_odom, 1}; 36 | float h_odomx=0 ,error_x_odomx=1, zt_odomx=0; 37 | float h_velx=0 ,error_v_velx=1, zt_velx=0; 38 | float H_odomx[2*2]={1,0, 39 | 0,1}; 40 | float HT_odomx[2*2]={1,0, 41 | 0,1}; 42 | float Rt_odomx[2*2]={ 0.0000001, 0, //预测数据x方差 43 | 0, 0.001}; //预测数据v方差 44 | float error_xv_x[2*2]={1,0, 45 | 0,1}; 46 | float error_odomx[2*2]={ 1.0, 0, 47 | 0, 1.0}; 48 | float error_p_odomx[2*2]; 49 | float* error1_odomx; 50 | float* error2_odomx; 51 | float* Kal_odomx; 52 | float inverse_x[4]={}; 53 | /*****************y-axis******************/ 54 | float delta_x_odomy=0, delta_v_vely=0; 55 | float G_odomy[2*2]={ 1, T_odom, 56 | 0, 1}; 57 | float GT_odomy[2*2]={1, 0, 58 | T_odom, 1}; 59 | float h_odomy=0 ,error_x_odomy=1, zt_odomy=0; 60 | float h_vely=0 ,error_v_vely=1, zt_vely=0; 61 | float H_odomy[2*2]={1,0, 62 | 0,1}; 63 | float HT_odomy[2*2]={1,0, 64 | 0,1}; 65 | float Rt_odomy[2*2]={ 0.0000001, 0, //预测数据x方差 66 | 0, 0.001}; //预测数据v方差 67 | float error_xv_y[2*2]={1,0, 68 | 0,1}; 69 | float error_odomy[2*2]={ 1.0, 0, 70 | 0, 1.0}; 71 | float error_p_odomy[2*2]; 72 | float* error1_odomy; 73 | float* error2_odomy; 74 | float* Kal_odomy; 75 | float inverse_y[4]={}; 76 | float accel_filt_hz=10;//Hz 震动对于速度预测影响非常大 所以要把截止频率设低一些 77 | float accelx_filt=0; 78 | float accely_filt=0; 79 | }; 80 | 81 | #endif /* INCLUDE_EKF_EKF_GNSS_H_ */ 82 | -------------------------------------------------------------------------------- /Cpplibrary/include/ekf/ekf_odometry.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ekf_odom.h 3 | * 4 | * Created on: 2021年7月9日 5 | * Author: 25053 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_EKF_EKF_ODOMETRY_H_ 10 | #define INCLUDE_EKF_EKF_ODOMETRY_H_ 11 | 12 | #include "common.h" 13 | 14 | class EKF_Odometry{ 15 | public: 16 | EKF_Odometry(float dt, float Q1, float Q2, float R1, float R2, float R3, float R4); 17 | void update(bool &get_odometry, float odom_pos_x, float odom_pos_y); 18 | bool is_initialed(void){return initialed;} 19 | void reset(void){ 20 | initialed=false; 21 | } 22 | float pos_x=0, pos_y=0, vel_x=0, vel_y=0; 23 | 24 | private: 25 | float _filt_alpha(float dt, float filt_hz); 26 | float _alpha=0; 27 | float Qt[2]={1.0f,1.0f}; //观测数据的方差 28 | float T_odom=0.0025; //2.5ms 29 | bool initialed=false; 30 | /*****************x-axis******************/ 31 | float delta_x_odomx=0; 32 | float G_odomx[2*2]={ 1, T_odom, 33 | 0, 1}; 34 | float GT_odomx[2*2]={1, 0, 35 | T_odom, 1}; 36 | float h_odomx=0 ,error_x_odomx=0, zt_odomx=0; 37 | float H_odomx[1*2]={1,0}; 38 | float HT_odomx[2*1]={1,0}; 39 | float Rt_odomx[2*2]={ 0.0025, 0, //预测数据x方差 40 | 0, 2.5}; //预测数据v方差 41 | float error_odomx[2*2]={ 1.0, 0, 42 | 0, 1.0}; 43 | float error_p_odomx[2*2]; 44 | float* error1_odomx; 45 | float* error2_odomx; 46 | float* Kal_odomx; 47 | 48 | /*****************y-axis******************/ 49 | float delta_x_odomy=0; 50 | float G_odomy[2*2]={ 1, T_odom, 51 | 0, 1}; 52 | float GT_odomy[2*2]={1, 0, 53 | T_odom, 1}; 54 | float h_odomy=0 ,error_x_odomy=0, zt_odomy=0; 55 | float H_odomy[1*2]={1,0}; 56 | float HT_odomy[2*1]={1,0}; 57 | float Rt_odomy[2*2]={ 0.0025, 0, //预测数据x方差 58 | 0, 2.5}; //预测数据v方差 59 | float error_odomy[2*2]={ 1.0, 0, 60 | 0, 1.0}; 61 | float error_p_odomy[2*2]; 62 | float* error1_odomy; 63 | float* error2_odomy; 64 | float* Kal_odomy; 65 | float accel_filt_hz=10;//Hz 震动对于速度预测影响非常大 所以要把截止频率设低一些 66 | float accelx_filt=0; 67 | float accely_filt=0; 68 | }; 69 | 70 | #endif /* INCLUDE_EKF_EKF_ODOMETRY_H_ */ 71 | -------------------------------------------------------------------------------- /Cpplibrary/include/ekf/ekf_rangefinder.h: -------------------------------------------------------------------------------- 1 | /* 2 | * ekf_rangfinder.h 3 | * 4 | * Created on: 2021年7月9日 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef INCLUDE_EKF_EKF_RANGEFINDER_H_ 10 | #define INCLUDE_EKF_EKF_RANGEFINDER_H_ 11 | 12 | #include "common.h" 13 | 14 | class EKF_Rangefinder{ 15 | 16 | public: 17 | EKF_Rangefinder(float dt, float Q, float R1, float R2); 18 | void update(bool &get_rangefinder_data, float rangefinder_alt); 19 | bool is_initialed(void){return initialed;} 20 | void reset(void){ 21 | initialed=false; 22 | } 23 | float pos_z=0, vel_z=0; 24 | 25 | private: 26 | float Qt=1.0f; //观测数据的方差 27 | bool initialed=false; 28 | float _filt_alpha(float dt, float filt_hz); 29 | float _alpha=0; 30 | float delta_x_rf=0; 31 | float T_rangefinder=0.0025; //2.5ms 32 | float G_rf[2*2]={ 1, T_rangefinder, 33 | 0, 1}; 34 | float GT_rf[2*2]={1, 0, 35 | T_rangefinder, 1}; 36 | float h_rf=0 ,error_x_rf=0, zt_rf=0; 37 | float H_rf[1*2]={1,0}; 38 | float HT_rf[2*1]={1,0}; 39 | float Rt_rf[2*2]={ 0.000016, 0, //预测数据x方差 40 | 0, 0.16}; //预测数据v方差 41 | float error_rf[2*2]={ 1.0, 0, 42 | 0, 1.0}; 43 | float error_p_rf[2*2]; 44 | float* error1_rf; 45 | float* error2_rf; 46 | float* Kal_rf; 47 | float accelz_filt_hz_rf=30;//Hz 48 | float accelz_filt_rf=0; 49 | }; 50 | 51 | #endif /* INCLUDE_EKF_EKF_RANGEFINDER_H_ */ 52 | -------------------------------------------------------------------------------- /Cpplibrary/include/filter/DerivativeFilter.h: -------------------------------------------------------------------------------- 1 | /* 2 | * DerivativeFilter.h 3 | * 4 | * Created on: 2022年7月21日 5 | * Author: 25053 6 | */ 7 | 8 | #ifndef INCLUDE_FILTER_DERIVATIVEFILTER_H_ 9 | #define INCLUDE_FILTER_DERIVATIVEFILTER_H_ 10 | 11 | #pragma once 12 | 13 | #include "FilterClass.h" 14 | #include "FilterWithBuffer.h" 15 | 16 | // 1st parameter is the type of data being filtered. 17 | // 2nd parameter is the number of elements in the filter 18 | template 19 | class DerivativeFilter : public FilterWithBuffer 20 | { 21 | public: 22 | // constructor 23 | DerivativeFilter() : FilterWithBuffer() { 24 | }; 25 | 26 | // update - Add a new raw value to the filter, but don't recalculate 27 | void update(T sample, uint32_t timestamp); 28 | 29 | // return the derivative value 30 | float slope(void); 31 | 32 | // reset - clear the filter 33 | virtual void reset(); 34 | 35 | private: 36 | bool _new_data; 37 | float _last_slope; 38 | 39 | // microsecond timestamps for samples. This is needed 40 | // to cope with non-uniform time spacing of the data 41 | uint32_t _timestamps[FILTER_SIZE]; 42 | }; 43 | 44 | typedef DerivativeFilter DerivativeFilterFloat_Size5; 45 | typedef DerivativeFilter DerivativeFilterFloat_Size7; 46 | typedef DerivativeFilter DerivativeFilterFloat_Size9; 47 | 48 | 49 | #endif /* INCLUDE_FILTER_DERIVATIVEFILTER_H_ */ 50 | -------------------------------------------------------------------------------- /Cpplibrary/include/filter/FilterClass.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FilterClass.h 3 | * 4 | * Created on: 2022年7月21日 5 | * Author: 25053 6 | */ 7 | 8 | #ifndef INCLUDE_FILTER_FILTERCLASS_H_ 9 | #define INCLUDE_FILTER_FILTERCLASS_H_ 10 | 11 | /// @file FilterClass.h 12 | /// @brief A pure virtual interface class 13 | /// 14 | #pragma once 15 | 16 | #include 17 | 18 | template 19 | class Filter 20 | { 21 | public: 22 | // apply - Add a new raw value to the filter, retrieve the filtered result 23 | virtual T apply(T sample) = 0; 24 | 25 | // reset - clear the filter 26 | virtual void reset() = 0; 27 | 28 | }; 29 | 30 | // Typedef for convenience 31 | typedef Filter FilterInt8; 32 | typedef Filter FilterUInt8; 33 | typedef Filter FilterInt16; 34 | typedef Filter FilterUInt16; 35 | typedef Filter FilterInt32; 36 | typedef Filter FilterUInt32; 37 | 38 | #endif /* INCLUDE_FILTER_FILTERCLASS_H_ */ 39 | -------------------------------------------------------------------------------- /Cpplibrary/include/filter/FilterWithBuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * FilterWithBuffer.h 3 | * 4 | * Created on: 2022年7月21日 5 | * Author: 25053 6 | */ 7 | 8 | #ifndef INCLUDE_FILTER_FILTERWITHBUFFER_H_ 9 | #define INCLUDE_FILTER_FILTERWITHBUFFER_H_ 10 | 11 | /// @file FilterWithBuffer.h 12 | /// @brief A filter with a buffer. 13 | /// This is implemented separately to the base Filter class to get around 14 | /// restrictions caused by the use of templates which makes different sizes essentially 15 | /// completely different classes 16 | #pragma once 17 | 18 | #include "FilterClass.h" 19 | 20 | template 21 | class FilterWithBuffer : public Filter 22 | { 23 | public: 24 | // constructor 25 | FilterWithBuffer(); 26 | 27 | // apply - Add a new raw value to the filter, retrieve the filtered result 28 | virtual T apply(T sample); 29 | 30 | // reset - clear the filter 31 | virtual void reset(); 32 | 33 | // get filter size 34 | uint8_t get_filter_size() const { 35 | return FILTER_SIZE; 36 | }; 37 | 38 | T get_sample(uint8_t i) const { 39 | return samples[i]; 40 | } 41 | 42 | protected: 43 | T samples[FILTER_SIZE]; // buffer of samples 44 | uint8_t sample_index; // pointer to the next empty slot in the buffer 45 | }; 46 | 47 | // Typedef for convenience 48 | typedef FilterWithBuffer FilterWithBufferInt16_Size2; 49 | typedef FilterWithBuffer FilterWithBufferInt16_Size3; 50 | typedef FilterWithBuffer FilterWithBufferInt16_Size4; 51 | typedef FilterWithBuffer FilterWithBufferInt16_Size5; 52 | typedef FilterWithBuffer FilterWithBufferInt16_Size6; 53 | typedef FilterWithBuffer FilterWithBufferInt16_Size7; 54 | typedef FilterWithBuffer FilterWithBufferUInt16_Size2; 55 | typedef FilterWithBuffer FilterWithBufferUInt16_Size3; 56 | typedef FilterWithBuffer FilterWithBufferUInt16_Size4; 57 | typedef FilterWithBuffer FilterWithBufferUInt16_Size5; 58 | typedef FilterWithBuffer FilterWithBufferUInt16_Size6; 59 | typedef FilterWithBuffer FilterWithBufferUInt16_Size7; 60 | 61 | typedef FilterWithBuffer FilterWithBufferInt32_Size2; 62 | typedef FilterWithBuffer FilterWithBufferInt32_Size3; 63 | typedef FilterWithBuffer FilterWithBufferInt32_Size4; 64 | typedef FilterWithBuffer FilterWithBufferInt32_Size5; 65 | typedef FilterWithBuffer FilterWithBufferInt32_Size6; 66 | typedef FilterWithBuffer FilterWithBufferInt32_Size7; 67 | typedef FilterWithBuffer FilterWithBufferUInt32_Size2; 68 | typedef FilterWithBuffer FilterWithBufferUInt32_Size3; 69 | typedef FilterWithBuffer FilterWithBufferUInt32_Size4; 70 | typedef FilterWithBuffer FilterWithBufferUInt32_Size5; 71 | typedef FilterWithBuffer FilterWithBufferUInt32_Size6; 72 | typedef FilterWithBuffer FilterWithBufferUInt32_Size7; 73 | 74 | // Constructor 75 | template 76 | FilterWithBuffer::FilterWithBuffer() : 77 | sample_index(0) 78 | { 79 | // clear sample buffer 80 | reset(); 81 | } 82 | 83 | // reset - clear all samples from the buffer 84 | template 85 | void FilterWithBuffer::reset() 86 | { 87 | // clear samples buffer 88 | for( int8_t i=0; i 98 | T FilterWithBuffer:: apply(T sample) 99 | { 100 | // add sample to array 101 | samples[sample_index++] = sample; 102 | 103 | // wrap index if necessary 104 | if( sample_index >= FILTER_SIZE ) 105 | sample_index = 0; 106 | 107 | // base class doesn't know what filtering to do so we just return the raw sample 108 | return sample; 109 | } 110 | 111 | 112 | #endif /* INCLUDE_FILTER_FILTERWITHBUFFER_H_ */ 113 | -------------------------------------------------------------------------------- /Cpplibrary/include/filter/LowPassFilter.h: -------------------------------------------------------------------------------- 1 | // 2 | /// @file LowPassFilter.h 3 | /// @brief A class to implement a low pass filter without losing precision even for int types 4 | /// the downside being that it's a little slower as it internally uses a float 5 | /// and it consumes an extra 4 bytes of memory to hold the constant gain 6 | 7 | /* 8 | * Created on: 2020.06.19 9 | * Author: JackyPan 10 | */ 11 | 12 | /* 13 | Note that this filter can be used in 2 ways: 14 | 15 | 1) providing dt on every sample, and calling apply like this: 16 | 17 | // call once 18 | filter.set_cutoff_frequency(frequency_hz); 19 | 20 | // then on each sample 21 | output = filter.apply(sample, dt); 22 | 23 | 2) providing a sample freq and cutoff_freq once at start 24 | 25 | // call once 26 | filter.set_cutoff_frequency(sample_freq, frequency_hz); 27 | 28 | // then on each sample 29 | output = filter.apply(sample); 30 | 31 | The second approach is more CPU efficient as it doesn't have to 32 | recalculate alpha each time, but it assumes that dt is constant 33 | */ 34 | 35 | #pragma once 36 | 37 | #include "hal.h" 38 | #include "math/math_inc.h" 39 | 40 | // DigitalLPF implements the filter math 41 | template 42 | class DigitalLPF { 43 | public: 44 | DigitalLPF(); 45 | // add a new raw value to the filter, retrieve the filtered result 46 | T apply(const T &sample, float cutoff_freq, float dt); 47 | T apply(const T &sample); 48 | 49 | void compute_alpha(float sample_freq, float cutoff_freq); 50 | 51 | // get latest filtered value from filter (equal to the value returned by latest call to apply method) 52 | const T &get() const; 53 | void reset(T value); 54 | 55 | private: 56 | T _output; 57 | float alpha = 1.0f; 58 | }; 59 | 60 | // LPF base class 61 | template 62 | class LowPassFilter { 63 | public: 64 | LowPassFilter(); 65 | LowPassFilter(float cutoff_freq); 66 | LowPassFilter(float sample_freq, float cutoff_freq); 67 | 68 | // change parameters 69 | void set_cutoff_frequency(float cutoff_freq); 70 | void set_cutoff_frequency(float sample_freq, float cutoff_freq); 71 | 72 | // return the cutoff frequency 73 | float get_cutoff_freq(void) const; 74 | T apply(T sample, float dt); 75 | T apply(T sample); 76 | const T &get() const; 77 | void reset(T value); 78 | void reset(void) { reset(T()); } 79 | 80 | protected: 81 | float _cutoff_freq; 82 | 83 | private: 84 | DigitalLPF _filter; 85 | }; 86 | 87 | // typedefs for compatibility 88 | typedef LowPassFilter LowPassFilterInt; 89 | typedef LowPassFilter LowPassFilterLong; 90 | typedef LowPassFilter LowPassFilterFloat; 91 | typedef LowPassFilter LowPassFilterVector2f; 92 | typedef LowPassFilter LowPassFilterVector3f; 93 | -------------------------------------------------------------------------------- /Cpplibrary/include/filter/LowPassFilter2p.h: -------------------------------------------------------------------------------- 1 | /* 2 | * LowPassFilter2p.h 3 | * 4 | * Created on: 2021.04.21 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | /* Define to prevent recursive inclusion -------------------------------------*/ 9 | #ifndef __LowPassFilter2p_H 10 | #define __LowPassFilter2p_H 11 | 12 | #include "hal.h" 13 | #include "math/math_inc.h" 14 | 15 | /// @file LowPassFilter2p.h 16 | /// @brief A class to implement a second order low pass filter 17 | template 18 | class DigitalBiquadFilter { 19 | public: 20 | struct biquad_params { 21 | float cutoff_freq; 22 | float sample_freq; 23 | float a1; 24 | float a2; 25 | float b0; 26 | float b1; 27 | float b2; 28 | }; 29 | 30 | DigitalBiquadFilter(); 31 | 32 | T apply(const T &sample, const struct biquad_params ¶ms); 33 | void reset(); 34 | static void compute_params(float sample_freq, float cutoff_freq, biquad_params &ret); 35 | 36 | private: 37 | T _delay_element_1; 38 | T _delay_element_2; 39 | }; 40 | 41 | template 42 | class LowPassFilter2p { 43 | public: 44 | LowPassFilter2p(); 45 | // constructor 46 | LowPassFilter2p(float sample_freq, float cutoff_freq); 47 | // change parameters 48 | void set_cutoff_frequency(float sample_freq, float cutoff_freq); 49 | // return the cutoff frequency 50 | float get_cutoff_freq(void) const; 51 | float get_sample_freq(void) const; 52 | T apply(const T &sample); 53 | void reset(void); 54 | 55 | protected: 56 | struct DigitalBiquadFilter::biquad_params _params; 57 | 58 | private: 59 | DigitalBiquadFilter _filter; 60 | }; 61 | 62 | typedef LowPassFilter2p LowPassFilter2pInt; 63 | typedef LowPassFilter2p LowPassFilter2pLong; 64 | typedef LowPassFilter2p LowPassFilter2pFloat; 65 | typedef LowPassFilter2p LowPassFilter2pVector2f; 66 | typedef LowPassFilter2p LowPassFilter2pVector3f; 67 | 68 | #endif /* __LowPassFilter2p_H */ 69 | -------------------------------------------------------------------------------- /Cpplibrary/include/flash/flash.h: -------------------------------------------------------------------------------- 1 | /* 2 | * flash.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef __FLASH_H 10 | #define __FLASH_H 11 | 12 | #include "common.h" 13 | 14 | class DataFlash{ 15 | public: 16 | DataFlash(){} 17 | void reset_addr_num_max(void); 18 | uint16_t get_addr_num_max(void); 19 | void set_param_uint8(uint16_t num, uint8_t value); 20 | void set_param_float(uint16_t num, float value); 21 | void set_param_vector3f(uint16_t num, Vector3f value); 22 | void set_param_pid(uint16_t num, float p, float i, float d, float i_max, float filt_hz); 23 | void set_param_pid_2d(uint16_t num, float p, float i, float d, float i_max, float filt_hz, float filt_d_hz); 24 | void set_param_uint16_channel8(uint16_t num, uint16_t *channel); 25 | void get_param_uint8(uint16_t num, uint8_t &value); 26 | void get_param_float(uint16_t num, float &value); 27 | void get_param_vector3f(uint16_t num, Vector3f &value); 28 | void get_param_pid(uint16_t num, float &p, float &i, float &d, float &i_max, float &filt_hz); 29 | void get_param_pid_2d(uint16_t num, float &p, float &i, float &d, float &i_max, float &filt_hz, float &filt_d_hz); 30 | void get_param_uint16_channel8(uint16_t num, uint16_t *channel); 31 | private: 32 | uint16_t addr_num_max=0; 33 | uint8_t addr[2]; 34 | uint8_t buf[DATA_FLASH_LENGTH]; 35 | void update_addr_num_max(uint16_t addr_num); 36 | void write_data(uint32_t addr, uint8_t *data, uint8_t length); 37 | void read_data(uint32_t addr, uint8_t *data, uint8_t length); 38 | }; 39 | 40 | #endif 41 | -------------------------------------------------------------------------------- /Cpplibrary/include/math/location.h: -------------------------------------------------------------------------------- 1 | /* 2 | * location.h 3 | * 4 | * Created on: 2021.02.01 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #include "math/math_inc.h" 10 | 11 | /* Define to prevent recursive inclusion -------------------------------------*/ 12 | #ifndef __LOCATION_H 13 | #define __LOCATION_H 14 | 15 | // scaling factor from 1e-7 degrees to meters at equator 16 | // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH 17 | #define LOCATION_SCALING_FACTOR 0.011131884502145034f 18 | // inverse of LOCATION_SCALING_FACTOR 19 | #define LOCATION_SCALING_FACTOR_INV 89.83204953368922f 20 | 21 | /* 22 | * LOCATION 23 | */ 24 | // longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator 25 | // Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters 26 | float longitude_scale(const struct Location &loc); 27 | 28 | // return distance in meters between two locations 29 | float get_distance(const struct Location &loc1, const struct Location &loc2); 30 | 31 | // return distance in centimeters between two locations 32 | uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2); 33 | 34 | // return horizontal distance in centimeters between two positions 35 | float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination); 36 | 37 | // return bearing in centi-degrees between two locations 38 | int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2); 39 | 40 | // return bearing in centi-degrees between two positions 41 | float get_bearing_cd(const Vector3f &origin, const Vector3f &destination); 42 | 43 | // see if location is past a line perpendicular to 44 | // the line between point1 and point2. If point1 is 45 | // our previous waypoint and point2 is our target waypoint 46 | // then this function returns true if we have flown past 47 | // the target waypoint 48 | bool location_passed_point(const struct Location & location, 49 | const struct Location & point1, 50 | const struct Location & point2); 51 | 52 | /* 53 | return the proportion we are along the path from point1 to 54 | point2. This will be less than >1 if we have passed point2 55 | */ 56 | float location_path_proportion(const struct Location &location, 57 | const struct Location &point1, 58 | const struct Location &point2); 59 | 60 | // extrapolate latitude/longitude given bearing and distance 61 | void location_update(struct Location &loc, float bearing, float distance); 62 | 63 | // extrapolate latitude/longitude given distances north and east 64 | void location_offset(struct Location &loc, float ofs_north, float ofs_east); 65 | 66 | /* 67 | return the distance in meters in North/East plane as a N/E vector 68 | from loc1 to loc2 69 | */ 70 | Vector2f location_diff(const struct Location &loc1, const struct Location &loc2); 71 | 72 | /* 73 | return the distance in meters in North/East/Down plane as a N/E/D vector 74 | from loc1 to loc2 75 | */ 76 | Vector3f location_3d_diff_NED(const struct Location &loc1, const struct Location &loc2); 77 | 78 | /* 79 | * check if lat and lng match. Ignore altitude and options 80 | */ 81 | bool locations_are_same(const struct Location &loc1, const struct Location &loc2); 82 | 83 | /* 84 | * convert invalid waypoint with useful data. return true if location changed 85 | */ 86 | bool location_sanitize(const struct Location &defaultLoc, struct Location &loc); 87 | 88 | // Converts from WGS84 geodetic coordinates (lat, lon, height) 89 | // into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates 90 | // (X, Y, Z) 91 | void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef); 92 | 93 | // Converts from WGS84 Earth Centered, Earth Fixed (ECEF) 94 | // coordinates (X, Y, Z), into WHS84 geodetic 95 | // coordinates (lat, lon, height) 96 | void wgsecef2llh(const Vector3d &ecef, Vector3d &llh); 97 | 98 | // return true when lat and lng are within range 99 | bool check_lat(float lat); 100 | bool check_lng(float lng); 101 | bool check_lat(int32_t lat); 102 | bool check_lng(int32_t lng); 103 | bool check_latlng(float lat, float lng); 104 | bool check_latlng(int32_t lat, int32_t lng); 105 | bool check_latlng(Location loc); 106 | 107 | #endif /* __LOCATION_H */ 108 | -------------------------------------------------------------------------------- /Cpplibrary/include/math/matrixN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * matrixN.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | 8 | /* 9 | * N dimensional matrix operations 10 | */ 11 | 12 | #pragma once 13 | 14 | #include "math/math_inc.h" 15 | 16 | #ifdef __cplusplus 17 | template 18 | class VectorN; 19 | 20 | 21 | template 22 | class MatrixN { 23 | 24 | friend class VectorN; 25 | 26 | public: 27 | // constructor from zeros 28 | MatrixN(void) { 29 | memset(v, 0, sizeof(v)); 30 | } 31 | 32 | // constructor from 4 diagonals 33 | MatrixN(const float d[N]) { 34 | memset(v, 0, sizeof(v)); 35 | for (uint8_t i = 0; i < N; i++) { 36 | v[i][i] = d[i]; 37 | } 38 | } 39 | 40 | // multiply two vectors to give a matrix, in-place 41 | void mult(const VectorN &A, const VectorN &B); 42 | 43 | // subtract B from the matrix 44 | MatrixN &operator -=(const MatrixN &B); 45 | 46 | // add B to the matrix 47 | MatrixN &operator +=(const MatrixN &B); 48 | 49 | // Matrix symmetry routine 50 | void force_symmetry(void); 51 | 52 | private: 53 | T v[N][N]; 54 | }; 55 | 56 | #endif/* __cplusplus */ 57 | -------------------------------------------------------------------------------- /Cpplibrary/include/math/quaternion.h: -------------------------------------------------------------------------------- 1 | /* 2 | * quaternion.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef __QUATERNION_H 10 | #define __QUATERNION_H 11 | 12 | #include "math/math_inc.h" 13 | 14 | #ifdef __cplusplus 15 | class Quaternion { 16 | public: 17 | float q1, q2, q3, q4; 18 | 19 | // constructor creates a quaternion equivalent 20 | // to roll=0, pitch=0, yaw=0 21 | Quaternion() 22 | { 23 | q1 = 1; 24 | q2 = q3 = q4 = 0; 25 | } 26 | 27 | // setting constructor 28 | Quaternion(const float _q1, const float _q2, const float _q3, const float _q4) : 29 | q1(_q1), q2(_q2), q3(_q3), q4(_q4) 30 | { 31 | } 32 | 33 | // setting constructor 34 | Quaternion(const float _q[4]) : 35 | q1(_q[0]), q2(_q[1]), q3(_q[2]), q4(_q[3]) 36 | { 37 | } 38 | 39 | // function call operator 40 | void operator()(const float _q1, const float _q2, const float _q3, const float _q4) 41 | { 42 | q1 = _q1; 43 | q2 = _q2; 44 | q3 = _q3; 45 | q4 = _q4; 46 | } 47 | 48 | // check if any elements are NAN 49 | bool is_nan(void) const 50 | { 51 | return isnan(q1) || isnan(q2) || isnan(q3) || isnan(q4); 52 | } 53 | 54 | // return the rotation matrix equivalent for this quaternion 55 | void rotation_matrix(Matrix3f &m) const; 56 | 57 | // return the rotation matrix equivalent for this quaternion after normalization 58 | void rotation_matrix_norm(Matrix3f &m) const; 59 | 60 | void from_rotation_matrix(const Matrix3f &m); 61 | 62 | // convert a vector from earth to body frame 63 | void earth_to_body(Vector3f &v) const; 64 | 65 | // convert a vector from body to earth frame 66 | void body_to_earth(Vector3f &v) const; 67 | 68 | // create a quaternion from Euler angles 69 | void from_euler(float roll, float pitch, float yaw); 70 | 71 | void from_vector312(float roll ,float pitch, float yaw); 72 | 73 | void to_axis_angle(Vector3f &v); 74 | 75 | void from_axis_angle(Vector3f v); 76 | 77 | void from_axis_angle(const Vector3f &axis, float theta); 78 | 79 | void rotate(const Vector3f &v); 80 | 81 | void from_axis_angle_fast(Vector3f v); 82 | 83 | void from_axis_angle_fast(const Vector3f &axis, float theta); 84 | 85 | void rotate_fast(const Vector3f &v); 86 | 87 | // get euler roll angle 88 | float get_euler_roll() const; 89 | 90 | // get euler pitch angle 91 | float get_euler_pitch() const; 92 | 93 | // get euler yaw angle 94 | float get_euler_yaw() const; 95 | 96 | // create eulers from a quaternion 97 | void to_euler(float &roll, float &pitch, float &yaw) const; 98 | 99 | // create eulers from a quaternion 100 | Vector3f to_vector312(void) const; 101 | 102 | float length(void) const; 103 | void normalize(); 104 | 105 | // initialise the quaternion to no rotation 106 | void initialise() 107 | { 108 | q1 = 1.0f; 109 | q2 = q3 = q4 = 0.0f; 110 | } 111 | 112 | Quaternion inverse(void) const; 113 | 114 | // allow a quaternion to be used as an array, 0 indexed 115 | float & operator[](uint8_t i) 116 | { 117 | float *_v = &q1; 118 | #if MATH_CHECK_INDEXES 119 | assert(i < 4); 120 | #endif 121 | return _v[i]; 122 | } 123 | 124 | const float & operator[](uint8_t i) const 125 | { 126 | const float *_v = &q1; 127 | #if MATH_CHECK_INDEXES 128 | assert(i < 4); 129 | #endif 130 | return _v[i]; 131 | } 132 | 133 | Quaternion operator*(const Quaternion &v) const; 134 | Quaternion &operator*=(const Quaternion &v); 135 | Quaternion operator/(const Quaternion &v) const; 136 | }; 137 | 138 | #endif /* __cplusplus */ 139 | 140 | #endif /* __QUATERNION_H */ 141 | -------------------------------------------------------------------------------- /Cpplibrary/include/math/rotations.h: -------------------------------------------------------------------------------- 1 | /* 2 | * rotations.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | 8 | #pragma once 9 | 10 | // these rotations form a full set - every rotation in the following 11 | // list when combined with another in the list forms an entry which is 12 | // also in the list. This is an important property. Please run the 13 | // rotations test suite if you add to the list. 14 | 15 | // NOTE!! these rotation values are stored to EEPROM, so be careful not to 16 | // change the numbering of any existing entry when adding a new entry. 17 | enum Rotation{ 18 | ROTATION_NONE = 0, 19 | ROTATION_YAW_45 = 1, 20 | ROTATION_YAW_90 = 2, 21 | ROTATION_YAW_135 = 3, 22 | ROTATION_YAW_180 = 4, 23 | ROTATION_YAW_225 = 5, 24 | ROTATION_YAW_270 = 6, 25 | ROTATION_YAW_315 = 7, 26 | ROTATION_ROLL_180 = 8, 27 | ROTATION_ROLL_180_YAW_45 = 9, 28 | ROTATION_ROLL_180_YAW_90 = 10, 29 | ROTATION_ROLL_180_YAW_135 = 11, 30 | ROTATION_PITCH_180 = 12, 31 | ROTATION_ROLL_180_YAW_225 = 13, 32 | ROTATION_ROLL_180_YAW_270 = 14, 33 | ROTATION_ROLL_180_YAW_315 = 15, 34 | ROTATION_ROLL_90 = 16, 35 | ROTATION_ROLL_90_YAW_45 = 17, 36 | ROTATION_ROLL_90_YAW_90 = 18, 37 | ROTATION_ROLL_90_YAW_135 = 19, 38 | ROTATION_ROLL_270 = 20, 39 | ROTATION_ROLL_270_YAW_45 = 21, 40 | ROTATION_ROLL_270_YAW_90 = 22, 41 | ROTATION_ROLL_270_YAW_135 = 23, 42 | ROTATION_PITCH_90 = 24, 43 | ROTATION_PITCH_270 = 25, 44 | ROTATION_PITCH_180_YAW_90 = 26, 45 | ROTATION_PITCH_180_YAW_270 = 27, 46 | ROTATION_ROLL_90_PITCH_90 = 28, 47 | ROTATION_ROLL_180_PITCH_90 = 29, 48 | ROTATION_ROLL_270_PITCH_90 = 30, 49 | ROTATION_ROLL_90_PITCH_180 = 31, 50 | ROTATION_ROLL_270_PITCH_180 = 32, 51 | ROTATION_ROLL_90_PITCH_270 = 33, 52 | ROTATION_ROLL_180_PITCH_270 = 34, 53 | ROTATION_ROLL_270_PITCH_270 = 35, 54 | ROTATION_ROLL_90_PITCH_180_YAW_90 = 36, 55 | ROTATION_ROLL_90_YAW_270 = 37, 56 | ROTATION_ROLL_90_PITCH_68_YAW_293 = 38, 57 | ROTATION_PITCH_315 = 39, 58 | ROTATION_ROLL_90_PITCH_315 = 40, 59 | /////////////////////////////////////////////////////////////////////// 60 | // Do not add more rotations without checking that there is not a conflict 61 | // with the MAVLink spec. MAV_SENSOR_ORIENTATION is expected to match our 62 | // list of rotations here. If a new rotation is added it needs to be added 63 | // to the MAVLink messages as well. 64 | /////////////////////////////////////////////////////////////////////// 65 | ROTATION_MAX, 66 | ROTATION_CUSTOM = 100, 67 | }; 68 | /* 69 | Here are the same values in a form sutable for a @Values attribute in 70 | auto documentation: 71 | 72 | @Values: 0:None,1:Yaw45,2:Yaw90,3:Yaw135,4:Yaw180,5:Yaw225,6:Yaw270,7:Yaw315,8:Roll180,9:Roll180Yaw45,10:Roll180Yaw90,11:Roll180Yaw135,12:Pitch180,13:Roll180Yaw225,14:Roll180Yaw270,15:Roll180Yaw315,16:Roll90,17:Roll90Yaw45,18:Roll90Yaw90,19:Roll90Yaw135,20:Roll270,21:Roll270Yaw45,22:Roll270Yaw90,23:Roll270Yaw135,24:Pitch90,25:Pitch270,26:Pitch180Yaw90,27:Pitch180Yaw270,28:Roll90Pitch90,29:Roll180Pitch90,30:Roll270Pitch90,31:Roll90Pitch180,32:Roll270Pitch180,33:Roll90Pitch270,34:Roll180Pitch270,35:Roll270Pitch270,36:Roll90Pitch180Yaw90,37:Roll90Yaw270,38:Yaw293Pitch68Roll180,39:Pitch315,40:Roll90Pitch315,100:Custom 73 | */ 74 | -------------------------------------------------------------------------------- /Cpplibrary/include/math/vectorN.h: -------------------------------------------------------------------------------- 1 | /* 2 | * vectorN.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | 8 | #pragma once 9 | 10 | #include "math/matrixN.h" 11 | #include 12 | 13 | #ifndef MATH_CHECK_INDEXES 14 | # define MATH_CHECK_INDEXES 0 15 | #endif 16 | 17 | #if MATH_CHECK_INDEXES 18 | #include 19 | #endif 20 | 21 | #ifdef __cplusplus 22 | 23 | template 24 | class MatrixN; 25 | 26 | 27 | template 28 | class VectorN 29 | { 30 | public: 31 | // trivial ctor 32 | inline VectorN() { 33 | memset(_v, 0, sizeof(T)*N); 34 | } 35 | 36 | // vector ctor 37 | inline VectorN(const T *v) { 38 | memcpy(_v, v, sizeof(T)*N); 39 | } 40 | 41 | inline T & operator[](uint8_t i) { 42 | #if MATH_CHECK_INDEXES 43 | assert(i >= 0 && i < N); 44 | #endif 45 | return _v[i]; 46 | } 47 | 48 | inline const T & operator[](uint8_t i) const { 49 | #if MATH_CHECK_INDEXES 50 | assert(i >= 0 && i < N); 51 | #endif 52 | return _v[i]; 53 | } 54 | 55 | // test for equality 56 | bool operator ==(const VectorN &v) const { 57 | for (uint8_t i=0; i operator -(void) const { 71 | VectorN v2; 72 | for (uint8_t i=0; i operator +(const VectorN &v) const { 80 | VectorN v2; 81 | for (uint8_t i=0; i operator -(const VectorN &v) const { 89 | VectorN v2; 90 | for (uint8_t i=0; i operator *(const T num) const { 98 | VectorN v2; 99 | for (uint8_t i=0; i operator /(const T num) const { 107 | VectorN v2; 108 | for (uint8_t i=0; i &operator +=(const VectorN &v) { 116 | for (uint8_t i=0; i &operator -=(const VectorN &v) { 124 | for (uint8_t i=0; i &operator *=(const T num) { 132 | for (uint8_t i=0; i &operator /=(const T num) { 140 | for (uint8_t i=0; i &v) const { 148 | float ret = 0; 149 | for (uint8_t i=0; i &A, const VectorN &B) { 158 | for (uint8_t i = 0; i < N; i++) { 159 | _v[i] = 0; 160 | for (uint8_t k = 0; k < N; k++) { 161 | _v[i] += A.v[i][k] * B[k]; 162 | } 163 | } 164 | } 165 | 166 | private: 167 | T _v[N]; 168 | }; 169 | 170 | #endif/* __cplusplus */ 171 | -------------------------------------------------------------------------------- /Cpplibrary/include/pid/p.h: -------------------------------------------------------------------------------- 1 | /* 2 | * p.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef __P_H 10 | #define __P_H 11 | 12 | #include 13 | #include "math/math_inc.h" 14 | 15 | #ifdef __cplusplus 16 | 17 | /// @class P 18 | /// @brief Object managing one P controller 19 | class P { 20 | public: 21 | 22 | /// Constructor for P that saves its settings to EEPROM 23 | /// 24 | /// @note PIs must be named to avoid either multiple parameters with the 25 | /// same name, or an overly complex constructor. 26 | /// 27 | /// @param initial_p Initial value for the P term. 28 | /// 29 | P(const float &initial_p = 0.0f) 30 | { 31 | _kp = initial_p; 32 | } 33 | 34 | /// Iterate the P controller, return the new control value 35 | /// 36 | /// Positive error produces positive output. 37 | /// 38 | /// @param error The measured error value 39 | /// @param dt The time delta in milliseconds (note 40 | /// that update interval cannot be more 41 | /// than 65.535 seconds due to limited range 42 | /// of the data type). 43 | /// 44 | /// @returns The updated control output. 45 | /// 46 | float get_p(float error) const; 47 | 48 | /// Overload the function call operator to permit relatively easy initialisation 49 | void operator() (const float p) { _kp = p; } 50 | 51 | // accessors 52 | const float kP() const { return _kp; } 53 | void kP(const float v) { _kp= v; } 54 | 55 | private: 56 | float _kp; 57 | }; 58 | 59 | 60 | #endif /* __cplusplus */ 61 | 62 | #endif /* __P_H */ 63 | -------------------------------------------------------------------------------- /Cpplibrary/include/pid/pid.h: -------------------------------------------------------------------------------- 1 | /* 2 | * pid.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef __PID_H 10 | #define __PID_H 11 | 12 | #include 13 | #include "math/math_inc.h" 14 | 15 | #define PID_FILT_HZ_DEFAULT 20.0f // default input filter frequency 16 | #define PID_FILT_HZ_MIN 0.01f // minimum input filter frequency 17 | 18 | #ifdef __cplusplus 19 | // @class PID 20 | // @brief Copter PID control class 21 | class PID { 22 | public: 23 | 24 | // Constructor for PID 25 | PID(float initial_p, float initial_i, float initial_d, float initial_imax, float initial_filt_hz, float dt, float initial_ff = 0); 26 | // set_dt - set time step in seconds 27 | void set_dt(float dt); 28 | 29 | // set_input_filter_all - set input to PID controller 30 | // input is filtered before the PID controllers are run 31 | // this should be called before any other calls to get_p, get_i or get_d 32 | void set_input_filter_all(float input); 33 | 34 | // set_input_filter_d - set input to PID controller 35 | // only input to the D portion of the controller is filtered 36 | // this should be called before any other calls to get_p, get_i or get_d 37 | void set_input_filter_d(float input); 38 | 39 | // get_pid - get results from pid controller 40 | float get_pid(); 41 | float get_pi(); 42 | float get_p(); 43 | float get_i(); 44 | float get_d(); 45 | float get_ff(float requested_rate); 46 | 47 | // reset_I - reset the integrator 48 | void reset_I(); 49 | 50 | // reset_filter - input filter will be reset to the next value provided to set_input() 51 | void reset_filter() { _flags._reset_filter = true; } 52 | 53 | /// operator function call for easy initialisation 54 | void operator() (float p, float i, float d, float imaxval, float input_filt_hz, float dt, float ffval = 0); 55 | 56 | // get accessors 57 | float kP() { return _kp; } 58 | float kI() { return _ki; } 59 | float kD() { return _kd; } 60 | float filt_hz() { return _filt_hz; } 61 | float imax() const { return _imax; } 62 | float get_filt_alpha() const; 63 | float ff() const { return _ff; } 64 | 65 | // set accessors 66 | void kP(const float v) { _kp=v; } 67 | void kI(const float v) { _ki=v; } 68 | void kD(const float v) { _kd=v; } 69 | void imax(const float v) { _imax=fabsf(v); } 70 | void filt_hz(const float v); 71 | void ff(const float v) { _ff=v; } 72 | 73 | float get_integrator() const { return _integrator; } 74 | void set_integrator(float i) { _integrator = i; } 75 | 76 | // parameter var table 77 | // static const struct AP_Param::GroupInfo var_info[]; 78 | 79 | protected: 80 | 81 | // parameters 82 | float _kp=0; 83 | float _ki=0; 84 | float _kd=0; 85 | float _imax=0; 86 | float _filt_hz=PID_FILT_HZ_DEFAULT; // PID Input filter frequency in Hz 87 | float _ff=0; 88 | 89 | // flags 90 | struct pid_flags { 91 | bool _reset_filter : 1; // true when input filter should be reset during next call to set_input 92 | } _flags; 93 | 94 | // internal variables 95 | float _dt; // timestep in seconds 96 | float _integrator; // integrator value 97 | float _input; // last input for derivative 98 | float _derivative; // last derivative for low-pass filter 99 | 100 | // DataFlash_Class::PID_Info _pid_info; 101 | }; 102 | #endif /* __cplusplus */ 103 | 104 | #endif /* __PID_H */ 105 | -------------------------------------------------------------------------------- /Cpplibrary/include/pid/pid_2d.h: -------------------------------------------------------------------------------- 1 | /* 2 | * pid_2d.h 3 | * 4 | * Created on: 2020.06.19 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | 9 | #ifndef __PID_2D_H 10 | #define __PID_2D_H 11 | 12 | #include 13 | #include "math/math_inc.h" 14 | 15 | #ifdef __cplusplus 16 | // @class PID_2D 17 | // @brief PID_2D control class 18 | class PID_2D { 19 | public: 20 | 21 | // Constructor for PID 22 | PID_2D(float initial_p_x, float initial_p_y, float initial_i_x, float initial_i_y, float initial_d_x, float initial_d_y, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt); 23 | 24 | // set_dt - set time step in seconds 25 | void set_dt(float dt); 26 | 27 | /// Overload the function call operator to permit easy initialisation 28 | void operator() (float initial_p_x, float initial_p_y, float initial_i_x, float initial_i_y, float initial_d_x, float initial_d_y, float initial_imax, float initial_filt_hz, float initial_filt_d_hz, float dt); 29 | 30 | // set_input - set input to PID controller 31 | // input is filtered before the PID controllers are run 32 | // this should be called before any other calls to get_p, get_i or get_d 33 | void set_input(const Vector2f &input); 34 | void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); } 35 | 36 | // get_pi - get results from pid controller 37 | Vector2f get_pid(); 38 | Vector2f get_p() const; 39 | Vector2f get_i(); 40 | Vector2f get_i_shrink(); // get_i but do not allow integrator to grow (it may shrink) 41 | Vector2f get_d(); 42 | 43 | // reset_I - reset the integrator 44 | void reset_I(); 45 | 46 | // reset_filter - input and D term filter will be reset to the next value provided to set_input() 47 | void reset_filter(); 48 | 49 | // get accessors 50 | Vector2f kP() { return _kp; } 51 | Vector2f kI() { return _ki; } 52 | Vector2f kD() { return _kd; } 53 | float imax() const { return _imax; } 54 | float filt_hz() const { return _filt_hz; } 55 | float get_filt_alpha() const { return _filt_alpha; } 56 | float filt_d_hz() const { return _filt_hz; } 57 | float get_filt_alpha_D() const { return _filt_alpha_d; } 58 | 59 | // set accessors 60 | void kP(const Vector2f v) { _kp=v; } 61 | void kI(const Vector2f v) { _ki=v; } 62 | void kD(const Vector2f v) { _kd=v; } 63 | void imax(const float v) { _imax=fabsf(v); } 64 | void filt_hz(const float v); 65 | void filt_d_hz(const float v); 66 | 67 | Vector2f get_integrator() const { return _integrator; } 68 | void set_integrator(const Vector2f &i) { _integrator = i; } 69 | void set_integrator(const Vector3f &i) { _integrator.x = i.x; _integrator.y = i.y; } 70 | 71 | protected: 72 | 73 | // set_input_filter_d - set input to PID controller 74 | // only input to the D portion of the controller is filtered 75 | // this should be called before any other calls to get_p, get_i or get_d 76 | void set_input_filter_d(const Vector2f& input_delta); 77 | 78 | // calc_filt_alpha - recalculate the input filter alpha 79 | void calc_filt_alpha(); 80 | 81 | // calc_filt_alpha - recalculate the input filter alpha 82 | void calc_filt_alpha_d(); 83 | 84 | // parameters 85 | Vector2f _kp; 86 | Vector2f _ki; 87 | Vector2f _kd; 88 | float _imax; 89 | float _filt_hz; // PID Input filter frequency in Hz 90 | float _filt_d_hz; // D term filter frequency in Hz 91 | 92 | // flags 93 | struct ac_pid_flags { 94 | bool _reset_filter : 1; // true when input filter should be reset during next call to set_input 95 | } _flags; 96 | 97 | // internal variables 98 | float _dt; // timestep in seconds 99 | float _filt_alpha; // input filter alpha 100 | float _filt_alpha_d; // input filter alpha 101 | Vector2f _integrator; // integrator value 102 | Vector2f _input; // last input for derivative 103 | Vector2f _derivative; // last derivative for low-pass filter 104 | }; 105 | 106 | #endif /* __cplusplus */ 107 | 108 | #endif /* __PID_H */ 109 | -------------------------------------------------------------------------------- /Cpplibrary/include/sdlog/sdlog.h: -------------------------------------------------------------------------------- 1 | /* 2 | * sdlog.h 3 | * 4 | * Created on: 2020.07.26 5 | * Author: JackyPan 6 | */ 7 | #pragma once 8 | /* Define to prevent recursive inclusion -------------------------------------*/ 9 | #ifndef __SDLOG_H 10 | #define __SDLOG_H 11 | 12 | #include "common.h" 13 | 14 | #define GNSS_POINT_MAX 1000 15 | class SDLog{ 16 | private: 17 | typedef enum{ 18 | LOG_CAT = 0, 19 | LOG_DATA, 20 | LOG_END, 21 | }Log_Type; 22 | DIR SD_Dir_Mlog,SD_Dir_Mission; 23 | UINT log_num; 24 | char buff_read[16]; /* Pointer to data buffer */ 25 | UINT btr=16; /* Number of bytes to read */ 26 | UINT br; /* number of bytes read */ 27 | uint16_t index_log=1; 28 | FRESULT res; 29 | UINT len_filename; 30 | char file_name[10]; 31 | va_list args; 32 | uint8_t buffer_max=128; 33 | UINT len; 34 | char log_buffer[128]; 35 | FRESULT Open_Log_File(void); 36 | FRESULT Write_Gnss_File(void); 37 | FRESULT Read_Gnss_File(void); 38 | void Close_Log_File(void); 39 | void Log_To_File(Log_Type log_type); 40 | 41 | public: 42 | SDLog(void); 43 | 44 | typedef enum{ 45 | Logger_Idle = 0, 46 | Logger_Open, 47 | Logger_Close, 48 | Logger_Record, 49 | Logger_Gnss_Write, 50 | Logger_Gnss_Read 51 | }Logger_Status; 52 | Logger_Status m_Logger_Status; 53 | uint16_t gnss_point_num=0; 54 | Vector3f gnss_point[GNSS_POINT_MAX]; 55 | void Logger_Update(void); 56 | void Logger_Enable(void); 57 | void Logger_Disable(void); 58 | void Logger_Write(const char* s, ...); 59 | void Logger_Write_Gnss(void); 60 | void Logger_Read_Gnss(void); 61 | }; 62 | 63 | #endif /* __SDLOG_H */ 64 | -------------------------------------------------------------------------------- /Cpplibrary/include/uwb/deca_param_types.h: -------------------------------------------------------------------------------- 1 | /*! ---------------------------------------------------------------------------- 2 | * @file deca_param_types.h 3 | * @brief DecaWave general type definitions for configuration structures 4 | * 5 | * @attention 6 | * 7 | * Copyright 2013 (c) DecaWave Ltd, Dublin, Ireland. 8 | * 9 | * All rights reserved. 10 | * 11 | */ 12 | #ifndef _DECA_PARAM_TYPES_H_ 13 | #define _DECA_PARAM_TYPES_H_ 14 | 15 | #ifdef __cplusplus 16 | extern "C" { 17 | #endif 18 | #include "hal.h" 19 | 20 | #define NUM_BR 3 21 | #define NUM_PRF 2 22 | #define NUM_PACS 4 23 | #define NUM_BW 2 //2 bandwidths are supported 24 | #define NUM_SFD 2 //supported number of SFDs - standard = 0, non-standard = 1 25 | #define NUM_CH 6 //supported channels are 1, 2, 3, 4, 5, 7 26 | #define NUM_CH_SUPPORTED 8 //supported channels are '0', 1, 2, 3, 4, 5, '6', 7 27 | #define PCODES 25 //supported preamble codes 28 | 29 | 30 | typedef struct { 31 | uint32_t lo32; 32 | uint16_t target[NUM_PRF]; 33 | } agc_cfg_struct ; 34 | 35 | extern const agc_cfg_struct agc_config ; 36 | 37 | //SFD threshold settings for 110k, 850k, 6.8Mb standard and non-standard 38 | extern const uint16_t sftsh[NUM_BR][NUM_SFD]; 39 | 40 | extern const uint16_t dtune1[NUM_PRF]; 41 | 42 | #define XMLPARAMS_VERSION (1.17f) 43 | 44 | extern const uint32_t fs_pll_cfg[NUM_CH]; 45 | extern const uint8_t fs_pll_tune[NUM_CH]; 46 | extern const uint8_t rx_config[NUM_BW]; 47 | extern const uint32_t tx_config[NUM_CH]; 48 | extern const uint8_t dwnsSFDlen[NUM_BR]; //length of SFD for each of the bitrates 49 | extern const uint32_t digital_bb_config[NUM_PRF][NUM_PACS]; 50 | extern const uint8_t chan_idx[NUM_CH_SUPPORTED]; 51 | 52 | #define TEMP_COMP_FACTOR_CH2 (327) //(INT) (0.0798 * 4096) 53 | #define TEMP_COMP_FACTOR_CH5 (607) //(INT) (0.1482 * 4096) 54 | #define SAR_TEMP_TO_CELCIUS_CONV (1.14) 55 | #define SAR_VBAT_TO_VOLT_CONV (1.0/173) 56 | 57 | #define DCELCIUS_TO_SAR_TEMP_CONV ((int)((0.10/1.14)*256)) 58 | #define MVOLT_TO_SAR_VBAT_CONV (173.0/1000) 59 | 60 | #define MIXER_GAIN_STEP (0.5) 61 | #define DA_ATTN_STEP (2.5) 62 | 63 | #define MIX_DA_FACTOR (DA_ATTN_STEP/MIXER_GAIN_STEP) 64 | 65 | #define PEAK_MULTPLIER (0x60) //3 -> (0x3 * 32) & 0x00E0 66 | #define N_STD_FACTOR (13) 67 | #define LDE_PARAM1 (PEAK_MULTPLIER | N_STD_FACTOR) 68 | 69 | #define LDE_PARAM3_16 (0x1607) 70 | #define LDE_PARAM3_64 (0x0607) 71 | 72 | extern const uint16_t lde_replicaCoeff[PCODES]; 73 | 74 | #ifdef __cplusplus 75 | } 76 | #endif 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /Cpplibrary/include/uwb/deca_version.h: -------------------------------------------------------------------------------- 1 | /*! ---------------------------------------------------------------------------- 2 | * @file deca_version.h 3 | * @brief Defines the version info for the DW1000 device driver including its API 4 | * 5 | * @attention 6 | * 7 | * Copyright 2013 (c) DecaWave Ltd, Dublin, Ireland. 8 | * 9 | * All rights reserved. 10 | * 11 | */ 12 | 13 | #ifndef _DECA_VERSION_H_ 14 | #define _DECA_VERSION_H_ 15 | 16 | #include "hal.h" 17 | 18 | 19 | // 20 | // The DW1000 device driver is separately version numbered to any version the application using it may have 21 | // 22 | // Two symbols are defined here: one hexadecimal value and one string that includes the hex bytes. 23 | // Both should be updated together in a consistent way when the software is being modified. 24 | // 25 | // The format of the hex version is 0xAABBCC and the string ends with AA.BB.CC, where... 26 | // 27 | // Quantity CC is updated for minor changes/bug fixes that should not need user code changes 28 | // Quantity BB is updated for changes/bug fixes that may need user code changes 29 | // Quantity AA is updated for major changes that will need user code changes 30 | // 31 | 32 | #define DW1000_DRIVER_VERSION 0x050100 33 | #define DW1000_DEVICE_DRIVER_VER_STRING "DW1000 Device Driver Version 05.01.00" 34 | 35 | #endif 36 | -------------------------------------------------------------------------------- /Cpplibrary/include/uwb/trilateration.h: -------------------------------------------------------------------------------- 1 | // ------------------------------------------------------------------------------------------------------------------- 2 | // 3 | // File: trilateration.h 4 | // 5 | // Copyright 2016 (c) Decawave Ltd, Dublin, Ireland. 6 | // 7 | // All rights reserved. 8 | // 9 | // 10 | // ------------------------------------------------------------------------------------------------------------------- 11 | // 12 | 13 | #ifndef __TRILATERATION_H__ 14 | #define __TRILATERATION_H__ 15 | 16 | #include "hal.h" 17 | 18 | //#define SHOW_PRINTS 19 | 20 | #define TRILATERATION (1) 21 | 22 | #define REGRESSION_NUM (10) 23 | #define SPEED_OF_LIGHT (299702547.0) // in m/s in air 24 | #define NUM_ANCHORS (5) 25 | #define REF_ANCHOR (5) //anchor IDs are 1,2,3,4,5 etc. (don't start from 0!) 26 | 27 | 28 | #define TRIL_3SPHERES 3 29 | #define TRIL_4SPHERES 4 30 | 31 | typedef struct vec3d vec3d; 32 | struct vec3d { 33 | double x; 34 | double y; 35 | double z; 36 | }; 37 | 38 | /* Return the difference of two vectors, (vector1 - vector2). */ 39 | vec3d vdiff(const vec3d vector1, const vec3d vector2); 40 | 41 | /* Return the sum of two vectors. */ 42 | vec3d vsum(const vec3d vector1, const vec3d vector2); 43 | 44 | /* Multiply vector by a number. */ 45 | vec3d vmul(const vec3d vector, const double n); 46 | 47 | /* Divide vector by a number. */ 48 | vec3d vdiv(const vec3d vector, const double n); 49 | 50 | /* Return the Euclidean norm. */ 51 | double vdist(const vec3d v1, const vec3d v2); 52 | 53 | /* Return the Euclidean norm. */ 54 | double vnorm(const vec3d vector); 55 | 56 | /* Return the dot product of two vectors. */ 57 | double dot(const vec3d vector1, const vec3d vector2); 58 | 59 | /* Replace vector with its cross product with another vector. */ 60 | vec3d cross(const vec3d vector1, const vec3d vector2); 61 | 62 | int GetLocation(vec3d *best_solution, int use4thAnchor, vec3d* anchorArray, int *distanceArray); 63 | int GetLocation2(vec3d * best_solution,int use4thAnchor,vec3d * anchorArray,int * distanceArray); 64 | void Th_Location(vec3d * anchorArray,int * distanceArray); 65 | void Th_Location2(vec3d * anchorArray,int * distanceArray); 66 | 67 | double vdist(const vec3d v1, const vec3d v2); 68 | #endif 69 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Device/ST/STM32F4xx/Include/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /** @addtogroup CMSIS 20 | * @{ 21 | */ 22 | 23 | /** @addtogroup stm32f4xx_system 24 | * @{ 25 | */ 26 | 27 | /** 28 | * @brief Define to prevent recursive inclusion 29 | */ 30 | #ifndef __SYSTEM_STM32F4XX_H 31 | #define __SYSTEM_STM32F4XX_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /** @addtogroup STM32F4xx_System_Includes 38 | * @{ 39 | */ 40 | 41 | /** 42 | * @} 43 | */ 44 | 45 | 46 | /** @addtogroup STM32F4xx_System_Exported_types 47 | * @{ 48 | */ 49 | /* This variable is updated in three ways: 50 | 1) by calling CMSIS function SystemCoreClockUpdate() 51 | 2) by calling HAL API function HAL_RCC_GetSysClockFreq() 52 | 3) each time HAL_RCC_ClockConfig() is called to configure the system clock frequency 53 | Note: If you use this function to configure the system clock; then there 54 | is no need to call the 2 first functions listed above, since SystemCoreClock 55 | variable is updated automatically. 56 | */ 57 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 58 | 59 | extern const uint8_t AHBPrescTable[16]; /*!< AHB prescalers table values */ 60 | extern const uint8_t APBPrescTable[8]; /*!< APB prescalers table values */ 61 | 62 | /** 63 | * @} 64 | */ 65 | 66 | /** @addtogroup STM32F4xx_System_Exported_Constants 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @addtogroup STM32F4xx_System_Exported_Macros 75 | * @{ 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @addtogroup STM32F4xx_System_Exported_Functions 83 | * @{ 84 | */ 85 | 86 | extern void SystemInit(void); 87 | extern void SystemCoreClockUpdate(void); 88 | /** 89 | * @} 90 | */ 91 | 92 | #ifdef __cplusplus 93 | } 94 | #endif 95 | 96 | #endif /*__SYSTEM_STM32F4XX_H */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** 103 | * @} 104 | */ 105 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Device/ST/STM32F4xx/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This software component is provided to you as part of a software package and 2 | applicable license terms are in the Package_license file. If you received this 3 | software component outside of a package or without applicable license terms, 4 | the terms of the Apache-2.0 license shall apply. 5 | You may obtain a copy of the Apache-2.0 at: 6 | https://opensource.org/licenses/Apache-2.0 7 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Include/cmsis_version.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************//** 2 | * @file cmsis_version.h 3 | * @brief CMSIS Core(M) Version definitions 4 | * @version V5.0.2 5 | * @date 19. April 2017 6 | ******************************************************************************/ 7 | /* 8 | * Copyright (c) 2009-2017 ARM Limited. All rights reserved. 9 | * 10 | * SPDX-License-Identifier: Apache-2.0 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the License); you may 13 | * not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * 16 | * www.apache.org/licenses/LICENSE-2.0 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 20 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the License for the specific language governing permissions and 22 | * limitations under the License. 23 | */ 24 | 25 | #if defined ( __ICCARM__ ) 26 | #pragma system_include /* treat file as system include file for MISRA check */ 27 | #elif defined (__clang__) 28 | #pragma clang system_header /* treat file as system include file */ 29 | #endif 30 | 31 | #ifndef __CMSIS_VERSION_H 32 | #define __CMSIS_VERSION_H 33 | 34 | /* CMSIS Version definitions */ 35 | #define __CM_CMSIS_VERSION_MAIN ( 5U) /*!< [31:16] CMSIS Core(M) main version */ 36 | #define __CM_CMSIS_VERSION_SUB ( 1U) /*!< [15:0] CMSIS Core(M) sub version */ 37 | #define __CM_CMSIS_VERSION ((__CM_CMSIS_VERSION_MAIN << 16U) | \ 38 | __CM_CMSIS_VERSION_SUB ) /*!< CMSIS Core(M) version number */ 39 | #endif 40 | -------------------------------------------------------------------------------- /Drivers/CMSIS/Include/tz_context.h: -------------------------------------------------------------------------------- 1 | /****************************************************************************** 2 | * @file tz_context.h 3 | * @brief Context Management for Armv8-M TrustZone 4 | * @version V1.0.1 5 | * @date 10. January 2018 6 | ******************************************************************************/ 7 | /* 8 | * Copyright (c) 2017-2018 Arm Limited. All rights reserved. 9 | * 10 | * SPDX-License-Identifier: Apache-2.0 11 | * 12 | * Licensed under the Apache License, Version 2.0 (the License); you may 13 | * not use this file except in compliance with the License. 14 | * You may obtain a copy of the License at 15 | * 16 | * www.apache.org/licenses/LICENSE-2.0 17 | * 18 | * Unless required by applicable law or agreed to in writing, software 19 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 20 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 21 | * See the License for the specific language governing permissions and 22 | * limitations under the License. 23 | */ 24 | 25 | #if defined ( __ICCARM__ ) 26 | #pragma system_include /* treat file as system include file for MISRA check */ 27 | #elif defined (__clang__) 28 | #pragma clang system_header /* treat file as system include file */ 29 | #endif 30 | 31 | #ifndef TZ_CONTEXT_H 32 | #define TZ_CONTEXT_H 33 | 34 | #include 35 | 36 | #ifndef TZ_MODULEID_T 37 | #define TZ_MODULEID_T 38 | /// \details Data type that identifies secure software modules called by a process. 39 | typedef uint32_t TZ_ModuleId_t; 40 | #endif 41 | 42 | /// \details TZ Memory ID identifies an allocated memory slot. 43 | typedef uint32_t TZ_MemoryId_t; 44 | 45 | /// Initialize secure context memory system 46 | /// \return execution status (1: success, 0: error) 47 | uint32_t TZ_InitContextSystem_S (void); 48 | 49 | /// Allocate context memory for calling secure software modules in TrustZone 50 | /// \param[in] module identifies software modules called from non-secure mode 51 | /// \return value != 0 id TrustZone memory slot identifier 52 | /// \return value 0 no memory available or internal error 53 | TZ_MemoryId_t TZ_AllocModuleContext_S (TZ_ModuleId_t module); 54 | 55 | /// Free context memory that was previously allocated with \ref TZ_AllocModuleContext_S 56 | /// \param[in] id TrustZone memory slot identifier 57 | /// \return execution status (1: success, 0: error) 58 | uint32_t TZ_FreeModuleContext_S (TZ_MemoryId_t id); 59 | 60 | /// Load secure context (called on RTOS thread context switch) 61 | /// \param[in] id TrustZone memory slot identifier 62 | /// \return execution status (1: success, 0: error) 63 | uint32_t TZ_LoadContext_S (TZ_MemoryId_t id); 64 | 65 | /// Store secure context (called on RTOS thread context switch) 66 | /// \param[in] id TrustZone memory slot identifier 67 | /// \return execution status (1: success, 0: error) 68 | uint32_t TZ_StoreContext_S (TZ_MemoryId_t id); 69 | 70 | #endif // TZ_CONTEXT_H 71 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_dma_ex.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_dma_ex.h 4 | * @author MCD Application Team 5 | * @brief Header file of DMA HAL extension module. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file in 13 | * the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __STM32F4xx_HAL_DMA_EX_H 21 | #define __STM32F4xx_HAL_DMA_EX_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "stm32f4xx_hal_def.h" 29 | 30 | /** @addtogroup STM32F4xx_HAL_Driver 31 | * @{ 32 | */ 33 | 34 | /** @addtogroup DMAEx 35 | * @{ 36 | */ 37 | 38 | /* Exported types ------------------------------------------------------------*/ 39 | /** @defgroup DMAEx_Exported_Types DMAEx Exported Types 40 | * @brief DMAEx Exported types 41 | * @{ 42 | */ 43 | 44 | /** 45 | * @brief HAL DMA Memory definition 46 | */ 47 | typedef enum 48 | { 49 | MEMORY0 = 0x00U, /*!< Memory 0 */ 50 | MEMORY1 = 0x01U /*!< Memory 1 */ 51 | }HAL_DMA_MemoryTypeDef; 52 | 53 | /** 54 | * @} 55 | */ 56 | 57 | /* Exported functions --------------------------------------------------------*/ 58 | /** @defgroup DMAEx_Exported_Functions DMAEx Exported Functions 59 | * @brief DMAEx Exported functions 60 | * @{ 61 | */ 62 | 63 | /** @defgroup DMAEx_Exported_Functions_Group1 Extended features functions 64 | * @brief Extended features functions 65 | * @{ 66 | */ 67 | 68 | /* IO operation functions *******************************************************/ 69 | HAL_StatusTypeDef HAL_DMAEx_MultiBufferStart(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t SecondMemAddress, uint32_t DataLength); 70 | HAL_StatusTypeDef HAL_DMAEx_MultiBufferStart_IT(DMA_HandleTypeDef *hdma, uint32_t SrcAddress, uint32_t DstAddress, uint32_t SecondMemAddress, uint32_t DataLength); 71 | HAL_StatusTypeDef HAL_DMAEx_ChangeMemory(DMA_HandleTypeDef *hdma, uint32_t Address, HAL_DMA_MemoryTypeDef memory); 72 | 73 | /** 74 | * @} 75 | */ 76 | /** 77 | * @} 78 | */ 79 | 80 | /* Private functions ---------------------------------------------------------*/ 81 | /** @defgroup DMAEx_Private_Functions DMAEx Private Functions 82 | * @brief DMAEx Private functions 83 | * @{ 84 | */ 85 | /** 86 | * @} 87 | */ 88 | 89 | /** 90 | * @} 91 | */ 92 | 93 | /** 94 | * @} 95 | */ 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /*__STM32F4xx_HAL_DMA_EX_H*/ 102 | 103 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_flash_ramfunc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_flash_ramfunc.h 4 | * @author MCD Application Team 5 | * @brief Header file of FLASH RAMFUNC driver. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file in 13 | * the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | ****************************************************************************** 16 | */ 17 | 18 | /* Define to prevent recursive inclusion -------------------------------------*/ 19 | #ifndef __STM32F4xx_FLASH_RAMFUNC_H 20 | #define __STM32F4xx_FLASH_RAMFUNC_H 21 | 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | #if defined(STM32F410Tx) || defined(STM32F410Cx) || defined(STM32F410Rx) || defined(STM32F411xE) || defined(STM32F446xx) || defined(STM32F412Zx) ||\ 26 | defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_hal_def.h" 30 | 31 | /** @addtogroup STM32F4xx_HAL_Driver 32 | * @{ 33 | */ 34 | 35 | /** @addtogroup FLASH_RAMFUNC 36 | * @{ 37 | */ 38 | 39 | /* Exported types ------------------------------------------------------------*/ 40 | /* Exported macro ------------------------------------------------------------*/ 41 | /* Exported functions --------------------------------------------------------*/ 42 | /** @addtogroup FLASH_RAMFUNC_Exported_Functions 43 | * @{ 44 | */ 45 | 46 | /** @addtogroup FLASH_RAMFUNC_Exported_Functions_Group1 47 | * @{ 48 | */ 49 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StopFlashInterfaceClk(void); 50 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_StartFlashInterfaceClk(void); 51 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_EnableFlashSleepMode(void); 52 | __RAM_FUNC HAL_StatusTypeDef HAL_FLASHEx_DisableFlashSleepMode(void); 53 | /** 54 | * @} 55 | */ 56 | 57 | /** 58 | * @} 59 | */ 60 | 61 | /** 62 | * @} 63 | */ 64 | 65 | /** 66 | * @} 67 | */ 68 | 69 | #endif /* STM32F410xx || STM32F411xE || STM32F446xx || STM32F412Zx || STM32F412Vx || STM32F412Rx || STM32F412Cx */ 70 | #ifdef __cplusplus 71 | } 72 | #endif 73 | 74 | 75 | #endif /* __STM32F4xx_FLASH_RAMFUNC_H */ 76 | 77 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_i2c_ex.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_i2c_ex.h 4 | * @author MCD Application Team 5 | * @brief Header file of I2C HAL Extension module. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2016 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __STM32F4xx_HAL_I2C_EX_H 21 | #define __STM32F4xx_HAL_I2C_EX_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | #if defined(I2C_FLTR_ANOFF)&&defined(I2C_FLTR_DNF) 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_hal_def.h" 30 | 31 | /** @addtogroup STM32F4xx_HAL_Driver 32 | * @{ 33 | */ 34 | 35 | /** @addtogroup I2CEx 36 | * @{ 37 | */ 38 | 39 | /* Exported types ------------------------------------------------------------*/ 40 | /* Exported constants --------------------------------------------------------*/ 41 | /** @defgroup I2CEx_Exported_Constants I2C Exported Constants 42 | * @{ 43 | */ 44 | 45 | /** @defgroup I2CEx_Analog_Filter I2C Analog Filter 46 | * @{ 47 | */ 48 | #define I2C_ANALOGFILTER_ENABLE 0x00000000U 49 | #define I2C_ANALOGFILTER_DISABLE I2C_FLTR_ANOFF 50 | /** 51 | * @} 52 | */ 53 | 54 | /** 55 | * @} 56 | */ 57 | 58 | /* Exported macro ------------------------------------------------------------*/ 59 | /* Exported functions --------------------------------------------------------*/ 60 | /** @addtogroup I2CEx_Exported_Functions 61 | * @{ 62 | */ 63 | 64 | /** @addtogroup I2CEx_Exported_Functions_Group1 65 | * @{ 66 | */ 67 | /* Peripheral Control functions ************************************************/ 68 | HAL_StatusTypeDef HAL_I2CEx_ConfigAnalogFilter(I2C_HandleTypeDef *hi2c, uint32_t AnalogFilter); 69 | HAL_StatusTypeDef HAL_I2CEx_ConfigDigitalFilter(I2C_HandleTypeDef *hi2c, uint32_t DigitalFilter); 70 | /** 71 | * @} 72 | */ 73 | 74 | /** 75 | * @} 76 | */ 77 | /* Private types -------------------------------------------------------------*/ 78 | /* Private variables ---------------------------------------------------------*/ 79 | /* Private constants ---------------------------------------------------------*/ 80 | /** @defgroup I2CEx_Private_Constants I2C Private Constants 81 | * @{ 82 | */ 83 | 84 | /** 85 | * @} 86 | */ 87 | 88 | /* Private macros ------------------------------------------------------------*/ 89 | /** @defgroup I2CEx_Private_Macros I2C Private Macros 90 | * @{ 91 | */ 92 | #define IS_I2C_ANALOG_FILTER(FILTER) (((FILTER) == I2C_ANALOGFILTER_ENABLE) || \ 93 | ((FILTER) == I2C_ANALOGFILTER_DISABLE)) 94 | #define IS_I2C_DIGITAL_FILTER(FILTER) ((FILTER) <= 0x0000000FU) 95 | /** 96 | * @} 97 | */ 98 | 99 | /** 100 | * @} 101 | */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | #endif 108 | 109 | #ifdef __cplusplus 110 | } 111 | #endif 112 | 113 | #endif /* __STM32F4xx_HAL_I2C_EX_H */ 114 | 115 | 116 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_pcd_ex.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_hal_pcd_ex.h 4 | * @author MCD Application Team 5 | * @brief Header file of PCD HAL Extension module. 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2016 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef STM32F4xx_HAL_PCD_EX_H 21 | #define STM32F4xx_HAL_PCD_EX_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif /* __cplusplus */ 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "stm32f4xx_hal_def.h" 29 | 30 | #if defined (USB_OTG_FS) || defined (USB_OTG_HS) 31 | /** @addtogroup STM32F4xx_HAL_Driver 32 | * @{ 33 | */ 34 | 35 | /** @addtogroup PCDEx 36 | * @{ 37 | */ 38 | /* Exported types ------------------------------------------------------------*/ 39 | /* Exported constants --------------------------------------------------------*/ 40 | /* Exported macros -----------------------------------------------------------*/ 41 | /* Exported functions --------------------------------------------------------*/ 42 | /** @addtogroup PCDEx_Exported_Functions PCDEx Exported Functions 43 | * @{ 44 | */ 45 | /** @addtogroup PCDEx_Exported_Functions_Group1 Peripheral Control functions 46 | * @{ 47 | */ 48 | 49 | #if defined (USB_OTG_FS) || defined (USB_OTG_HS) 50 | HAL_StatusTypeDef HAL_PCDEx_SetTxFiFo(PCD_HandleTypeDef *hpcd, uint8_t fifo, uint16_t size); 51 | HAL_StatusTypeDef HAL_PCDEx_SetRxFiFo(PCD_HandleTypeDef *hpcd, uint16_t size); 52 | #endif /* defined (USB_OTG_FS) || defined (USB_OTG_HS) */ 53 | 54 | #if defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) || defined(STM32F413xx) || defined(STM32F423xx) 55 | HAL_StatusTypeDef HAL_PCDEx_ActivateLPM(PCD_HandleTypeDef *hpcd); 56 | HAL_StatusTypeDef HAL_PCDEx_DeActivateLPM(PCD_HandleTypeDef *hpcd); 57 | #endif /* defined(STM32F446xx) || defined(STM32F469xx) || defined(STM32F479xx) || defined(STM32F412Zx) || defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) || defined(STM32F413xx) || defined(STM32F423xx) */ 58 | #if defined(STM32F412Zx) || defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) || defined(STM32F413xx) || defined(STM32F423xx) 59 | HAL_StatusTypeDef HAL_PCDEx_ActivateBCD(PCD_HandleTypeDef *hpcd); 60 | HAL_StatusTypeDef HAL_PCDEx_DeActivateBCD(PCD_HandleTypeDef *hpcd); 61 | void HAL_PCDEx_BCD_VBUSDetect(PCD_HandleTypeDef *hpcd); 62 | #endif /* defined(STM32F412Zx) || defined(STM32F412Vx) || defined(STM32F412Rx) || defined(STM32F412Cx) || defined(STM32F413xx) || defined(STM32F423xx) */ 63 | void HAL_PCDEx_LPM_Callback(PCD_HandleTypeDef *hpcd, PCD_LPM_MsgTypeDef msg); 64 | void HAL_PCDEx_BCD_Callback(PCD_HandleTypeDef *hpcd, PCD_BCD_MsgTypeDef msg); 65 | 66 | /** 67 | * @} 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** 75 | * @} 76 | */ 77 | 78 | /** 79 | * @} 80 | */ 81 | #endif /* defined (USB_OTG_FS) || defined (USB_OTG_HS) */ 82 | 83 | #ifdef __cplusplus 84 | } 85 | #endif /* __cplusplus */ 86 | 87 | 88 | #endif /* STM32F4xx_HAL_PCD_EX_H */ 89 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/LICENSE.txt: -------------------------------------------------------------------------------- 1 | This software component is provided to you as part of a software package and 2 | applicable license terms are in the Package_license file. If you received this 3 | software component outside of a package or without applicable license terms, 4 | the terms of the BSD-3-Clause license shall apply. 5 | You may obtain a copy of the BSD-3-Clause at: 6 | https://opensource.org/licenses/BSD-3-Clause 7 | -------------------------------------------------------------------------------- /Drivers/STM32F4xx_HAL_Driver/License.md: -------------------------------------------------------------------------------- 1 | # Copyright (c) 2016 STMicroelectronics 2 | 3 | This software component is licensed by STMicroelectronics under the **BSD-3-Clause** license. You may not use this file except in compliance with this license. You may obtain a copy of the license [here](https://opensource.org/licenses/BSD-3-Clause). -------------------------------------------------------------------------------- /FATFS/App/fatfs.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file fatfs.c 5 | * @brief Code for fatfs applications 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2021 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | /* USER CODE END Header */ 19 | #include "fatfs.h" 20 | 21 | uint8_t retSD; /* Return value for SD */ 22 | char SDPath[4]; /* SD logical drive path */ 23 | FATFS SDFatFS; /* File system object for SD logical drive */ 24 | FIL SDFile; /* File object for SD */ 25 | 26 | /* USER CODE BEGIN Variables */ 27 | FIL SDMission; 28 | /* USER CODE END Variables */ 29 | 30 | void MX_FATFS_Init(void) 31 | { 32 | /*## FatFS: Link the SD driver ###########################*/ 33 | retSD = FATFS_LinkDriver(&SD_Driver, SDPath); 34 | 35 | /* USER CODE BEGIN Init */ 36 | /* additional user code for init */ 37 | /* USER CODE END Init */ 38 | } 39 | 40 | /** 41 | * @brief Gets Time from RTC 42 | * @param None 43 | * @retval Time in DWORD 44 | */ 45 | DWORD get_fattime(void) 46 | { 47 | /* USER CODE BEGIN get_fattime */ 48 | return 0; 49 | /* USER CODE END get_fattime */ 50 | } 51 | 52 | /* USER CODE BEGIN Application */ 53 | 54 | /* USER CODE END Application */ 55 | -------------------------------------------------------------------------------- /FATFS/App/fatfs.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file fatfs.h 5 | * @brief Header for fatfs applications 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2021 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | /* USER CODE END Header */ 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __fatfs_H 21 | #define __fatfs_H 22 | #ifdef __cplusplus 23 | extern "C" { 24 | #endif 25 | 26 | #include "ff.h" 27 | #include "ff_gen_drv.h" 28 | #include "sd_diskio.h" /* defines SD_Driver as external */ 29 | 30 | /* USER CODE BEGIN Includes */ 31 | #include "hal.h" 32 | /* USER CODE END Includes */ 33 | 34 | extern uint8_t retSD; /* Return value for SD */ 35 | extern char SDPath[4]; /* SD logical drive path */ 36 | extern FATFS SDFatFS; /* File system object for SD logical drive */ 37 | extern FIL SDFile; /* File object for SD */ 38 | 39 | void MX_FATFS_Init(void); 40 | 41 | /* USER CODE BEGIN Prototypes */ 42 | extern FIL SDMission; 43 | /* USER CODE END Prototypes */ 44 | #ifdef __cplusplus 45 | } 46 | #endif 47 | #endif /*__fatfs_H */ 48 | -------------------------------------------------------------------------------- /FATFS/Target/bsp_driver_sd.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file bsp_driver_sd.h for F4 (based on stm324x9i_eval_sd.h) 5 | * @brief This file contains the common defines and functions prototypes for 6 | * the bsp_driver_sd.c driver. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | * Copyright (c) 2021 STMicroelectronics. 11 | * All rights reserved. 12 | * 13 | * This software is licensed under terms that can be found in the LICENSE file 14 | * in the root directory of this software component. 15 | * If no LICENSE file comes with this software, it is provided AS-IS. 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | /* Define to prevent recursive inclusion -------------------------------------*/ 21 | #ifndef __STM32F4_SD_H 22 | #define __STM32F4_SD_H 23 | 24 | #ifdef __cplusplus 25 | extern "C" { 26 | #endif 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "stm32f4xx_hal.h" 30 | #include "fatfs_platform.h" 31 | 32 | /* Exported types --------------------------------------------------------*/ 33 | /** 34 | * @brief SD Card information structure 35 | */ 36 | #define BSP_SD_CardInfo HAL_SD_CardInfoTypeDef 37 | 38 | /* Exported constants --------------------------------------------------------*/ 39 | /** 40 | * @brief SD status structure definition 41 | */ 42 | #define MSD_OK ((uint8_t)0x00) 43 | #define MSD_ERROR ((uint8_t)0x01) 44 | 45 | /** 46 | * @brief SD transfer state definition 47 | */ 48 | #define SD_TRANSFER_OK ((uint8_t)0x00) 49 | #define SD_TRANSFER_BUSY ((uint8_t)0x01) 50 | 51 | #define SD_PRESENT ((uint8_t)0x01) 52 | #define SD_NOT_PRESENT ((uint8_t)0x00) 53 | #define SD_DATATIMEOUT ((uint32_t)100000000) 54 | 55 | #ifdef OLD_API 56 | /* kept to avoid issue when migrating old projects. */ 57 | /* USER CODE BEGIN 0 */ 58 | 59 | /* USER CODE END 0 */ 60 | #else 61 | /* USER CODE BEGIN BSP_H_CODE */ 62 | /* Exported functions --------------------------------------------------------*/ 63 | uint8_t BSP_SD_Init(void); 64 | uint8_t BSP_SD_ITConfig(void); 65 | void BSP_SD_DetectIT(void); 66 | void BSP_SD_DetectCallback(void); 67 | uint8_t BSP_SD_ReadBlocks(uint32_t *pData, uint32_t ReadAddr, uint32_t NumOfBlocks, uint32_t Timeout); 68 | uint8_t BSP_SD_WriteBlocks(uint32_t *pData, uint32_t WriteAddr, uint32_t NumOfBlocks, uint32_t Timeout); 69 | uint8_t BSP_SD_ReadBlocks_DMA(uint32_t *pData, uint32_t ReadAddr, uint32_t NumOfBlocks); 70 | uint8_t BSP_SD_WriteBlocks_DMA(uint32_t *pData, uint32_t WriteAddr, uint32_t NumOfBlocks); 71 | uint8_t BSP_SD_Erase(uint32_t StartAddr, uint32_t EndAddr); 72 | void BSP_SD_IRQHandler(void); 73 | void BSP_SD_DMA_Tx_IRQHandler(void); 74 | void BSP_SD_DMA_Rx_IRQHandler(void); 75 | uint8_t BSP_SD_GetCardState(void); 76 | void BSP_SD_GetCardInfo(HAL_SD_CardInfoTypeDef *CardInfo); 77 | uint8_t BSP_SD_IsDetected(void); 78 | 79 | /* These functions can be modified in case the current settings (e.g. DMA stream) 80 | need to be changed for specific application needs */ 81 | void BSP_SD_AbortCallback(void); 82 | void BSP_SD_WriteCpltCallback(void); 83 | void BSP_SD_ReadCpltCallback(void); 84 | /* USER CODE END BSP_H_CODE */ 85 | #endif 86 | 87 | #ifdef __cplusplus 88 | } 89 | #endif 90 | 91 | #endif /* __STM32F4_SD_H */ 92 | -------------------------------------------------------------------------------- /FATFS/Target/fatfs_platform.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : fatfs_platform.c 5 | * @brief : fatfs_platform source file 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2021 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | /* USER CODE END Header */ 19 | #include "fatfs_platform.h" 20 | 21 | uint8_t BSP_PlatformIsDetected(void) { 22 | uint8_t status = SD_PRESENT; 23 | /* Check SD card detect pin */ 24 | if(HAL_GPIO_ReadPin(SD_DETECT_GPIO_PORT, SD_DETECT_PIN) != GPIO_PIN_RESET) 25 | { 26 | status = SD_NOT_PRESENT; 27 | } 28 | /* USER CODE BEGIN 1 */ 29 | /* user code can be inserted here */ 30 | /* USER CODE END 1 */ 31 | return status; 32 | } 33 | -------------------------------------------------------------------------------- /FATFS/Target/fatfs_platform.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : fatfs_platform.h 5 | * @brief : fatfs_platform header file 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2021 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | /* USER CODE END Header */ 19 | /* Includes ------------------------------------------------------------------*/ 20 | #include "stm32f4xx_hal.h" 21 | /* Defines ------------------------------------------------------------------*/ 22 | #define SD_PRESENT ((uint8_t)0x01) /* also in bsp_driver_sd.h */ 23 | #define SD_NOT_PRESENT ((uint8_t)0x00) /* also in bsp_driver_sd.h */ 24 | #define SD_DETECT_PIN GPIO_PIN_15 25 | #define SD_DETECT_GPIO_PORT GPIOE 26 | /* Prototypes ---------------------------------------------------------------*/ 27 | uint8_t BSP_PlatformIsDetected(void); 28 | -------------------------------------------------------------------------------- /FATFS/Target/sd_diskio.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file sd_diskio.h 5 | * @brief Header for sd_diskio.c module 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | *

© Copyright (c) 2020 STMicroelectronics. 10 | * All rights reserved.

11 | * 12 | * This software component is licensed by ST under Ultimate Liberty license 13 | * SLA0044, the "License"; You may not use this file except in compliance with 14 | * the License. You may obtain a copy of the License at: 15 | * www.st.com/SLA0044 16 | * 17 | ****************************************************************************** 18 | */ 19 | /* USER CODE END Header */ 20 | 21 | /* Note: code generation based on sd_diskio_dma_rtos_template.h */ 22 | 23 | /* Define to prevent recursive inclusion -------------------------------------*/ 24 | #ifndef __SD_DISKIO_H 25 | #define __SD_DISKIO_H 26 | 27 | /* USER CODE BEGIN firstSection */ 28 | /* can be used to modify / undefine following code or add new definitions */ 29 | /* USER CODE END firstSection */ 30 | 31 | /* Includes ------------------------------------------------------------------*/ 32 | #include "bsp_driver_sd.h" 33 | /* Exported types ------------------------------------------------------------*/ 34 | /* Exported constants --------------------------------------------------------*/ 35 | /* Exported functions ------------------------------------------------------- */ 36 | extern const Diskio_drvTypeDef SD_Driver; 37 | 38 | /* USER CODE BEGIN lastSection */ 39 | /* can be used to modify / undefine previous code or add new definitions */ 40 | /* USER CODE END lastSection */ 41 | 42 | #endif /* __SD_DISKIO_H */ 43 | -------------------------------------------------------------------------------- /Maincontroller/demo/demo_can.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * demo_can.cpp 3 | * 4 | * Created on: Aug 26, 2022 5 | * Author: 25053 6 | */ 7 | #include"maincontroller.h" 8 | 9 | void can_init(void){//在系统初始化的时候运行一次 10 | //首先配置CAN接收过滤器 11 | canFilterConfig->FilterBank = 14; 12 | canFilterConfig->FilterMode = CAN_FILTERMODE_IDMASK; 13 | canFilterConfig->FilterScale = CAN_FILTERSCALE_32BIT; 14 | canFilterConfig->FilterIdHigh = 0x0000; 15 | canFilterConfig->FilterIdLow = 0x0000; 16 | canFilterConfig->FilterMaskIdHigh = 0x0000; 17 | canFilterConfig->FilterMaskIdLow = 0x0000; 18 | canFilterConfig->FilterFIFOAssignment = CAN_FILTER_FIFO0; 19 | canFilterConfig->FilterActivation = ENABLE; 20 | canFilterConfig->SlaveStartFilterBank = 14; 21 | //配置can发送数据的header 22 | canTxHeader->StdId = 0x200; 23 | canTxHeader->RTR = CAN_RTR_DATA; 24 | canTxHeader->IDE = CAN_ID_STD; 25 | canTxHeader->DLC = 8; 26 | canTxHeader->TransmitGlobalTime = DISABLE; 27 | set_can_recieve_multifold(2);//设置CAN总线接收倍率,倍率数对应于开辟的接收缓存数 28 | set_comm3_as_can();//把串口3重置为fdcan 29 | } 30 | 31 | static uint32_t notify=0; 32 | void can_update(void){//以自己需要的频率循环运行即可 33 | //CAN数据的接收 34 | if(get_can_notification()!=notify){ 35 | notify=get_can_notification(); 36 | //获取接收到的can header,本demo直接打印了header中的id.由于开辟了两个接收缓存,所以调用的时候需要指明缓存编号为0和1 37 | usb_printf("id:%x|%x|%d\n",get_canRxHeader_prt(0)->StdId, get_canRxHeader_prt(1)->StdId, notify); 38 | //获取接收到的can data,打印了缓存0接收的8字节数据 39 | usb_printf("data:%d|%d|%d|%d|%d|%d|%d|%d\n",get_canRxData_prt(0)[0],get_canRxData_prt(0)[1],get_canRxData_prt(0)[2],get_canRxData_prt(0)[3],get_canRxData_prt(0)[4],get_canRxData_prt(0)[5],get_canRxData_prt(0)[6],get_canRxData_prt(0)[7]); 40 | //获取接收到的can data,打印了缓存1接收的8字节数据 41 | usb_printf("data:%d|%d|%d|%d|%d|%d|%d|%d\n",get_canRxData_prt(1)[0],get_canRxData_prt(1)[1],get_canRxData_prt(1)[2],get_canRxData_prt(1)[3],get_canRxData_prt(1)[4],get_canRxData_prt(1)[5],get_canRxData_prt(1)[6],get_canRxData_prt(1)[7]); 42 | } 43 | //CAN数据的发送 44 | canTxHeader->StdId = 0x200;;//配置发送数据id 45 | canTxData[0]=5000>>8; 46 | canTxData[1]=5000&0xFF; 47 | canTxData[2]=0; 48 | canTxData[3]=0; 49 | canTxData[4]=0; 50 | canTxData[5]=0; 51 | canTxData[6]=0; 52 | canTxData[7]=0; 53 | can_send_data();//demo演示了发送8字节数据 54 | } 55 | 56 | 57 | -------------------------------------------------------------------------------- /Maincontroller/demo/demo_ekf.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * demo_ekf.cpp 3 | * 4 | * Created on: 2022年6月18日 5 | * Author: 10427 6 | */ 7 | #include"maincontroller.h" 8 | 9 | //Q表示观测数据的方差;R1表示预测数据的位置的方差;R2表示预测数据速度的方差 10 | EKF_Baro *ekf_baro_1=new EKF_Baro(_dt, 0.0016, 1.0, 0.000016, 0.000016); 11 | 12 | //Q表示观测数据的方差;R1表示预测数据的位置的方差;R2表示预测数据速度的方差 13 | EKF_Rangefinder *ekf_rangefinder_1=new EKF_Rangefinder(_dt, 1.0, 0.000016, 0.16); 14 | 15 | /*Q1表示观测数据的位置的x轴方差;Q2表示观测数据的位置的y轴方差; 16 | R1表示预测数据的位置的x轴方差;R2表示预测数据的速度的x轴方差;R3表示预测数据的位置的y轴方差;R4表示预测数据的速度的y轴方差*/ 17 | EKF_Odometry *ekf_odometry_1=new EKF_Odometry(_dt, 0.0016, 0.0016, 0.000016, 0.00016, 0.000016, 0.00016); 18 | 19 | /*Q1表示观测数据的位置的x轴方差;Q2表示观测数据的速度的x轴方差;Q3表示观测数据的位置的y轴方差;Q4表示观测数据的速度的y轴方差;观测数据就是GPS的测量值 20 | R1表示预测数据的位置的x轴方差;R2表示预测数据的速度的x轴方差;R3表示预测数据的位置的y轴方差;R4表示预测数据的速度的y轴方差;预测数据跟加速度测量值相关*/ 21 | EKF_GNSS *ekf_gnss_1=new EKF_GNSS(_dt, 1.0, 1.0, 1.0, 1.0, 0.0000001, 0.001, 0.0000001, 0.001); 22 | 23 | bool get_baro_alt_filt = false; 24 | bool get_gnss_location = false; 25 | bool get_odom_xy = false; 26 | bool get_rangefinder_data = false; 27 | 28 | void ekf_baro_demo() 29 | { 30 | ekf_baro_1->update(get_baro_alt_filt, get_baroalt_filt()); 31 | } 32 | void ekf_gnss_demo() 33 | { 34 | ekf_gnss_1->update(get_gnss_location,get_ned_pos_x(),get_ned_pos_y(),get_ned_vel_x(),get_ned_vel_y()); 35 | } 36 | 37 | void ekf_odometry_demo() 38 | { 39 | ekf_odometry_1->update(get_odom_xy,get_odom_x(),get_odom_y()); 40 | } 41 | 42 | void ekf_rf_demo() 43 | { 44 | ekf_rangefinder_1->update(get_rangefinder_data, get_rangefinder_alt()); 45 | } 46 | -------------------------------------------------------------------------------- /Maincontroller/demo/demo_filter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * demo_filter.cpp 3 | * 4 | * Created on: 2022年6月16日 5 | * Author: 10427 6 | */ 7 | #include "maincontroller.h" 8 | 9 | LowPassFilterFloat throttle; //这里以float类型为例说明,可根据不同需求选择int类型,long类型,Vector2f类型以及Vector3f类型。 10 | LowPassFilter2pFloat throttle_2p; 11 | 12 | float throttle_in,throtte_out; 13 | float throttle_in_2p, throttle_out_2p; 14 | 15 | bool filter_init() // 运行一次,设置滤波器的采样频率以及截止频率。 16 | { 17 | throttle.set_cutoff_frequency(400, 1); //两个参数分别为采样频率(Hz),截至频率(Hz)。采样频率的设置决定demo函数的循环频率。 18 | throttle.reset(0); 19 | return true; 20 | } 21 | bool filter_2p_init() // 运行一次,设置滤波器的采样频率以及截止频率。 22 | { 23 | throttle_2p.set_cutoff_frequency(400, 1);//两个参数分别为采样频率(Hz),截至频率(Hz)。采样频率的设置决定demo函数的循环频率。 24 | throttle_2p.reset(); 25 | return true; 26 | } 27 | void filter_demo() //循环运行,输入是采样值,输出是滤波后的油门。循环频率与采样频率一致。 28 | { 29 | throtte_out = throttle.apply(throttle_in); 30 | } 31 | 32 | void filter_2p_demo() //循环运行,输入是采样值,输出是滤波后的油门。循环频率与采样频率一致。 33 | { 34 | throttle_out_2p = throttle_2p.apply(throttle_in_2p); 35 | } 36 | 37 | 38 | -------------------------------------------------------------------------------- /Maincontroller/demo/demo_pid.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * demo_pid.cpp 3 | * 4 | * Created on: 2022年6月16日 5 | * Author: 10427 6 | */ 7 | #include "maincontroller.h" 8 | 9 | PID *pid = new PID(0.1, 0.1, 0.1, 1, 20, 0.0025); //定义PID对象 参数分别是p;i;d;积分结果最大值;输入error的低通滤波截止频率(Hz);pid运行周期(s) 10 | PID *accel_pid = new PID(0.1, 0.1, 0.1, 1, 20, 0.0025); 11 | P *p_pos = new P(0.1);// 定义P对象 参数为p 12 | P *v_pos = new P(0.1); 13 | PID_2D *pid_2d = new PID_2D(0.1,0.1,0.2,0.2,0.3,0.3,1,20,30,0.0025); 14 | 15 | float demo_pid_single(float target, float current) //单环pid 16 | { 17 | float error = target - current; //误差 = 目标值-当前值 18 | 19 | pid->set_input_filter_all(error); 20 | 21 | return pid->get_p() + pid->get_i() + pid->get_d(); 22 | 23 | } 24 | 25 | float demo_pid_poscontrol(float target, float current1, float current2, float current3) //串级pid(3环) 26 | { 27 | float error1 = target - current1;//最外环 28 | float p_result1 = p_pos->get_p(error1); 29 | 30 | 31 | float error2 = p_result1 - current2;//中间环 32 | float p_result2 = v_pos->get_p(error2); 33 | 34 | float error3 = p_result2 - current3;// 最内环 35 | accel_pid->set_input_filter_all(error3); 36 | 37 | return accel_pid->get_pid(); 38 | 39 | } 40 | 41 | Vector2f pid_2d_demo(Vector2f &target, Vector2f ¤t) //二维pid 42 | { 43 | Vector2f error; 44 | error.x = target.x - current.x; 45 | error.y = target.y - current.y; 46 | 47 | pid_2d->set_input(error); 48 | 49 | return pid_2d->get_pid(); 50 | 51 | } 52 | -------------------------------------------------------------------------------- /Maincontroller/inc/maincontroller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * maincontroller.h 3 | * 4 | * Created on: Aug 28, 2021 5 | * Author: 25053 6 | */ 7 | #pragma once 8 | 9 | #ifndef INC_MAINCONTROLLER_H_ 10 | #define INC_MAINCONTROLLER_H_ 11 | 12 | #include "ahrs/ahrs.h" 13 | #include "ekf/ekf_baro.h" 14 | #include "ekf/ekf_rangefinder.h" 15 | #include "ekf/ekf_odometry.h" 16 | #include "ekf/ekf_gnss.h" 17 | #include "position/position.h" 18 | #include "compass/compassCalibrator.h" 19 | #include "compass/declination.h" 20 | #include "accel/accelCalibrator.h" 21 | #include "flash/flash.h" 22 | #include "sdlog/sdlog.h" 23 | #include "uwb/uwb.h" 24 | #include "common.h" 25 | 26 | extern ap_t *ap; 27 | extern AHRS *ahrs; 28 | extern EKF_Baro *ekf_baro; 29 | extern EKF_Rangefinder *ekf_rangefinder; 30 | extern EKF_Odometry *ekf_odometry; 31 | extern EKF_GNSS *ekf_gnss; 32 | extern Motors *motors; 33 | extern Attitude_Multi *attitude; 34 | extern PosControl *pos_control; 35 | extern CompassCalibrator *compassCalibrator; 36 | extern AccelCalibrator *accelCalibrator; 37 | extern DataFlash *dataflash; 38 | extern SDLog *sdlog; 39 | 40 | const float _dt=0.0025; 41 | void send_mavlink_param_list(mavlink_channel_t chan); 42 | void send_mavlink_mission_ack(mavlink_channel_t chan, MAV_MISSION_RESULT result); 43 | void send_mavlink_mission_item_reached(mavlink_channel_t chan, uint16_t seq); 44 | void send_mavlink_mission_count(mavlink_channel_t chan); 45 | void send_mavlink_mission_list(mavlink_channel_t chan); 46 | void send_mavlink_commond_ack(mavlink_channel_t chan, MAV_CMD mav_cmd, MAV_CMD_ACK result); 47 | float get_non_takeoff_throttle(void); 48 | void zero_throttle_and_relax_ac(void); 49 | void set_land_complete(bool b); 50 | void get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit); 51 | void update_air_resistance(void); 52 | void get_air_resistance_lean_angles(float &roll_d, float &pitch_d, float angle_max, float gain); 53 | float get_pilot_desired_yaw_rate(float stick_angle); 54 | float get_pilot_desired_throttle(float throttle_control, float thr_mid); 55 | float get_pilot_desired_climb_rate(float throttle_control); 56 | void set_return(bool set); 57 | bool get_return(void); 58 | void set_takeoff(void); 59 | bool get_takeoff(void); 60 | bool takeoff_running(void); 61 | void takeoff_stop(void); 62 | bool takeoff_triggered( float target_climb_rate); 63 | void takeoff_start(float alt_cm); 64 | void set_throttle_takeoff(void); 65 | void get_takeoff_climb_rates(float& pilot_climb_rate, float& takeoff_climb_rate); 66 | void set_target_rangefinder_alt(float alt_target); 67 | float get_surface_tracking_climb_rate(float target_rate, float current_alt_target, float dt); 68 | void motors_test_update(void); 69 | 70 | #endif /* INC_MAINCONTROLLER_H_ */ 71 | -------------------------------------------------------------------------------- /Maincontroller/src/common.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * common.cpp 3 | * 4 | * Created on: 2020.07.16 5 | * Author: JackyPan 6 | */ 7 | #include "maincontroller.h" 8 | 9 | static bool _soft_armed=false;//心跳包中表示是否解锁的标志位 10 | static bool _thr_force_decrease=false;//强制油门下降 11 | ROBOT_STATE robot_state=STATE_NONE; 12 | ROBOT_STATE robot_state_desired=STATE_NONE; 13 | ROBOT_MAIN_MODE robot_main_mode=MODE_AIR; 14 | ROBOT_SUB_MODE robot_sub_mode=MODE_STABILIZE; 15 | 16 | bool get_soft_armed(void){ 17 | return _soft_armed; 18 | } 19 | 20 | void set_soft_armed(bool soft_armed){ 21 | _soft_armed=soft_armed; 22 | } 23 | 24 | bool get_thr_force_decrease(void){ 25 | return _thr_force_decrease; 26 | } 27 | 28 | void set_thr_force_decrease(bool force_decrease){ 29 | _thr_force_decrease=force_decrease; 30 | } 31 | 32 | bool mode_init(void){ 33 | return mode_stabilize_init(); 34 | } 35 | 36 | /* 模式切换说明: 37 | * (1) 模式切换分为主模式和子模式, 其中主模式由app控制, 子模式隶属于主模式由channel5控制; 38 | * (2) 在空中模式下: 39 | * channel5>0.7为自稳模式; 40 | * 0.7>=channel5>0.3为定高模式; 41 | * 0.3>=channel5为定点模式; 42 | * (3) 在无人车模式下: 43 | * channel5>0.7为遥控直通模式; 44 | * 0.7>=channel5>0.3为无人车定速模式; 45 | * 0.3>=channel5为无人车定点模式; 46 | * */ 47 | //should be run at 400hz 48 | void mode_update(void){ 49 | float ch5=get_channel_5();//用于机器人子模态切换 50 | switch(robot_main_mode){ 51 | case MODE_AIR: 52 | if(ch5>0.7){ 53 | if(robot_sub_mode!=MODE_STABILIZE){ 54 | if(mode_stabilize_init()){ 55 | robot_sub_mode=MODE_STABILIZE; 56 | } 57 | } 58 | }else if(ch5>0.3){ 59 | if(robot_sub_mode!=MODE_ALTHOLD){ 60 | if(mode_althold_init()){ 61 | robot_sub_mode=MODE_ALTHOLD; 62 | } 63 | } 64 | }else{ 65 | if(robot_sub_mode!=MODE_POSHOLD){ 66 | if(mode_poshold_init()){ 67 | robot_sub_mode=MODE_POSHOLD; 68 | } 69 | } 70 | } 71 | break; 72 | case MODE_MECANUM: 73 | if(robot_sub_mode!=MODE_MECANUM_A){ 74 | if(mode_mecanum_init()){ 75 | robot_sub_mode=MODE_MECANUM_A; 76 | } 77 | } 78 | break; 79 | case MODE_SPIDER: 80 | robot_state=STATE_CLIMB; 81 | break; 82 | case MODE_UGV: 83 | robot_state=STATE_DRIVE; 84 | if(ch5>0.7){ 85 | //无人车遥控直通模式 86 | if(robot_sub_mode!=MODE_UGV_A){ 87 | //init() 88 | robot_sub_mode=MODE_UGV_A; 89 | } 90 | }else if(ch5>0.3){ 91 | //无人车定速模式 92 | if(robot_sub_mode!=MODE_UGV_V){ 93 | //init() 94 | robot_sub_mode=MODE_UGV_V; 95 | } 96 | }else{ 97 | //无人车定点模式 98 | if(robot_sub_mode!=MODE_UGV_P){ 99 | mode_ugv_p_init(); 100 | robot_sub_mode=MODE_UGV_P; 101 | } 102 | } 103 | break; 104 | default: 105 | break; 106 | } 107 | switch(robot_sub_mode){ 108 | case MODE_STABILIZE: 109 | mode_stabilize(); 110 | break; 111 | case MODE_ALTHOLD: 112 | mode_althold(); 113 | break; 114 | case MODE_POSHOLD: 115 | mode_poshold(); 116 | break; 117 | case MODE_AUTONAV: 118 | mode_autonav(); 119 | break; 120 | case MODE_PERCH: 121 | mode_perch(); 122 | break; 123 | case MODE_MECANUM_A: 124 | mode_mecanum(); 125 | break; 126 | case MODE_UGV_A: 127 | //无人车遥控直通update() 128 | mode_ugv_a(); 129 | break; 130 | case MODE_UGV_V: 131 | //无人车定速模式update() 132 | mode_ugv_v(); 133 | break; 134 | case MODE_UGV_P: 135 | //无人车定点模式update() 136 | mode_ugv_p(); 137 | break; 138 | default: 139 | break; 140 | } 141 | } 142 | -------------------------------------------------------------------------------- /Maincontroller/src/mode_mecanum.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mode_mecanum.cpp 3 | * 4 | * Created on: 2022年4月10日 5 | * Author: 10427 6 | */ 7 | 8 | #include "maincontroller.h" 9 | 10 | static float channel_roll = 0.0; //滚转角通道控制左右 y 11 | static float channel_yaw = 0.0; //偏航角控制旋转 rotate 12 | static float channel_pitch = 0.0; //俯仰角通道控制前后 x 13 | static float channel_throttle=0.0;//油门通道控制是否启动电机 14 | 15 | bool mode_mecanum_init(void) 16 | { 17 | if(motors->get_armed()||motors->get_interlock()){//电机未锁定,禁止切换至该模式 18 | Buzzer_set_ring_type(BUZZER_ERROR); 19 | return false; 20 | } 21 | channel_roll = 0.0; 22 | channel_yaw = 0.0; 23 | channel_pitch = 0.0; 24 | channel_throttle=0.0; 25 | Buzzer_set_ring_type(BUZZER_MODE_SWITCH); 26 | usb_printf("switch mode mecanum!\n"); 27 | return true; 28 | } 29 | 30 | void mode_mecanum(void) 31 | { 32 | robot_state=STATE_DRIVE; 33 | //(1)首先获取遥控器通道值 34 | channel_roll = get_channel_roll(); //range(-1,1) 35 | channel_yaw = get_channel_yaw(); //range(-1,1) 36 | channel_pitch = get_channel_pitch(); //range(-1,1) 37 | channel_throttle=get_channel_throttle(); //range(0-1) 38 | 39 | //(2)对遥控器通道值进行混控 40 | float left_head = channel_pitch+channel_roll+channel_yaw; 41 | float right_head = -channel_pitch+channel_roll+channel_yaw; 42 | float left_tail = -channel_pitch+channel_roll-channel_yaw; 43 | float right_tail = channel_pitch+channel_roll-channel_yaw; 44 | 45 | float max = abs(left_head);//取出最大值 46 | if (max < abs(right_head)){max = abs(right_head);} 47 | if (max < abs(left_tail)){max = abs(left_tail);} 48 | if (max < abs(right_tail)){max = abs(right_tail);} 49 | 50 | //(3)将混控值映射至PWM脉宽输出给电机 51 | if(get_soft_armed()&&(channel_throttle>0.1)){//解锁后,油门推起来才允许电机输出 52 | //电机1左前,用m1口控制电机1速度,用gpio1控制电机正反转 53 | if (left_head >= 0) 54 | { 55 | write_gpio1(true);//用gpio口控制电机1正反转 56 | Motor_Set_Value(1, left_head / max * PWM_BRUSH_MAX);//用m1 PWM口控制电机1转速 57 | } 58 | else 59 | { 60 | write_gpio1(false); 61 | Motor_Set_Value(1, abs(left_head) / max * PWM_BRUSH_MAX); 62 | } 63 | 64 | //电机2右前,用m5口控制电机2速度,用gpio5控制电机正反转 65 | if (right_head >= 0) 66 | { 67 | write_gpio5(true); 68 | Motor_Set_Value(5, right_head / max * PWM_BRUSH_MAX); 69 | } 70 | else 71 | { 72 | write_gpio5(false); 73 | Motor_Set_Value(5, abs(right_head) / max * PWM_BRUSH_MAX); 74 | } 75 | 76 | //电机3左后,用m2口控制电机3速度,用gpio2控制电机正反转 77 | if (left_tail >= 0) 78 | { 79 | write_gpio2(true); 80 | Motor_Set_Value(2, left_tail / max * PWM_BRUSH_MAX); 81 | } 82 | else 83 | { 84 | write_gpio2(false); 85 | Motor_Set_Value(2, abs(left_tail) / max * PWM_BRUSH_MAX); 86 | } 87 | 88 | //电机4右后,用m6口控制电机4速度,用gpio6控制电机正反转 89 | if (right_tail >= 0) 90 | { 91 | write_gpio6(true); 92 | Motor_Set_Value(6, right_tail / max * PWM_BRUSH_MAX); 93 | } 94 | else 95 | { 96 | write_gpio6(false); 97 | Motor_Set_Value(6, abs(right_tail) / max * PWM_BRUSH_MAX); 98 | } 99 | }else{//未解锁时电机无输出 100 | Motor_Set_Value(1, PWM_BRUSH_MIN); 101 | Motor_Set_Value(2, PWM_BRUSH_MIN); 102 | Motor_Set_Value(5, PWM_BRUSH_MIN); 103 | Motor_Set_Value(6, PWM_BRUSH_MIN); 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /Maincontroller/src/mode_stabilize.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mode_stabilize.cpp 3 | * 4 | * Created on: 2021年8月26日 5 | * Author: 25053 6 | */ 7 | #include "maincontroller.h" 8 | 9 | static float target_yaw=0.0f; 10 | bool mode_stabilize_init(void){ 11 | // if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high 12 | if (motors->get_armed() && ap->land_complete && !has_manual_throttle() && 13 | (get_pilot_desired_throttle(get_channel_throttle(), 0.0f) > get_non_takeoff_throttle())) { 14 | Buzzer_set_ring_type(BUZZER_ERROR); 15 | return false; 16 | } 17 | // set target altitude to zero for reporting 18 | pos_control->set_alt_target(0); 19 | set_manual_throttle(true);//设置为手动油门 20 | target_yaw=ahrs_yaw_deg(); 21 | Buzzer_set_ring_type(BUZZER_MODE_SWITCH); 22 | usb_printf("switch mode stabilize!\n"); 23 | return true; 24 | } 25 | 26 | static int16_t esc_counter=0, esc_delay=0; 27 | void mode_stabilize(void){ 28 | float target_roll, target_pitch; 29 | float target_yaw_rate; 30 | float pilot_throttle_scaled; 31 | 32 | // if not armed set throttle to zero and exit immediately 33 | if (!motors->get_armed() || ap->throttle_zero || !motors->get_interlock()) { 34 | zero_throttle_and_relax_ac(); 35 | attitude->rate_controller_run(); 36 | target_yaw=ahrs_yaw_deg(); 37 | motors->output(); 38 | robot_state=STATE_STOP; 39 | motors_test_update(); 40 | /* 41 | * 以下为电调校准模式说明: 42 | * (1) 在电机锁定状态下,首先进行硬件解锁(注意不要进行手势解锁), 硬件解锁后Mcontroller右侧绿色指示灯长亮; 43 | * (2) 将油门推到最高, 偏航推到最左, 持续5s进入电调校准模式; 44 | * (3) 进入电调校准模式后提示音“嘟嘟嘟...”响起。此时将偏航回中, Mcontroller M1~M8插口会产生PWM输出; 45 | * (4) 在电调校准模式中, Mcontroller M1~M8口输出的PWM波脉宽直接由油门推杆控制, 即最大油门对应最大脉宽, 最小油门对应最小脉宽。 46 | * (5) 电调校准模式默认持续时间为50s, 即进入电调校准模式50s后自动退出电调校准模式; 47 | * (6) 在电调校准模式中, 将偏航推到最右可以立即退出电调校准模式; 48 | * */ 49 | float throttle=get_channel_throttle(); 50 | float tmp = get_channel_yaw(); 51 | if (tmp < -0.9) { //full left 52 | // wait for 5s to enter esc correct mode 53 | if( throttle > 0.9 && esc_counter < 2000) { 54 | esc_counter++; 55 | } 56 | if (esc_counter == 2000) { 57 | esc_delay=20000; //设置电调校准持续时间为50s, 50s后自动退出电调校准模式 58 | Buzzer_set_ring_type(BUZZER_ESC); 59 | } 60 | }else if(tmp>0.5){ // Yaw is right to reset esc counter 61 | esc_counter = 0; 62 | }else{ // Yaw is centered to delay 20s to reset esc counter 63 | // correct the esc 64 | if (esc_counter == 2000) { 65 | Buzzer_set_ring_type(BUZZER_ESC); 66 | motors->set_throttle_passthrough_for_motors(throttle);//只校准当前机型使能的电机 67 | } 68 | if(esc_delay>0){ 69 | esc_delay--; 70 | }else{ 71 | esc_counter = 0; 72 | esc_delay = 0; 73 | } 74 | } 75 | return; 76 | } 77 | robot_state=STATE_FLYING; 78 | // clear landing flag 79 | set_land_complete(false); 80 | 81 | motors->set_desired_spool_state(Motors::DESIRED_THROTTLE_UNLIMITED); 82 | 83 | // convert pilot input to lean angles 84 | get_pilot_desired_lean_angles(target_roll, target_pitch, param->angle_max.value, param->angle_max.value); 85 | 86 | // get pilot's desired yaw rate 87 | target_yaw_rate = get_pilot_desired_yaw_rate(get_channel_yaw_angle()); 88 | 89 | // get pilot's desired throttle 90 | pilot_throttle_scaled = get_pilot_desired_throttle(get_channel_throttle(),0.0f); 91 | 92 | // call attitude controller 93 | target_yaw+=target_yaw_rate*_dt; 94 | attitude->input_euler_angle_roll_pitch_yaw(target_roll, target_pitch, target_yaw, true); 95 | 96 | // output pilot's throttle 97 | attitude->set_throttle_out(pilot_throttle_scaled, true, param->throttle_filt.value); 98 | 99 | attitude->rate_controller_run(); 100 | motors->output(); 101 | } 102 | -------------------------------------------------------------------------------- /Maincontroller/src/mode_ugv_a.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mode_ugv_a.cpp 3 | * 4 | * Created on: 2022年9月25日 5 | * Author: 25053 6 | */ 7 | #include "maincontroller.h" 8 | 9 | static float channel_roll = 0.0; //滚转角通道控制左右 y 10 | static float channel_yaw = 0.0; //偏航角控制旋转 rotate 11 | static float channel_pitch = 0.0; //俯仰角通道控制前后 x 12 | static float channel_throttle=0.0;//油门通道控制是否启动电机 13 | void mode_ugv_a(void) 14 | { 15 | robot_state=STATE_DRIVE; 16 | //(1)首先获取遥控器通道值 17 | channel_roll = get_channel_roll(); //range(-1,1) 18 | channel_yaw = get_channel_yaw(); //range(-1,1) 19 | channel_pitch = get_channel_pitch(); //range(-1,1) 20 | channel_throttle=get_channel_throttle(); //range(0-1) 21 | 22 | //(2)输出 23 | if(get_soft_armed()&&(channel_throttle>0.1)){//解锁后,油门推起来才允许电机输出 24 | //前进后退 25 | if(channel_pitch>0.9){ 26 | write_gpio1(true);//前进 27 | write_gpio2(false); 28 | }else if(channel_pitch<-0.9){ 29 | write_gpio1(false);//后退 30 | write_gpio2(true); 31 | }else{ 32 | write_gpio1(false);//停止 33 | write_gpio2(false); 34 | } 35 | 36 | //左转右转 37 | if(channel_roll>0.9){ 38 | write_gpio3(true);//右转 39 | write_gpio4(false); 40 | }else if(channel_roll<-0.9){ 41 | write_gpio3(false);//左转 42 | write_gpio4(true); 43 | }else{ 44 | write_gpio3(false);//停止 45 | write_gpio4(false); 46 | } 47 | }else{//未解锁时电机无输出 48 | write_gpio1(false);//停止 49 | write_gpio2(false); 50 | write_gpio3(false);//停止 51 | write_gpio4(false); 52 | } 53 | } 54 | 55 | 56 | -------------------------------------------------------------------------------- /Maincontroller/src/mode_ugv_v.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mode_ugv_v.cpp 3 | * 4 | * Created on: 2022年9月27日 5 | * Author: 25053 6 | */ 7 | #include "maincontroller.h" 8 | 9 | static float channel_roll = 0.0; //滚转角通道控制左右 y 10 | static float channel_yaw = 0.0; //偏航角控制旋转 rotate 11 | static float channel_pitch = 0.0; //俯仰角通道控制前后 x 12 | static float channel_throttle=0.0;//油门通道控制是否启动电机 13 | static float target_yaw=0.0f; 14 | static uint16_t target_point=0; 15 | static Location gnss_target_pos; 16 | static Vector3f ned_target_pos; 17 | static Vector2f ned_dis_2d; 18 | void mode_ugv_v(void) 19 | { 20 | robot_state=STATE_DRIVE; 21 | //(1)首先获取遥控器通道值 22 | channel_roll = get_channel_roll(); //range(-1,1) 23 | channel_yaw = get_channel_yaw(); //range(-1,1) 24 | channel_pitch = get_channel_pitch(); //range(-1,1) 25 | channel_throttle=get_channel_throttle(); //range(0-1) 26 | 27 | //(2)输出 28 | if(get_soft_armed()&&(channel_throttle>0.1)){//解锁后,油门推起来才允许电机输出 29 | if(sdlog->gnss_point_num>0){ 30 | if(target_pointgnss_point_num){ 31 | gnss_target_pos.lat=(int32_t)(sdlog->gnss_point[target_point].x*1e7); 32 | gnss_target_pos.lng=(int32_t)(sdlog->gnss_point[target_point].y*1e7); 33 | ned_target_pos=location_3d_diff_NED(get_gnss_origin_pos(), gnss_target_pos)*100;//cm 34 | ned_dis_2d.x=ned_target_pos.x-get_pos_x(); 35 | ned_dis_2d.y=ned_target_pos.y-get_pos_y(); 36 | if(ned_dis_2d.length()<100){//距离目标点小于1m认为到达 37 | target_point++; 38 | }else{ 39 | if(ned_dis_2d.y>=0){ 40 | target_yaw=acosf(ned_dis_2d.x/ned_dis_2d.length())/M_PI*180; 41 | }else{ 42 | target_yaw=-acosf(ned_dis_2d.x/ned_dis_2d.length())/M_PI*180; 43 | } 44 | } 45 | } 46 | if(target_point>sdlog->gnss_point_num){ 47 | write_gpio1(false);//停止 48 | write_gpio2(false); 49 | write_gpio3(false);//停止 50 | write_gpio4(false); 51 | }else{ 52 | float delta_yaw=wrap_180(target_yaw-ahrs_yaw_deg(), 1); 53 | usb_printf("yaw:%f|%f|%f\n",delta_yaw, target_yaw, ahrs_yaw_deg()); 54 | if(delta_yaw>=0&&delta_yaw<=90){ 55 | write_gpio1(true);//前进 56 | write_gpio2(false); 57 | write_gpio3(true);//右转 58 | write_gpio4(false); 59 | }else if(delta_yaw<0&&delta_yaw>=-90){ 60 | write_gpio1(true);//前进 61 | write_gpio2(false); 62 | write_gpio3(false);//左转 63 | write_gpio4(true); 64 | }else if(delta_yaw<-90&&delta_yaw>-180){ 65 | write_gpio1(false);//后退 66 | write_gpio2(true); 67 | write_gpio3(false);//左转 68 | write_gpio4(true); 69 | }else if(delta_yaw>90&&delta_yaw<=180){ 70 | write_gpio1(false);//后退 71 | write_gpio2(true); 72 | write_gpio3(true);//右转 73 | write_gpio4(false); 74 | } 75 | } 76 | }else{ 77 | write_gpio1(false);//停止 78 | write_gpio2(false); 79 | write_gpio3(false);//停止 80 | write_gpio4(false); 81 | } 82 | }else{ 83 | write_gpio1(false);//停止 84 | write_gpio2(false); 85 | write_gpio3(false);//停止 86 | write_gpio4(false); 87 | } 88 | } 89 | 90 | 91 | -------------------------------------------------------------------------------- /Maincontroller/src/mode_usv_a.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mode_usv_a.cpp 3 | * 4 | * Created on: 2023年5月19日 5 | * Author: 25053 6 | */ 7 | #include "maincontroller.h" 8 | 9 | static float channel_roll = 0.0; //滚转角通道控制左右 y 10 | static float channel_pitch = 0.0; //俯仰角通道控制前后 x 11 | static float channel_throttle=0.0;//油门通道控制是否启动电机 12 | void mode_usv_a(void) 13 | { 14 | robot_state=STATE_DRIVE; 15 | //(1)首先获取遥控器通道值 16 | channel_roll = get_channel_roll(); //range(-1,1) 17 | channel_pitch = get_channel_pitch(); //range(-1,1) 18 | channel_throttle=get_channel_throttle(); //range(0-1) 19 | 20 | //(2)输出 21 | if(get_soft_armed()&&(channel_throttle>0.1)){//解锁后,油门推起来才允许电机输出 22 | Motor_Set_Value(1, 1500+500*constrain_float(-channel_pitch+channel_roll, -1.0, 1.0)); 23 | Motor_Set_Value(5, 1500+500*constrain_float(-channel_pitch-channel_roll, -1.0, 1.0)); 24 | }else{//未解锁时电机无输出 25 | Motor_Set_Value(1, 1500); 26 | Motor_Set_Value(5, 1500); 27 | } 28 | } 29 | 30 | 31 | 32 | -------------------------------------------------------------------------------- /Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ctlreq.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_req.h 4 | * @author MCD Application Team 5 | * @brief Header file for the usbd_req.c file 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2015 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __USB_REQUEST_H 21 | #define __USB_REQUEST_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "usbd_def.h" 29 | 30 | 31 | /** @addtogroup STM32_USB_DEVICE_LIBRARY 32 | * @{ 33 | */ 34 | 35 | /** @defgroup USBD_REQ 36 | * @brief header file for the usbd_req.c file 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USBD_REQ_Exported_Defines 41 | * @{ 42 | */ 43 | /** 44 | * @} 45 | */ 46 | 47 | 48 | /** @defgroup USBD_REQ_Exported_Types 49 | * @{ 50 | */ 51 | /** 52 | * @} 53 | */ 54 | 55 | 56 | 57 | /** @defgroup USBD_REQ_Exported_Macros 58 | * @{ 59 | */ 60 | /** 61 | * @} 62 | */ 63 | 64 | /** @defgroup USBD_REQ_Exported_Variables 65 | * @{ 66 | */ 67 | /** 68 | * @} 69 | */ 70 | 71 | /** @defgroup USBD_REQ_Exported_FunctionsPrototype 72 | * @{ 73 | */ 74 | 75 | USBD_StatusTypeDef USBD_StdDevReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); 76 | USBD_StatusTypeDef USBD_StdItfReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); 77 | USBD_StatusTypeDef USBD_StdEPReq(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); 78 | 79 | void USBD_CtlError(USBD_HandleTypeDef *pdev, USBD_SetupReqTypedef *req); 80 | void USBD_ParseSetupRequest(USBD_SetupReqTypedef *req, uint8_t *pdata); 81 | void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len); 82 | 83 | /** 84 | * @} 85 | */ 86 | 87 | #ifdef __cplusplus 88 | } 89 | #endif 90 | 91 | #endif /* __USB_REQUEST_H */ 92 | 93 | /** 94 | * @} 95 | */ 96 | 97 | /** 98 | * @} 99 | */ 100 | 101 | 102 | -------------------------------------------------------------------------------- /Middlewares/ST/STM32_USB_Device_Library/Core/Inc/usbd_ioreq.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_ioreq.h 4 | * @author MCD Application Team 5 | * @brief Header file for the usbd_ioreq.c file 6 | ****************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2015 STMicroelectronics. 10 | * All rights reserved. 11 | * 12 | * This software is licensed under terms that can be found in the LICENSE file 13 | * in the root directory of this software component. 14 | * If no LICENSE file comes with this software, it is provided AS-IS. 15 | * 16 | ****************************************************************************** 17 | */ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __USBD_IOREQ_H 21 | #define __USBD_IOREQ_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "usbd_def.h" 29 | #include "usbd_core.h" 30 | 31 | /** @addtogroup STM32_USB_DEVICE_LIBRARY 32 | * @{ 33 | */ 34 | 35 | /** @defgroup USBD_IOREQ 36 | * @brief header file for the usbd_ioreq.c file 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USBD_IOREQ_Exported_Defines 41 | * @{ 42 | */ 43 | /** 44 | * @} 45 | */ 46 | 47 | 48 | /** @defgroup USBD_IOREQ_Exported_Types 49 | * @{ 50 | */ 51 | 52 | 53 | /** 54 | * @} 55 | */ 56 | 57 | 58 | 59 | /** @defgroup USBD_IOREQ_Exported_Macros 60 | * @{ 61 | */ 62 | 63 | /** 64 | * @} 65 | */ 66 | 67 | /** @defgroup USBD_IOREQ_Exported_Variables 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup USBD_IOREQ_Exported_FunctionsPrototype 76 | * @{ 77 | */ 78 | 79 | USBD_StatusTypeDef USBD_CtlSendData(USBD_HandleTypeDef *pdev, 80 | uint8_t *pbuf, uint32_t len); 81 | 82 | USBD_StatusTypeDef USBD_CtlContinueSendData(USBD_HandleTypeDef *pdev, 83 | uint8_t *pbuf, uint32_t len); 84 | 85 | USBD_StatusTypeDef USBD_CtlPrepareRx(USBD_HandleTypeDef *pdev, 86 | uint8_t *pbuf, uint32_t len); 87 | 88 | USBD_StatusTypeDef USBD_CtlContinueRx(USBD_HandleTypeDef *pdev, 89 | uint8_t *pbuf, uint32_t len); 90 | 91 | USBD_StatusTypeDef USBD_CtlSendStatus(USBD_HandleTypeDef *pdev); 92 | USBD_StatusTypeDef USBD_CtlReceiveStatus(USBD_HandleTypeDef *pdev); 93 | 94 | uint32_t USBD_GetRxCount(USBD_HandleTypeDef *pdev, uint8_t ep_addr); 95 | 96 | /** 97 | * @} 98 | */ 99 | 100 | #ifdef __cplusplus 101 | } 102 | #endif 103 | 104 | #endif /* __USBD_IOREQ_H */ 105 | 106 | /** 107 | * @} 108 | */ 109 | 110 | /** 111 | * @} 112 | */ 113 | 114 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FatFs/src/diskio.c: -------------------------------------------------------------------------------- 1 | /*-----------------------------------------------------------------------*/ 2 | /* Low level disk I/O module skeleton for FatFs (C)ChaN, 2017 */ 3 | /* */ 4 | /* Portions COPYRIGHT 2017 STMicroelectronics */ 5 | /* Portions Copyright (C) 2017, ChaN, all right reserved */ 6 | /*-----------------------------------------------------------------------*/ 7 | /* If a working storage control module is available, it should be */ 8 | /* attached to the FatFs via a glue function rather than modifying it. */ 9 | /* This is an example of glue functions to attach various existing */ 10 | /* storage control modules to the FatFs module with a defined API. */ 11 | /*-----------------------------------------------------------------------*/ 12 | 13 | /* Includes ------------------------------------------------------------------*/ 14 | #include "diskio.h" 15 | #include "ff_gen_drv.h" 16 | 17 | #if defined ( __GNUC__ ) 18 | #ifndef __weak 19 | #define __weak __attribute__((weak)) 20 | #endif 21 | #endif 22 | 23 | /* Private typedef -----------------------------------------------------------*/ 24 | /* Private define ------------------------------------------------------------*/ 25 | /* Private variables ---------------------------------------------------------*/ 26 | extern Disk_drvTypeDef disk; 27 | 28 | /* Private function prototypes -----------------------------------------------*/ 29 | /* Private functions ---------------------------------------------------------*/ 30 | 31 | /** 32 | * @brief Gets Disk Status 33 | * @param pdrv: Physical drive number (0..) 34 | * @retval DSTATUS: Operation status 35 | */ 36 | DSTATUS disk_status ( 37 | BYTE pdrv /* Physical drive number to identify the drive */ 38 | ) 39 | { 40 | DSTATUS stat; 41 | 42 | stat = disk.drv[pdrv]->disk_status(disk.lun[pdrv]); 43 | return stat; 44 | } 45 | 46 | /** 47 | * @brief Initializes a Drive 48 | * @param pdrv: Physical drive number (0..) 49 | * @retval DSTATUS: Operation status 50 | */ 51 | DSTATUS disk_initialize ( 52 | BYTE pdrv /* Physical drive nmuber to identify the drive */ 53 | ) 54 | { 55 | DSTATUS stat = RES_OK; 56 | 57 | if(disk.is_initialized[pdrv] == 0) 58 | { 59 | disk.is_initialized[pdrv] = 1; 60 | stat = disk.drv[pdrv]->disk_initialize(disk.lun[pdrv]); 61 | } 62 | return stat; 63 | } 64 | 65 | /** 66 | * @brief Reads Sector(s) 67 | * @param pdrv: Physical drive number (0..) 68 | * @param *buff: Data buffer to store read data 69 | * @param sector: Sector address (LBA) 70 | * @param count: Number of sectors to read (1..128) 71 | * @retval DRESULT: Operation result 72 | */ 73 | DRESULT disk_read ( 74 | BYTE pdrv, /* Physical drive nmuber to identify the drive */ 75 | BYTE *buff, /* Data buffer to store read data */ 76 | DWORD sector, /* Sector address in LBA */ 77 | UINT count /* Number of sectors to read */ 78 | ) 79 | { 80 | DRESULT res; 81 | 82 | res = disk.drv[pdrv]->disk_read(disk.lun[pdrv], buff, sector, count); 83 | return res; 84 | } 85 | 86 | /** 87 | * @brief Writes Sector(s) 88 | * @param pdrv: Physical drive number (0..) 89 | * @param *buff: Data to be written 90 | * @param sector: Sector address (LBA) 91 | * @param count: Number of sectors to write (1..128) 92 | * @retval DRESULT: Operation result 93 | */ 94 | #if _USE_WRITE == 1 95 | DRESULT disk_write ( 96 | BYTE pdrv, /* Physical drive nmuber to identify the drive */ 97 | const BYTE *buff, /* Data to be written */ 98 | DWORD sector, /* Sector address in LBA */ 99 | UINT count /* Number of sectors to write */ 100 | ) 101 | { 102 | DRESULT res; 103 | 104 | res = disk.drv[pdrv]->disk_write(disk.lun[pdrv], buff, sector, count); 105 | return res; 106 | } 107 | #endif /* _USE_WRITE == 1 */ 108 | 109 | /** 110 | * @brief I/O control operation 111 | * @param pdrv: Physical drive number (0..) 112 | * @param cmd: Control code 113 | * @param *buff: Buffer to send/receive control data 114 | * @retval DRESULT: Operation result 115 | */ 116 | #if _USE_IOCTL == 1 117 | DRESULT disk_ioctl ( 118 | BYTE pdrv, /* Physical drive nmuber (0..) */ 119 | BYTE cmd, /* Control code */ 120 | void *buff /* Buffer to send/receive control data */ 121 | ) 122 | { 123 | DRESULT res; 124 | 125 | res = disk.drv[pdrv]->disk_ioctl(disk.lun[pdrv], cmd, buff); 126 | return res; 127 | } 128 | #endif /* _USE_IOCTL == 1 */ 129 | 130 | /** 131 | * @brief Gets Time from RTC 132 | * @param None 133 | * @retval Time in DWORD 134 | */ 135 | __weak DWORD get_fattime (void) 136 | { 137 | return 0; 138 | } 139 | 140 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 141 | 142 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FatFs/src/diskio.h: -------------------------------------------------------------------------------- 1 | /*-----------------------------------------------------------------------/ 2 | / Low level disk interface modlue include file (C)ChaN, 2014 / 3 | /-----------------------------------------------------------------------*/ 4 | 5 | #ifndef _DISKIO_DEFINED 6 | #define _DISKIO_DEFINED 7 | 8 | #ifdef __cplusplus 9 | extern "C" { 10 | #endif 11 | 12 | #define _USE_WRITE 1 /* 1: Enable disk_write function */ 13 | #define _USE_IOCTL 1 /* 1: Enable disk_ioctl function */ 14 | 15 | #include "integer.h" 16 | 17 | 18 | /* Status of Disk Functions */ 19 | typedef BYTE DSTATUS; 20 | 21 | /* Results of Disk Functions */ 22 | typedef enum { 23 | RES_OK = 0, /* 0: Successful */ 24 | RES_ERROR, /* 1: R/W Error */ 25 | RES_WRPRT, /* 2: Write Protected */ 26 | RES_NOTRDY, /* 3: Not Ready */ 27 | RES_PARERR /* 4: Invalid Parameter */ 28 | } DRESULT; 29 | 30 | 31 | /*---------------------------------------*/ 32 | /* Prototypes for disk control functions */ 33 | 34 | 35 | DSTATUS disk_initialize (BYTE pdrv); 36 | DSTATUS disk_status (BYTE pdrv); 37 | DRESULT disk_read (BYTE pdrv, BYTE* buff, DWORD sector, UINT count); 38 | DRESULT disk_write (BYTE pdrv, const BYTE* buff, DWORD sector, UINT count); 39 | DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void* buff); 40 | DWORD get_fattime (void); 41 | 42 | /* Disk Status Bits (DSTATUS) */ 43 | 44 | #define STA_NOINIT 0x01 /* Drive not initialized */ 45 | #define STA_NODISK 0x02 /* No medium in the drive */ 46 | #define STA_PROTECT 0x04 /* Write protected */ 47 | 48 | 49 | /* Command code for disk_ioctrl fucntion */ 50 | 51 | /* Generic command (Used by FatFs) */ 52 | #define CTRL_SYNC 0 /* Complete pending write process (needed at _FS_READONLY == 0) */ 53 | #define GET_SECTOR_COUNT 1 /* Get media size (needed at _USE_MKFS == 1) */ 54 | #define GET_SECTOR_SIZE 2 /* Get sector size (needed at _MAX_SS != _MIN_SS) */ 55 | #define GET_BLOCK_SIZE 3 /* Get erase block size (needed at _USE_MKFS == 1) */ 56 | #define CTRL_TRIM 4 /* Inform device that the data on the block of sectors is no longer used (needed at _USE_TRIM == 1) */ 57 | 58 | /* Generic command (Not used by FatFs) */ 59 | #define CTRL_POWER 5 /* Get/Set power status */ 60 | #define CTRL_LOCK 6 /* Lock/Unlock media removal */ 61 | #define CTRL_EJECT 7 /* Eject media */ 62 | #define CTRL_FORMAT 8 /* Create physical format on the media */ 63 | 64 | /* MMC/SDC specific ioctl command */ 65 | #define MMC_GET_TYPE 10 /* Get card type */ 66 | #define MMC_GET_CSD 11 /* Get CSD */ 67 | #define MMC_GET_CID 12 /* Get CID */ 68 | #define MMC_GET_OCR 13 /* Get OCR */ 69 | #define MMC_GET_SDSTAT 14 /* Get SD status */ 70 | 71 | /* ATA/CF specific ioctl command */ 72 | #define ATA_GET_REV 20 /* Get F/W revision */ 73 | #define ATA_GET_MODEL 21 /* Get model name */ 74 | #define ATA_GET_SN 22 /* Get serial number */ 75 | 76 | #ifdef __cplusplus 77 | } 78 | #endif 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FatFs/src/ff_gen_drv.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file ff_gen_drv.c 4 | * @author MCD Application Team 5 | * @brief FatFs generic low level driver. 6 | ***************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. All rights reserved. 10 | * 11 | * This software component is licensed by ST under BSD 3-Clause license, 12 | * the "License"; You may not use this file except in compliance with the 13 | * License. You may obtain a copy of the License at: 14 | * opensource.org/licenses/BSD-3-Clause 15 | * 16 | ****************************************************************************** 17 | **/ 18 | /* Includes ------------------------------------------------------------------*/ 19 | #include "ff_gen_drv.h" 20 | 21 | /* Private typedef -----------------------------------------------------------*/ 22 | /* Private define ------------------------------------------------------------*/ 23 | /* Private variables ---------------------------------------------------------*/ 24 | Disk_drvTypeDef disk = {{0},{0},{0},0}; 25 | 26 | /* Private function prototypes -----------------------------------------------*/ 27 | /* Private functions ---------------------------------------------------------*/ 28 | 29 | /** 30 | * @brief Links a compatible diskio driver/lun id and increments the number of active 31 | * linked drivers. 32 | * @note The number of linked drivers (volumes) is up to 10 due to FatFs limits. 33 | * @param drv: pointer to the disk IO Driver structure 34 | * @param path: pointer to the logical drive path 35 | * @param lun : only used for USB Key Disk to add multi-lun management 36 | else the parameter must be equal to 0 37 | * @retval Returns 0 in case of success, otherwise 1. 38 | */ 39 | uint8_t FATFS_LinkDriverEx(const Diskio_drvTypeDef *drv, char *path, uint8_t lun) 40 | { 41 | uint8_t ret = 1; 42 | uint8_t DiskNum = 0; 43 | 44 | if(disk.nbr < _VOLUMES) 45 | { 46 | disk.is_initialized[disk.nbr] = 0; 47 | disk.drv[disk.nbr] = drv; 48 | disk.lun[disk.nbr] = lun; 49 | DiskNum = disk.nbr++; 50 | path[0] = DiskNum + '0'; 51 | path[1] = ':'; 52 | path[2] = '/'; 53 | path[3] = 0; 54 | ret = 0; 55 | } 56 | 57 | return ret; 58 | } 59 | 60 | /** 61 | * @brief Links a compatible diskio driver and increments the number of active 62 | * linked drivers. 63 | * @note The number of linked drivers (volumes) is up to 10 due to FatFs limits 64 | * @param drv: pointer to the disk IO Driver structure 65 | * @param path: pointer to the logical drive path 66 | * @retval Returns 0 in case of success, otherwise 1. 67 | */ 68 | uint8_t FATFS_LinkDriver(const Diskio_drvTypeDef *drv, char *path) 69 | { 70 | return FATFS_LinkDriverEx(drv, path, 0); 71 | } 72 | 73 | /** 74 | * @brief Unlinks a diskio driver and decrements the number of active linked 75 | * drivers. 76 | * @param path: pointer to the logical drive path 77 | * @param lun : not used 78 | * @retval Returns 0 in case of success, otherwise 1. 79 | */ 80 | uint8_t FATFS_UnLinkDriverEx(char *path, uint8_t lun) 81 | { 82 | uint8_t DiskNum = 0; 83 | uint8_t ret = 1; 84 | 85 | if(disk.nbr >= 1) 86 | { 87 | DiskNum = path[0] - '0'; 88 | if(disk.drv[DiskNum] != 0) 89 | { 90 | disk.drv[DiskNum] = 0; 91 | disk.lun[DiskNum] = 0; 92 | disk.nbr--; 93 | ret = 0; 94 | } 95 | } 96 | 97 | return ret; 98 | } 99 | 100 | /** 101 | * @brief Unlinks a diskio driver and decrements the number of active linked 102 | * drivers. 103 | * @param path: pointer to the logical drive path 104 | * @retval Returns 0 in case of success, otherwise 1. 105 | */ 106 | uint8_t FATFS_UnLinkDriver(char *path) 107 | { 108 | return FATFS_UnLinkDriverEx(path, 0); 109 | } 110 | 111 | /** 112 | * @brief Gets number of linked drivers to the FatFs module. 113 | * @param None 114 | * @retval Number of attached drivers. 115 | */ 116 | uint8_t FATFS_GetAttachedDriversNbr(void) 117 | { 118 | return disk.nbr; 119 | } 120 | 121 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 122 | 123 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FatFs/src/ff_gen_drv.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file ff_gen_drv.h 4 | * @author MCD Application Team 5 | * @brief Header for ff_gen_drv.c module. 6 | ***************************************************************************** 7 | * @attention 8 | * 9 | * Copyright (c) 2017 STMicroelectronics. All rights reserved. 10 | * 11 | * This software component is licensed by ST under BSD 3-Clause license, 12 | * the "License"; You may not use this file except in compliance with the 13 | * License. You may obtain a copy of the License at: 14 | * opensource.org/licenses/BSD-3-Clause 15 | * 16 | ****************************************************************************** 17 | **/ 18 | 19 | /* Define to prevent recursive inclusion -------------------------------------*/ 20 | #ifndef __FF_GEN_DRV_H 21 | #define __FF_GEN_DRV_H 22 | 23 | #ifdef __cplusplus 24 | extern "C" { 25 | #endif 26 | 27 | /* Includes ------------------------------------------------------------------*/ 28 | #include "diskio.h" 29 | #include "ff.h" 30 | #include "stdint.h" 31 | 32 | 33 | /* Exported types ------------------------------------------------------------*/ 34 | 35 | /** 36 | * @brief Disk IO Driver structure definition 37 | */ 38 | typedef struct 39 | { 40 | DSTATUS (*disk_initialize) (BYTE); /*!< Initialize Disk Drive */ 41 | DSTATUS (*disk_status) (BYTE); /*!< Get Disk Status */ 42 | DRESULT (*disk_read) (BYTE, BYTE*, DWORD, UINT); /*!< Read Sector(s) */ 43 | #if _USE_WRITE == 1 44 | DRESULT (*disk_write) (BYTE, const BYTE*, DWORD, UINT); /*!< Write Sector(s) when _USE_WRITE = 0 */ 45 | #endif /* _USE_WRITE == 1 */ 46 | #if _USE_IOCTL == 1 47 | DRESULT (*disk_ioctl) (BYTE, BYTE, void*); /*!< I/O control operation when _USE_IOCTL = 1 */ 48 | #endif /* _USE_IOCTL == 1 */ 49 | 50 | }Diskio_drvTypeDef; 51 | 52 | /** 53 | * @brief Global Disk IO Drivers structure definition 54 | */ 55 | typedef struct 56 | { 57 | uint8_t is_initialized[_VOLUMES]; 58 | const Diskio_drvTypeDef *drv[_VOLUMES]; 59 | uint8_t lun[_VOLUMES]; 60 | volatile uint8_t nbr; 61 | 62 | }Disk_drvTypeDef; 63 | 64 | /* Exported constants --------------------------------------------------------*/ 65 | /* Exported macro ------------------------------------------------------------*/ 66 | /* Exported functions ------------------------------------------------------- */ 67 | uint8_t FATFS_LinkDriver(const Diskio_drvTypeDef *drv, char *path); 68 | uint8_t FATFS_UnLinkDriver(char *path); 69 | uint8_t FATFS_LinkDriverEx(const Diskio_drvTypeDef *drv, char *path, BYTE lun); 70 | uint8_t FATFS_UnLinkDriverEx(char *path, BYTE lun); 71 | uint8_t FATFS_GetAttachedDriversNbr(void); 72 | 73 | #ifdef __cplusplus 74 | } 75 | #endif 76 | 77 | #endif /* __FF_GEN_DRV_H */ 78 | 79 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 80 | 81 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FatFs/src/integer.h: -------------------------------------------------------------------------------- 1 | /*-------------------------------------------*/ 2 | /* Integer type definitions for FatFs module */ 3 | /*-------------------------------------------*/ 4 | 5 | #ifndef _FF_INTEGER 6 | #define _FF_INTEGER 7 | 8 | #ifdef _WIN32 /* FatFs development platform */ 9 | 10 | #include 11 | #include 12 | typedef unsigned __int64 QWORD; 13 | 14 | 15 | #else /* Embedded platform */ 16 | 17 | /* These types MUST be 16-bit or 32-bit */ 18 | typedef int INT; 19 | typedef unsigned int UINT; 20 | 21 | /* This type MUST be 8-bit */ 22 | typedef unsigned char BYTE; 23 | 24 | /* These types MUST be 16-bit */ 25 | typedef short SHORT; 26 | typedef unsigned short WORD; 27 | typedef unsigned short WCHAR; 28 | 29 | /* These types MUST be 32-bit */ 30 | typedef long LONG; 31 | typedef unsigned long DWORD; 32 | 33 | /* This type MUST be 64-bit (Remove this for ANSI C (C89) compatibility) */ 34 | typedef unsigned long long QWORD; 35 | 36 | #endif 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/freertos_mpool.h: -------------------------------------------------------------------------------- 1 | /* -------------------------------------------------------------------------- 2 | * Copyright (c) 2013-2020 Arm Limited. All rights reserved. 3 | * 4 | * SPDX-License-Identifier: Apache-2.0 5 | * 6 | * Licensed under the Apache License, Version 2.0 (the License); you may 7 | * not use this file except in compliance with the License. 8 | * You may obtain a copy of the License at 9 | * 10 | * www.apache.org/licenses/LICENSE-2.0 11 | * 12 | * Unless required by applicable law or agreed to in writing, software 13 | * distributed under the License is distributed on an AS IS BASIS, WITHOUT 14 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 15 | * See the License for the specific language governing permissions and 16 | * limitations under the License. 17 | * 18 | * Name: freertos_mpool.h 19 | * Purpose: CMSIS RTOS2 wrapper for FreeRTOS 20 | * 21 | *---------------------------------------------------------------------------*/ 22 | 23 | #ifndef FREERTOS_MPOOL_H_ 24 | #define FREERTOS_MPOOL_H_ 25 | 26 | #include 27 | #include "FreeRTOS.h" 28 | #include "semphr.h" 29 | 30 | /* Memory Pool implementation definitions */ 31 | #define MPOOL_STATUS 0x5EED0000U 32 | 33 | /* Memory Block header */ 34 | typedef struct { 35 | void *next; /* Pointer to next block */ 36 | } MemPoolBlock_t; 37 | 38 | /* Memory Pool control block */ 39 | typedef struct MemPoolDef_t { 40 | MemPoolBlock_t *head; /* Pointer to head block */ 41 | SemaphoreHandle_t sem; /* Pool semaphore handle */ 42 | uint8_t *mem_arr; /* Pool memory array */ 43 | uint32_t mem_sz; /* Pool memory array size */ 44 | const char *name; /* Pointer to name string */ 45 | uint32_t bl_sz; /* Size of a single block */ 46 | uint32_t bl_cnt; /* Number of blocks */ 47 | uint32_t n; /* Block allocation index */ 48 | volatile uint32_t status; /* Object status flags */ 49 | #if (configSUPPORT_STATIC_ALLOCATION == 1) 50 | StaticSemaphore_t mem_sem; /* Semaphore object memory */ 51 | #endif 52 | } MemPool_t; 53 | 54 | /* No need to hide static object type, just align to coding style */ 55 | #define StaticMemPool_t MemPool_t 56 | 57 | /* Define memory pool control block size */ 58 | #define MEMPOOL_CB_SIZE (sizeof(StaticMemPool_t)) 59 | 60 | /* Define size of the byte array required to create count of blocks of given size */ 61 | #define MEMPOOL_ARR_SIZE(bl_count, bl_size) (((((bl_size) + (4 - 1)) / 4) * 4)*(bl_count)) 62 | 63 | #endif /* FREERTOS_MPOOL_H_ */ 64 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/FreeRTOS/Source/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved. 2 | Permission is hereby granted, free of charge, to any person obtaining a copy of 3 | this software and associated documentation files (the "Software"), to deal in 4 | the Software without restriction, including without limitation the rights to 5 | use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 6 | the Software, and to permit persons to whom the Software is furnished to do so, 7 | subject to the following conditions: 8 | 9 | The above copyright notice and this permission notice shall be included in all 10 | copies or substantial portions of the Software. 11 | 12 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 13 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 14 | FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 15 | COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 16 | IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 17 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 18 | 19 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/mavlink/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | // Visual Studio versions before 2010 don't have stdint.h, so we just error out. 9 | #if (defined _MSC_VER) && (_MSC_VER < 1600) 10 | #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" 11 | #endif 12 | 13 | #include 14 | 15 | /** 16 | * 17 | * CALCULATE THE CHECKSUM 18 | * 19 | */ 20 | 21 | #define X25_INIT_CRC 0xffff 22 | #define X25_VALIDATE_CRC 0xf0b8 23 | 24 | #ifndef HAVE_CRC_ACCUMULATE 25 | /** 26 | * @brief Accumulate the X.25 CRC by adding one char at a time. 27 | * 28 | * The checksum function adds the hash of one char at a time to the 29 | * 16 bit checksum (uint16_t). 30 | * 31 | * @param data new char to hash 32 | * @param crcAccum the already accumulated checksum 33 | **/ 34 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 35 | { 36 | /*Accumulate one byte of data into the CRC*/ 37 | uint8_t tmp; 38 | 39 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 40 | tmp ^= (tmp<<4); 41 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 42 | } 43 | #endif 44 | 45 | 46 | /** 47 | * @brief Initiliaze the buffer for the X.25 CRC 48 | * 49 | * @param crcAccum the 16 bit X.25 CRC 50 | */ 51 | static inline void crc_init(uint16_t* crcAccum) 52 | { 53 | *crcAccum = X25_INIT_CRC; 54 | } 55 | 56 | 57 | /** 58 | * @brief Calculates the X.25 checksum on a byte buffer 59 | * 60 | * @param pBuffer buffer containing the byte array to hash 61 | * @param length length of the byte array 62 | * @return the checksum over the buffer bytes 63 | **/ 64 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 65 | { 66 | uint16_t crcTmp; 67 | crc_init(&crcTmp); 68 | while (length--) { 69 | crc_accumulate(*pBuffer++, &crcTmp); 70 | } 71 | return crcTmp; 72 | } 73 | 74 | 75 | /** 76 | * @brief Accumulate the X.25 CRC by adding an array of bytes 77 | * 78 | * The checksum function adds the hash of one char at a time to the 79 | * 16 bit checksum (uint16_t). 80 | * 81 | * @param data new bytes to hash 82 | * @param crcAccum the already accumulated checksum 83 | **/ 84 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) 85 | { 86 | const uint8_t *p = (const uint8_t *)pBuffer; 87 | while (length--) { 88 | crc_accumulate(*p++, crcAccum); 89 | } 90 | } 91 | 92 | #endif /* _CHECKSUM_H_ */ 93 | 94 | #ifdef __cplusplus 95 | } 96 | #endif 97 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/mavlink/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | #ifndef MAVLINK_H 7 | #define MAVLINK_H 8 | 9 | #define MAVLINK_PRIMARY_XML_IDX 0 10 | 11 | #ifndef MAVLINK_STX 12 | #define MAVLINK_STX 254 13 | #endif 14 | 15 | #ifndef MAVLINK_ENDIAN 16 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 17 | #endif 18 | 19 | #ifndef MAVLINK_ALIGNED_FIELDS 20 | #define MAVLINK_ALIGNED_FIELDS 1 21 | #endif 22 | 23 | #ifndef MAVLINK_CRC_EXTRA 24 | #define MAVLINK_CRC_EXTRA 1 25 | #endif 26 | 27 | #ifndef MAVLINK_COMMAND_24BIT 28 | #define MAVLINK_COMMAND_24BIT 0 29 | #endif 30 | 31 | #include "version.h" 32 | #include "common.h" 33 | 34 | #endif // MAVLINK_H 35 | -------------------------------------------------------------------------------- /Middlewares/Third_Party/mavlink/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #pragma once 6 | 7 | #ifndef MAVLINK_VERSION_H 8 | #define MAVLINK_VERSION_H 9 | 10 | #define MAVLINK_BUILD_DATE "Fri Feb 14 2020" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Mcontroller-v5 2 | 3 | ## 这是Mcontroller跨模态机器人运动控制器固件v5---developed by Fancinnov 4 | 5 | ## 开发文档和教程 6 | 请登录[幻思创新Fancinnov官网](https://www.fancinnov.com/Mcontroller.html) 7 | 8 | ## 注意 9 | #### 本工程托管于Github中的全部代码都是持续更新的。 10 | #### 编译工程需要在本工程根目录中添加依赖包(依赖包文件名:libMcontroller-v5.a)。 11 | #### 您可以登录[幻思创新Fancinnov官网](https://www.fancinnov.com/Mcontroller.html)下载最新版本依赖包。 12 | #### 依赖包不定期更新,请您持续关注官网动态,如有更新请及时下载后替换旧版本依赖包。 13 | 14 | ## 通知 15 | #### 运行本工程需要更新您的STM32CubeIDE至1.9.0以上版本 16 | -------------------------------------------------------------------------------- /USB_DEVICE/App/usb_device.c: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : usb_device.c 5 | * @version : v1.0_Cube 6 | * @brief : This file implements the USB Device 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under Ultimate Liberty license 14 | * SLA0044, the "License"; You may not use this file except in compliance with 15 | * the License. You may obtain a copy of the License at: 16 | * www.st.com/SLA0044 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Includes ------------------------------------------------------------------*/ 23 | 24 | #include "usb_device.h" 25 | #include "usbd_core.h" 26 | #include "usbd_desc.h" 27 | #include "usbd_cdc.h" 28 | #include "usbd_cdc_if.h" 29 | 30 | /* USER CODE BEGIN Includes */ 31 | 32 | /* USER CODE END Includes */ 33 | 34 | /* USER CODE BEGIN PV */ 35 | /* Private variables ---------------------------------------------------------*/ 36 | 37 | /* USER CODE END PV */ 38 | 39 | /* USER CODE BEGIN PFP */ 40 | /* Private function prototypes -----------------------------------------------*/ 41 | 42 | /* USER CODE END PFP */ 43 | 44 | /* USB Device Core handle declaration. */ 45 | USBD_HandleTypeDef hUsbDeviceFS; 46 | 47 | /* 48 | * -- Insert your variables declaration here -- 49 | */ 50 | /* USER CODE BEGIN 0 */ 51 | 52 | /* USER CODE END 0 */ 53 | 54 | /* 55 | * -- Insert your external function declaration here -- 56 | */ 57 | /* USER CODE BEGIN 1 */ 58 | 59 | /* USER CODE END 1 */ 60 | 61 | /** 62 | * Init USB device Library, add supported class and start the library 63 | * @retval None 64 | */ 65 | void MX_USB_DEVICE_Init(void) 66 | { 67 | /* USER CODE BEGIN USB_DEVICE_Init_PreTreatment */ 68 | 69 | /* USER CODE END USB_DEVICE_Init_PreTreatment */ 70 | 71 | /* Init Device Library, add supported class and start the library. */ 72 | if (USBD_Init(&hUsbDeviceFS, &FS_Desc, DEVICE_FS) != USBD_OK) 73 | { 74 | Error_Handler(); 75 | } 76 | if (USBD_RegisterClass(&hUsbDeviceFS, &USBD_CDC) != USBD_OK) 77 | { 78 | Error_Handler(); 79 | } 80 | if (USBD_CDC_RegisterInterface(&hUsbDeviceFS, &USBD_Interface_fops_FS) != USBD_OK) 81 | { 82 | Error_Handler(); 83 | } 84 | if (USBD_Start(&hUsbDeviceFS) != USBD_OK) 85 | { 86 | Error_Handler(); 87 | } 88 | 89 | /* USER CODE BEGIN USB_DEVICE_Init_PostTreatment */ 90 | 91 | /* USER CODE END USB_DEVICE_Init_PostTreatment */ 92 | } 93 | 94 | /** 95 | * @} 96 | */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | -------------------------------------------------------------------------------- /USB_DEVICE/App/usb_device.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : usb_device.h 5 | * @version : v1.0_Cube 6 | * @brief : Header for usb_device.c file. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under Ultimate Liberty license 14 | * SLA0044, the "License"; You may not use this file except in compliance with 15 | * the License. You may obtain a copy of the License at: 16 | * www.st.com/SLA0044 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USB_DEVICE__H__ 24 | #define __USB_DEVICE__H__ 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | /* Includes ------------------------------------------------------------------*/ 31 | #include "stm32f4xx.h" 32 | #include "stm32f4xx_hal.h" 33 | #include "usbd_def.h" 34 | 35 | /* USER CODE BEGIN INCLUDE */ 36 | 37 | /* USER CODE END INCLUDE */ 38 | 39 | /** @addtogroup USBD_OTG_DRIVER 40 | * @{ 41 | */ 42 | 43 | /** @defgroup USBD_DEVICE USBD_DEVICE 44 | * @brief Device file for Usb otg low level driver. 45 | * @{ 46 | */ 47 | 48 | /** @defgroup USBD_DEVICE_Exported_Variables USBD_DEVICE_Exported_Variables 49 | * @brief Public variables. 50 | * @{ 51 | */ 52 | 53 | /* Private variables ---------------------------------------------------------*/ 54 | /* USER CODE BEGIN PV */ 55 | 56 | /* USER CODE END PV */ 57 | 58 | /* Private function prototypes -----------------------------------------------*/ 59 | /* USER CODE BEGIN PFP */ 60 | 61 | /* USER CODE END PFP */ 62 | 63 | /* 64 | * -- Insert your variables declaration here -- 65 | */ 66 | /* USER CODE BEGIN VARIABLES */ 67 | 68 | /* USER CODE END VARIABLES */ 69 | /** 70 | * @} 71 | */ 72 | 73 | /** @defgroup USBD_DEVICE_Exported_FunctionsPrototype USBD_DEVICE_Exported_FunctionsPrototype 74 | * @brief Declaration of public functions for Usb device. 75 | * @{ 76 | */ 77 | 78 | /** USB Device initialization function. */ 79 | void MX_USB_DEVICE_Init(void); 80 | 81 | /* 82 | * -- Insert functions declaration here -- 83 | */ 84 | /* USER CODE BEGIN FD */ 85 | 86 | /* USER CODE END FD */ 87 | /** 88 | * @} 89 | */ 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | 99 | #ifdef __cplusplus 100 | } 101 | #endif 102 | 103 | #endif /* __USB_DEVICE__H__ */ 104 | -------------------------------------------------------------------------------- /USB_DEVICE/App/usbd_cdc_if.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : usbd_cdc_if.h 5 | * @version : v1.0_Cube 6 | * @brief : Header for usbd_cdc_if.c file. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under Ultimate Liberty license 14 | * SLA0044, the "License"; You may not use this file except in compliance with 15 | * the License. You may obtain a copy of the License at: 16 | * www.st.com/SLA0044 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USBD_CDC_IF_H__ 24 | #define __USBD_CDC_IF_H__ 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | /* Includes ------------------------------------------------------------------*/ 31 | #include "usbd_cdc.h" 32 | 33 | /* USER CODE BEGIN INCLUDE */ 34 | #include "hal.h" 35 | /* USER CODE END INCLUDE */ 36 | 37 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 38 | * @brief For Usb device. 39 | * @{ 40 | */ 41 | 42 | /** @defgroup USBD_CDC_IF USBD_CDC_IF 43 | * @brief Usb VCP device module 44 | * @{ 45 | */ 46 | 47 | /** @defgroup USBD_CDC_IF_Exported_Defines USBD_CDC_IF_Exported_Defines 48 | * @brief Defines. 49 | * @{ 50 | */ 51 | /* Define size for the receive and transmit buffer over CDC */ 52 | #define APP_RX_DATA_SIZE 128 53 | #define APP_TX_DATA_SIZE 128 54 | /* USER CODE BEGIN EXPORTED_DEFINES */ 55 | 56 | /* USER CODE END EXPORTED_DEFINES */ 57 | 58 | /** 59 | * @} 60 | */ 61 | 62 | /** @defgroup USBD_CDC_IF_Exported_Types USBD_CDC_IF_Exported_Types 63 | * @brief Types. 64 | * @{ 65 | */ 66 | 67 | /* USER CODE BEGIN EXPORTED_TYPES */ 68 | 69 | /* USER CODE END EXPORTED_TYPES */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup USBD_CDC_IF_Exported_Macros USBD_CDC_IF_Exported_Macros 76 | * @brief Aliases. 77 | * @{ 78 | */ 79 | 80 | /* USER CODE BEGIN EXPORTED_MACRO */ 81 | 82 | /* USER CODE END EXPORTED_MACRO */ 83 | 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup USBD_CDC_IF_Exported_Variables USBD_CDC_IF_Exported_Variables 89 | * @brief Public variables. 90 | * @{ 91 | */ 92 | 93 | /** CDC Interface callback. */ 94 | extern USBD_CDC_ItfTypeDef USBD_Interface_fops_FS; 95 | 96 | /* USER CODE BEGIN EXPORTED_VARIABLES */ 97 | 98 | /* USER CODE END EXPORTED_VARIABLES */ 99 | 100 | /** 101 | * @} 102 | */ 103 | 104 | /** @defgroup USBD_CDC_IF_Exported_FunctionsPrototype USBD_CDC_IF_Exported_FunctionsPrototype 105 | * @brief Public functions declaration. 106 | * @{ 107 | */ 108 | 109 | uint8_t CDC_Transmit_FS(uint8_t* Buf, uint16_t Len); 110 | 111 | /* USER CODE BEGIN EXPORTED_FUNCTIONS */ 112 | 113 | /* USER CODE END EXPORTED_FUNCTIONS */ 114 | 115 | /** 116 | * @} 117 | */ 118 | 119 | /** 120 | * @} 121 | */ 122 | 123 | /** 124 | * @} 125 | */ 126 | 127 | #ifdef __cplusplus 128 | } 129 | #endif 130 | 131 | #endif /* __USBD_CDC_IF_H__ */ 132 | 133 | -------------------------------------------------------------------------------- /USB_DEVICE/App/usbd_desc.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : usbd_desc.c 5 | * @version : v1.0_Cube 6 | * @brief : Header for usbd_conf.c file. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under Ultimate Liberty license 14 | * SLA0044, the "License"; You may not use this file except in compliance with 15 | * the License. You may obtain a copy of the License at: 16 | * www.st.com/SLA0044 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | /* Define to prevent recursive inclusion -------------------------------------*/ 22 | #ifndef __USBD_DESC__C__ 23 | #define __USBD_DESC__C__ 24 | 25 | #ifdef __cplusplus 26 | extern "C" { 27 | #endif 28 | 29 | /* Includes ------------------------------------------------------------------*/ 30 | #include "usbd_def.h" 31 | 32 | /* USER CODE BEGIN INCLUDE */ 33 | 34 | /* USER CODE END INCLUDE */ 35 | 36 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USBD_DESC USBD_DESC 41 | * @brief Usb device descriptors module. 42 | * @{ 43 | */ 44 | 45 | /** @defgroup USBD_DESC_Exported_Constants USBD_DESC_Exported_Constants 46 | * @brief Constants. 47 | * @{ 48 | */ 49 | #define DEVICE_ID1 (UID_BASE) 50 | #define DEVICE_ID2 (UID_BASE + 0x4) 51 | #define DEVICE_ID3 (UID_BASE + 0x8) 52 | 53 | #define USB_SIZ_STRING_SERIAL 0x1A 54 | 55 | /* USER CODE BEGIN EXPORTED_CONSTANTS */ 56 | 57 | /* USER CODE END EXPORTED_CONSTANTS */ 58 | 59 | /** 60 | * @} 61 | */ 62 | 63 | /** @defgroup USBD_DESC_Exported_Defines USBD_DESC_Exported_Defines 64 | * @brief Defines. 65 | * @{ 66 | */ 67 | 68 | /* USER CODE BEGIN EXPORTED_DEFINES */ 69 | 70 | /* USER CODE END EXPORTED_DEFINES */ 71 | 72 | /** 73 | * @} 74 | */ 75 | 76 | /** @defgroup USBD_DESC_Exported_TypesDefinitions USBD_DESC_Exported_TypesDefinitions 77 | * @brief Types. 78 | * @{ 79 | */ 80 | 81 | /* USER CODE BEGIN EXPORTED_TYPES */ 82 | 83 | /* USER CODE END EXPORTED_TYPES */ 84 | 85 | /** 86 | * @} 87 | */ 88 | 89 | /** @defgroup USBD_DESC_Exported_Macros USBD_DESC_Exported_Macros 90 | * @brief Aliases. 91 | * @{ 92 | */ 93 | 94 | /* USER CODE BEGIN EXPORTED_MACRO */ 95 | 96 | /* USER CODE END EXPORTED_MACRO */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** @defgroup USBD_DESC_Exported_Variables USBD_DESC_Exported_Variables 103 | * @brief Public variables. 104 | * @{ 105 | */ 106 | 107 | /** Descriptor for the Usb device. */ 108 | extern USBD_DescriptorsTypeDef FS_Desc; 109 | 110 | /* USER CODE BEGIN EXPORTED_VARIABLES */ 111 | 112 | /* USER CODE END EXPORTED_VARIABLES */ 113 | 114 | /** 115 | * @} 116 | */ 117 | 118 | /** @defgroup USBD_DESC_Exported_FunctionsPrototype USBD_DESC_Exported_FunctionsPrototype 119 | * @brief Public functions declaration. 120 | * @{ 121 | */ 122 | 123 | /* USER CODE BEGIN EXPORTED_FUNCTIONS */ 124 | 125 | /* USER CODE END EXPORTED_FUNCTIONS */ 126 | 127 | /** 128 | * @} 129 | */ 130 | 131 | /** 132 | * @} 133 | */ 134 | 135 | /** 136 | * @} 137 | */ 138 | 139 | #ifdef __cplusplus 140 | } 141 | #endif 142 | 143 | #endif /* __USBD_DESC__C__ */ 144 | 145 | -------------------------------------------------------------------------------- /USB_DEVICE/Target/usbd_conf.h: -------------------------------------------------------------------------------- 1 | /* USER CODE BEGIN Header */ 2 | /** 3 | ****************************************************************************** 4 | * @file : usbd_conf.h 5 | * @version : v1.0_Cube 6 | * @brief : Header for usbd_conf.c file. 7 | ****************************************************************************** 8 | * @attention 9 | * 10 | *

© Copyright (c) 2020 STMicroelectronics. 11 | * All rights reserved.

12 | * 13 | * This software component is licensed by ST under Ultimate Liberty license 14 | * SLA0044, the "License"; You may not use this file except in compliance with 15 | * the License. You may obtain a copy of the License at: 16 | * www.st.com/SLA0044 17 | * 18 | ****************************************************************************** 19 | */ 20 | /* USER CODE END Header */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __USBD_CONF__H__ 24 | #define __USBD_CONF__H__ 25 | 26 | #ifdef __cplusplus 27 | extern "C" { 28 | #endif 29 | 30 | /* Includes ------------------------------------------------------------------*/ 31 | #include 32 | #include 33 | #include 34 | #include "main.h" 35 | #include "stm32f4xx.h" 36 | #include "stm32f4xx_hal.h" 37 | 38 | /* USER CODE BEGIN INCLUDE */ 39 | 40 | /* USER CODE END INCLUDE */ 41 | 42 | /** @addtogroup USBD_OTG_DRIVER 43 | * @brief Driver for Usb device. 44 | * @{ 45 | */ 46 | 47 | /** @defgroup USBD_CONF USBD_CONF 48 | * @brief Configuration file for Usb otg low level driver. 49 | * @{ 50 | */ 51 | 52 | /** @defgroup USBD_CONF_Exported_Variables USBD_CONF_Exported_Variables 53 | * @brief Public variables. 54 | * @{ 55 | */ 56 | 57 | /** 58 | * @} 59 | */ 60 | 61 | /** @defgroup USBD_CONF_Exported_Defines USBD_CONF_Exported_Defines 62 | * @brief Defines for configuration of the Usb device. 63 | * @{ 64 | */ 65 | 66 | /*---------- -----------*/ 67 | #define USBD_MAX_NUM_INTERFACES 1U 68 | /*---------- -----------*/ 69 | #define USBD_MAX_NUM_CONFIGURATION 1U 70 | /*---------- -----------*/ 71 | #define USBD_MAX_STR_DESC_SIZ 128U 72 | /*---------- -----------*/ 73 | #define USBD_DEBUG_LEVEL 1U 74 | /*---------- -----------*/ 75 | #define USBD_LPM_ENABLED 0U 76 | /*---------- -----------*/ 77 | #define USBD_SELF_POWERED 1U 78 | 79 | /****************************************/ 80 | /* #define for FS and HS identification */ 81 | #define DEVICE_FS 0 82 | #define DEVICE_HS 1 83 | 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup USBD_CONF_Exported_Macros USBD_CONF_Exported_Macros 89 | * @brief Aliases. 90 | * @{ 91 | */ 92 | /* Memory management macros make sure to use static memory allocation */ 93 | /** Alias for memory allocation. */ 94 | 95 | #define USBD_malloc (void *)USBD_static_malloc 96 | 97 | /** Alias for memory release. */ 98 | #define USBD_free USBD_static_free 99 | 100 | /** Alias for memory set. */ 101 | #define USBD_memset memset 102 | 103 | /** Alias for memory copy. */ 104 | #define USBD_memcpy memcpy 105 | 106 | /** Alias for delay. */ 107 | #define USBD_Delay HAL_Delay 108 | 109 | /* DEBUG macros */ 110 | 111 | #if (USBD_DEBUG_LEVEL > 0) 112 | #define USBD_UsrLog(...) printf(__VA_ARGS__);\ 113 | printf("\n"); 114 | #else 115 | #define USBD_UsrLog(...) 116 | #endif /* (USBD_DEBUG_LEVEL > 0U) */ 117 | 118 | #if (USBD_DEBUG_LEVEL > 1) 119 | 120 | #define USBD_ErrLog(...) printf("ERROR: ");\ 121 | printf(__VA_ARGS__);\ 122 | printf("\n"); 123 | #else 124 | #define USBD_ErrLog(...) 125 | #endif /* (USBD_DEBUG_LEVEL > 1U) */ 126 | 127 | #if (USBD_DEBUG_LEVEL > 2) 128 | #define USBD_DbgLog(...) printf("DEBUG : ");\ 129 | printf(__VA_ARGS__);\ 130 | printf("\n"); 131 | #else 132 | #define USBD_DbgLog(...) 133 | #endif /* (USBD_DEBUG_LEVEL > 2U) */ 134 | 135 | /** 136 | * @} 137 | */ 138 | 139 | /** @defgroup USBD_CONF_Exported_Types USBD_CONF_Exported_Types 140 | * @brief Types. 141 | * @{ 142 | */ 143 | 144 | /** 145 | * @} 146 | */ 147 | 148 | /** @defgroup USBD_CONF_Exported_FunctionsPrototype USBD_CONF_Exported_FunctionsPrototype 149 | * @brief Declaration of public functions for Usb device. 150 | * @{ 151 | */ 152 | 153 | /* Exported functions -------------------------------------------------------*/ 154 | void *USBD_static_malloc(uint32_t size); 155 | void USBD_static_free(void *p); 156 | 157 | /** 158 | * @} 159 | */ 160 | 161 | /** 162 | * @} 163 | */ 164 | 165 | /** 166 | * @} 167 | */ 168 | 169 | #ifdef __cplusplus 170 | } 171 | #endif 172 | 173 | #endif /* __USBD_CONF__H__ */ 174 | 175 | --------------------------------------------------------------------------------