├── .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 |
--------------------------------------------------------------------------------