├── .editorconfig ├── .gitignore ├── .vscode ├── c_cpp_properties.json └── settings.json ├── FatFs ├── 00readme.txt ├── diskio.c ├── diskio.h ├── ff.c ├── ff.h ├── ffconf.h ├── integer.h └── option │ ├── ccsbcs.c │ ├── syscall.c │ └── unicode.c ├── FreeRTOS └── Source │ ├── croutine.c │ ├── event_groups.c │ ├── include │ ├── FreeRTOS.h │ ├── FreeRTOSConfig.h │ ├── StackMacros.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 │ ├── stdint.readme │ ├── stream_buffer.h │ ├── task.h │ └── timers.h │ ├── list.c │ ├── portable │ ├── Keil │ │ └── See-also-the-RVDS-directory.txt │ ├── MemMang │ │ ├── ReadMe.url │ │ ├── heap_1.c │ │ ├── heap_2.c │ │ ├── heap_3.c │ │ ├── heap_4.c │ │ └── heap_5.c │ └── RVDS │ │ ├── ARM_CM0 │ │ ├── port.c │ │ └── portmacro.h │ │ ├── ARM_CM3 │ │ ├── port.c │ │ └── portmacro.h │ │ ├── ARM_CM4F │ │ ├── port.c │ │ └── portmacro.h │ │ ├── ARM_CM4_MPU │ │ ├── port.c │ │ └── portmacro.h │ │ └── ARM_CM7 │ │ ├── ReadMe.txt │ │ └── r0p1 │ │ ├── port.c │ │ └── portmacro.h │ ├── queue.c │ ├── readme.txt │ ├── stream_buffer.c │ ├── tasks.c │ └── timers.c ├── HARDWARE └── 天穹飞控V3原理图.pdf ├── LICENSE ├── 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_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_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_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 ├── message_definitions │ └── v1.0 │ │ └── common.xml └── protocol.h ├── PIC ├── BlueSkyPilot.jpg ├── QGroudControl.jpg └── 飞控配置.jpg ├── PROJECT └── BlueSkyFlightControl.uvprojx ├── README.md ├── SRC ├── CONTROL │ ├── flightControl.c │ ├── flightControl.h │ ├── missionControl.c │ ├── missionControl.h │ ├── motor.c │ ├── motor.h │ ├── rc.c │ ├── rc.h │ ├── safeControl.c │ ├── safeControl.h │ ├── userControl.c │ ├── userControl.h │ ├── waypointControl.c │ └── waypointControl.h ├── DRIVER │ ├── board.c │ ├── board.h │ ├── boardConfigBlueSkyV3.h │ ├── drv_adc.c │ ├── drv_adc.h │ ├── drv_can.c │ ├── drv_can.h │ ├── drv_flash.c │ ├── drv_flash.h │ ├── drv_i2c_soft.c │ ├── drv_i2c_soft.h │ ├── drv_ppm.c │ ├── drv_ppm.h │ ├── drv_pwm.c │ ├── drv_pwm.h │ ├── drv_sbus.c │ ├── drv_sbus.h │ ├── drv_spi.c │ ├── drv_spi.h │ ├── drv_usart.c │ ├── drv_usart.h │ ├── drv_usb.c │ └── drv_usb.h ├── LOG │ ├── logger.c │ ├── logger.h │ ├── ulog.c │ ├── ulog.h │ ├── ulog_data.c │ └── ulog_data.h ├── MATH │ ├── LevenbergMarquardt.c │ ├── LevenbergMarquardt.h │ ├── declination.c │ ├── declination.h │ ├── kalman3.c │ ├── kalman3.h │ ├── kalmanVel.c │ ├── kalmanVel.h │ ├── lowPassFilter.c │ ├── lowPassFilter.h │ ├── mathTool.c │ ├── mathTool.h │ ├── matrix3.c │ ├── matrix3.h │ ├── matrix6.c │ ├── matrix6.h │ ├── pid.c │ ├── pid.h │ ├── quaternion.c │ ├── quaternion.h │ ├── rotation.c │ ├── rotation.h │ ├── vector3.c │ └── vector3.h ├── MESSAGE │ ├── bsklink.c │ ├── bsklink.h │ ├── bsklinkDecode.c │ ├── bsklinkDecode.h │ ├── bsklinkSend.c │ ├── bsklinkSend.h │ ├── mavlinkDecode.c │ ├── mavlinkDecode.h │ ├── mavlinkNotice.c │ ├── mavlinkNotice.h │ ├── mavlinkParam.c │ ├── mavlinkParam.h │ ├── mavlinkSend.c │ ├── mavlinkSend.h │ ├── message.c │ └── message.h ├── MODULE │ ├── 2smpb.c │ ├── 2smpb.h │ ├── battery.c │ ├── battery.h │ ├── icm20602.c │ ├── icm20602.h │ ├── icm20689.c │ ├── icm20689.h │ ├── ist8310.c │ ├── ist8310.h │ ├── mmc3630.c │ ├── mmc3630.h │ ├── module.c │ ├── module.h │ ├── mpu6000.c │ ├── mpu6000.h │ ├── mpu6500.c │ ├── mpu6500.h │ ├── ms5611.c │ ├── ms5611.h │ ├── qmc5883.c │ ├── qmc5883.h │ ├── rgb.c │ ├── rgb.h │ ├── ublox.c │ └── ublox.h ├── NAVIGATION │ ├── ahrs.c │ ├── ahrs.h │ ├── ahrsAux.c │ ├── ahrsAux.h │ ├── navigation.c │ └── navigation.h ├── SENSOR │ ├── accelerometer.c │ ├── accelerometer.h │ ├── barometer.c │ ├── barometer.h │ ├── gps.c │ ├── gps.h │ ├── gyroscope.c │ ├── gyroscope.h │ ├── magnetometer.c │ ├── magnetometer.h │ ├── sensor.c │ └── sensor.h ├── SYSTEM │ ├── faultDetect.c │ ├── faultDetect.h │ ├── flightStatus.c │ ├── flightStatus.h │ ├── parameter.c │ └── parameter.h ├── TASK │ ├── TaskConfig.h │ ├── control_task.c │ ├── control_task.h │ ├── log_task.c │ ├── log_task.h │ ├── messageQueue.c │ ├── messageQueue.h │ ├── message_task.c │ ├── message_task.h │ ├── module_task.c │ ├── module_task.h │ ├── navigation_task.c │ ├── navigation_task.h │ ├── sensor_task.c │ └── sensor_task.h └── main.c ├── STMLIB ├── CORE │ ├── core_cm4.h │ ├── core_cm4_simd.h │ ├── startup_stm32f40_41xxx.s │ └── startup_stm32f427x.s ├── Include │ ├── arm_common_tables.h │ ├── arm_const_structs.h │ ├── arm_math.h │ ├── cmsis_armcc.h │ ├── cmsis_armcc_V6.h │ ├── cmsis_gcc.h │ ├── core_cm0.h │ ├── core_cm0plus.h │ ├── core_cm3.h │ ├── core_cm4.h │ ├── core_cm7.h │ ├── core_cmFunc.h │ ├── core_cmInstr.h │ ├── core_cmSimd.h │ ├── core_sc000.h │ └── core_sc300.h ├── SDIO │ ├── stm32f4_sdio_sd.c │ ├── stm32f4_sdio_sd.h │ ├── stm32f4_sdio_sd_LowLevel.c │ └── stm32f4_sdio_sd_LowLevel.h ├── USB │ ├── STM32_USB_Device_Library │ │ ├── Class │ │ │ ├── audio │ │ │ │ ├── inc │ │ │ │ │ ├── usbd_audio_core.h │ │ │ │ │ └── usbd_audio_out_if.h │ │ │ │ └── src │ │ │ │ │ ├── usbd_audio_core.c │ │ │ │ │ └── usbd_audio_out_if.c │ │ │ ├── cdc │ │ │ │ ├── inc │ │ │ │ │ ├── usbd_cdc_core.h │ │ │ │ │ └── usbd_cdc_if_template.h │ │ │ │ └── src │ │ │ │ │ ├── usbd_cdc_core.c │ │ │ │ │ └── usbd_cdc_if_template.c │ │ │ ├── dfu │ │ │ │ ├── inc │ │ │ │ │ ├── usbd_dfu_core.h │ │ │ │ │ ├── usbd_dfu_mal.h │ │ │ │ │ ├── usbd_flash_if.h │ │ │ │ │ ├── usbd_mem_if_template.h │ │ │ │ │ └── usbd_otp_if.h │ │ │ │ └── src │ │ │ │ │ ├── usbd_dfu_core.c │ │ │ │ │ ├── usbd_dfu_mal.c │ │ │ │ │ ├── usbd_flash_if.c │ │ │ │ │ ├── usbd_mem_if_template.c │ │ │ │ │ └── usbd_otp_if.c │ │ │ ├── hid │ │ │ │ ├── inc │ │ │ │ │ └── usbd_hid_core.h │ │ │ │ └── src │ │ │ │ │ └── usbd_hid_core.c │ │ │ └── msc │ │ │ │ ├── inc │ │ │ │ ├── usbd_msc_bot.h │ │ │ │ ├── usbd_msc_core.h │ │ │ │ ├── usbd_msc_data.h │ │ │ │ ├── usbd_msc_mem.h │ │ │ │ └── usbd_msc_scsi.h │ │ │ │ └── src │ │ │ │ ├── usbd_msc_bot.c │ │ │ │ ├── usbd_msc_core.c │ │ │ │ ├── usbd_msc_data.c │ │ │ │ ├── usbd_msc_scsi.c │ │ │ │ └── usbd_storage_template.c │ │ ├── Core │ │ │ ├── inc │ │ │ │ ├── usbd_conf_template.h │ │ │ │ ├── usbd_core.h │ │ │ │ ├── usbd_def.h │ │ │ │ ├── usbd_ioreq.h │ │ │ │ ├── usbd_req.h │ │ │ │ └── usbd_usr.h │ │ │ └── src │ │ │ │ ├── usbd_core.c │ │ │ │ ├── usbd_ioreq.c │ │ │ │ └── usbd_req.c │ │ └── Release_Notes.html │ ├── STM32_USB_HOST_Library │ │ ├── Class │ │ │ ├── HID │ │ │ │ ├── inc │ │ │ │ │ ├── usbh_hid_core.h │ │ │ │ │ ├── usbh_hid_keybd.h │ │ │ │ │ └── usbh_hid_mouse.h │ │ │ │ └── src │ │ │ │ │ ├── usbh_hid_core.c │ │ │ │ │ ├── usbh_hid_keybd.c │ │ │ │ │ └── usbh_hid_mouse.c │ │ │ └── MSC │ │ │ │ ├── inc │ │ │ │ ├── usbh_msc_bot.h │ │ │ │ ├── usbh_msc_core.h │ │ │ │ └── usbh_msc_scsi.h │ │ │ │ └── src │ │ │ │ ├── usbh_msc_bot.c │ │ │ │ ├── usbh_msc_core.c │ │ │ │ ├── usbh_msc_fatfs.c │ │ │ │ └── usbh_msc_scsi.c │ │ ├── Core │ │ │ ├── inc │ │ │ │ ├── usbh_conf_template.h │ │ │ │ ├── usbh_core.h │ │ │ │ ├── usbh_def.h │ │ │ │ ├── usbh_hcs.h │ │ │ │ ├── usbh_ioreq.h │ │ │ │ └── usbh_stdreq.h │ │ │ └── src │ │ │ │ ├── usbh_core.c │ │ │ │ ├── usbh_hcs.c │ │ │ │ ├── usbh_ioreq.c │ │ │ │ └── usbh_stdreq.c │ │ └── Release_Notes.html │ ├── STM32_USB_OTG_Driver │ │ ├── Release_Notes.html │ │ ├── inc │ │ │ ├── usb_bsp.h │ │ │ ├── usb_conf_template.h │ │ │ ├── usb_core.h │ │ │ ├── usb_dcd.h │ │ │ ├── usb_dcd_int.h │ │ │ ├── usb_defines.h │ │ │ ├── usb_hcd.h │ │ │ ├── usb_hcd_int.h │ │ │ ├── usb_otg.h │ │ │ └── usb_regs.h │ │ └── src │ │ │ ├── usb_bsp_template.c │ │ │ ├── usb_core.c │ │ │ ├── usb_dcd.c │ │ │ ├── usb_dcd_int.c │ │ │ ├── usb_hcd.c │ │ │ ├── usb_hcd_int.c │ │ │ └── usb_otg.c │ ├── usb_bsp.c │ ├── usb_conf.h │ ├── usbd_conf.h │ ├── usbd_desc.c │ └── usbd_desc.h ├── inc │ ├── misc.h │ ├── stm32f4xx_adc.h │ ├── stm32f4xx_can.h │ ├── stm32f4xx_cec.h │ ├── stm32f4xx_crc.h │ ├── stm32f4xx_cryp.h │ ├── stm32f4xx_dac.h │ ├── stm32f4xx_dbgmcu.h │ ├── stm32f4xx_dcmi.h │ ├── stm32f4xx_dfsdm.h │ ├── stm32f4xx_dma.h │ ├── stm32f4xx_dma2d.h │ ├── stm32f4xx_dsi.h │ ├── stm32f4xx_exti.h │ ├── stm32f4xx_flash.h │ ├── stm32f4xx_flash_ramfunc.h │ ├── stm32f4xx_fmc.h │ ├── stm32f4xx_fmpi2c.h │ ├── stm32f4xx_fsmc.h │ ├── stm32f4xx_gpio.h │ ├── stm32f4xx_hash.h │ ├── stm32f4xx_i2c.h │ ├── stm32f4xx_iwdg.h │ ├── stm32f4xx_lptim.h │ ├── stm32f4xx_ltdc.h │ ├── stm32f4xx_pwr.h │ ├── stm32f4xx_qspi.h │ ├── stm32f4xx_rcc.h │ ├── stm32f4xx_rng.h │ ├── stm32f4xx_rtc.h │ ├── stm32f4xx_sai.h │ ├── stm32f4xx_sdio.h │ ├── stm32f4xx_spdifrx.h │ ├── stm32f4xx_spi.h │ ├── stm32f4xx_syscfg.h │ ├── stm32f4xx_tim.h │ ├── stm32f4xx_usart.h │ └── stm32f4xx_wwdg.h ├── src │ ├── misc.c │ ├── stm32f4xx_adc.c │ ├── stm32f4xx_can.c │ ├── stm32f4xx_cec.c │ ├── stm32f4xx_crc.c │ ├── stm32f4xx_cryp.c │ ├── stm32f4xx_cryp_aes.c │ ├── stm32f4xx_cryp_des.c │ ├── stm32f4xx_cryp_tdes.c │ ├── stm32f4xx_dac.c │ ├── stm32f4xx_dbgmcu.c │ ├── stm32f4xx_dcmi.c │ ├── stm32f4xx_dfsdm.c │ ├── stm32f4xx_dma.c │ ├── stm32f4xx_dma2d.c │ ├── stm32f4xx_dsi.c │ ├── stm32f4xx_exti.c │ ├── stm32f4xx_flash.c │ ├── stm32f4xx_flash_ramfunc.c │ ├── stm32f4xx_fmc.c │ ├── stm32f4xx_fmpi2c.c │ ├── stm32f4xx_fsmc.c │ ├── stm32f4xx_gpio.c │ ├── stm32f4xx_hash.c │ ├── stm32f4xx_hash_md5.c │ ├── stm32f4xx_hash_sha1.c │ ├── stm32f4xx_i2c.c │ ├── stm32f4xx_iwdg.c │ ├── stm32f4xx_lptim.c │ ├── stm32f4xx_ltdc.c │ ├── stm32f4xx_pwr.c │ ├── stm32f4xx_qspi.c │ ├── stm32f4xx_rcc.c │ ├── stm32f4xx_rng.c │ ├── stm32f4xx_rtc.c │ ├── stm32f4xx_sai.c │ ├── stm32f4xx_sdio.c │ ├── stm32f4xx_spdifrx.c │ ├── stm32f4xx_spi.c │ ├── stm32f4xx_syscfg.c │ ├── stm32f4xx_tim.c │ ├── stm32f4xx_usart.c │ └── stm32f4xx_wwdg.c ├── stm32f4xx.h ├── stm32f4xx_conf.h ├── stm32f4xx_it.c ├── stm32f4xx_it.h ├── system_stm32f4xx.c └── system_stm32f4xx.h ├── TOOLS ├── flightplot.jar ├── 天穹地面站V0.5.1.exe └── 天穹地面站运行说明.txt ├── keilkilll.bat └── 项目开发记录.md /.editorconfig: -------------------------------------------------------------------------------- 1 | # top-most EditorConfig file 2 | root = true 3 | 4 | # all files 5 | [*] 6 | indent_style = tab 7 | indent_size = 4 -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | OBJ/ 2 | *.TMP 3 | PROJECT/*.txt 4 | PROJECT/*.ini 5 | PROJECT/*.bak 6 | PROJECT/DebugConfig/ 7 | PROJECT/*.scvd 8 | PROJECT/*.Administrator 9 | PROJECT/*.uvoptx 10 | PROJECT/*.dep 11 | PROJECT/*.BlueSky 12 | *.orig -------------------------------------------------------------------------------- /.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "name": "Win32", 5 | "browse": { 6 | "path": [ 7 | "${workspaceFolder}" 8 | ], 9 | "limitSymbolsToIncludedHeaders": true 10 | }, 11 | "includePath": [ 12 | "${workspaceFolder}", 13 | "${workspaceFolder}/FreeRTOS/Source/include", 14 | "${workspaceFolder}/FreeRTOS/Source/portable/RVDS/ARM_CM4F", 15 | "${workspaceFolder}/STMLIB", 16 | "${workspaceFolder}/STMLIB/inc", 17 | "${workspaceFolder}/STMLIB/include", 18 | "${workspaceFolder}/STMLIB/CORE", 19 | "${workspaceFolder}/STMLIB/USB", 20 | "${workspaceFolder}/STMLIB/USB/STM32_USB_OTG_Driver/inc", 21 | "${workspaceFolder}/STMLIB/USB/STM32_USB_Device_Library/Core/inc", 22 | "${workspaceFolder}/STMLIB/USB/STM32_USB_Device_Library/Class/cdc/inc", 23 | "${workspaceFolder}/SRC/CONTROL", 24 | "${workspaceFolder}/SRC/MESSAGE", 25 | "${workspaceFolder}/SRC/DRIVER", 26 | "${workspaceFolder}/SRC/MATH", 27 | "${workspaceFolder}/SRC/MODULE", 28 | "${workspaceFolder}/SRC/NAVIGATION", 29 | "${workspaceFolder}/SRC/SENSOR", 30 | "${workspaceFolder}/SRC/SYSTEM", 31 | "${workspaceFolder}/SRC/TASK", 32 | "${workspaceFolder}/MAVLINK" 33 | ], 34 | "defines": [ 35 | "_DEBUG", 36 | "UNICODE", 37 | "_UNICODE" 38 | ], 39 | "cStandard": "c11", 40 | "cppStandard": "c++17", 41 | "intelliSenseMode": "msvc-x64" 42 | } 43 | ], 44 | "version": 4 45 | } -------------------------------------------------------------------------------- /.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "C_Cpp.errorSquiggles": "Disabled" 3 | } -------------------------------------------------------------------------------- /FatFs/diskio.h: -------------------------------------------------------------------------------- 1 | /*----------------------------------------------------------------------- 2 | / Low level disk interface modlue include file (C)ChaN, 2013 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 fucntion */ 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, BYTE count); 38 | DRESULT disk_write (BYTE pdrv, const BYTE* buff, DWORD sector, BYTE count); 39 | DRESULT disk_ioctl (BYTE pdrv, BYTE cmd, void* buff); 40 | 41 | 42 | /* Disk Status Bits (DSTATUS) */ 43 | #define STA_NOINIT 0x01 /* Drive not initialized */ 44 | #define STA_NODISK 0x02 /* No medium in the drive */ 45 | #define STA_PROTECT 0x04 /* Write protected */ 46 | 47 | 48 | /* Command code for disk_ioctrl fucntion */ 49 | 50 | /* Generic command (used by FatFs) */ 51 | #define CTRL_SYNC 0 /* Flush disk cache (for write functions) */ 52 | #define GET_SECTOR_COUNT 1 /* Get media size (for only f_mkfs()) */ 53 | #define GET_SECTOR_SIZE 2 /* Get sector size (for multiple sector size (_MAX_SS >= 1024)) */ 54 | #define GET_BLOCK_SIZE 3 /* Get erase block size (for only f_mkfs()) */ 55 | #define CTRL_ERASE_SECTOR 4 /* Force erased a block of sectors (for only _USE_ERASE) */ 56 | 57 | /* Generic command (not used by FatFs) */ 58 | #define CTRL_POWER 5 /* Get/Set power status */ 59 | #define CTRL_LOCK 6 /* Lock/Unlock media removal */ 60 | #define CTRL_EJECT 7 /* Eject media */ 61 | #define CTRL_FORMAT 8 /* Create physical format on the media */ 62 | 63 | /* MMC/SDC specific ioctl command */ 64 | #define MMC_GET_TYPE 10 /* Get card type */ 65 | #define MMC_GET_CSD 11 /* Get CSD */ 66 | #define MMC_GET_CID 12 /* Get CID */ 67 | #define MMC_GET_OCR 13 /* Get OCR */ 68 | #define MMC_GET_SDSTAT 14 /* Get SD status */ 69 | 70 | /* ATA/CF specific ioctl command */ 71 | #define ATA_GET_REV 20 /* Get F/W revision */ 72 | #define ATA_GET_MODEL 21 /* Get model name */ 73 | #define ATA_GET_SN 22 /* Get serial number */ 74 | 75 | 76 | /* MMC card type flags (MMC_GET_TYPE) */ 77 | #define CT_MMC 0x01 /* MMC ver 3 */ 78 | #define CT_SD1 0x02 /* SD ver 1 */ 79 | #define CT_SD2 0x04 /* SD ver 2 */ 80 | #define CT_SDC (CT_SD1|CT_SD2) /* SD */ 81 | #define CT_BLOCK 0x08 /* Block addressing */ 82 | 83 | 84 | #ifdef __cplusplus 85 | } 86 | #endif 87 | 88 | #endif 89 | -------------------------------------------------------------------------------- /FatFs/integer.h: -------------------------------------------------------------------------------- 1 | /*-------------------------------------------*/ 2 | /* Integer type definitions for FatFs module */ 3 | /*-------------------------------------------*/ 4 | 5 | #ifndef _INTEGER 6 | #define _INTEGER 7 | 8 | #ifdef _WIN32 /* FatFs development platform */ 9 | 10 | #include 11 | #include 12 | 13 | #else /* Embedded platform */ 14 | 15 | /* These types must be 16-bit, 32-bit or larger integer */ 16 | typedef int INT; 17 | typedef unsigned int UINT; 18 | 19 | /* These types must be 8-bit integer */ 20 | typedef char CHAR; 21 | typedef unsigned char UCHAR; 22 | typedef unsigned char BYTE; 23 | 24 | /* These types must be 16-bit integer */ 25 | typedef short SHORT; 26 | typedef unsigned short USHORT; 27 | typedef unsigned short WORD; 28 | typedef unsigned short WCHAR; 29 | 30 | /* These types must be 32-bit integer */ 31 | typedef long LONG; 32 | typedef unsigned long ULONG; 33 | typedef unsigned long DWORD; 34 | 35 | #endif 36 | 37 | #endif 38 | -------------------------------------------------------------------------------- /FatFs/option/unicode.c: -------------------------------------------------------------------------------- 1 | #include "../ff.h" 2 | 3 | #if _USE_LFN != 0 4 | 5 | #if _CODE_PAGE == 932 6 | #include "cc932.c" 7 | #elif _CODE_PAGE == 936 8 | #include "cc936.c" 9 | #elif _CODE_PAGE == 949 10 | #include "cc949.c" 11 | #elif _CODE_PAGE == 950 12 | #include "cc950.c" 13 | #else 14 | #include "ccsbcs.c" 15 | #endif 16 | 17 | #endif 18 | -------------------------------------------------------------------------------- /FreeRTOS/Source/include/stdint.readme: -------------------------------------------------------------------------------- 1 | 2 | #ifndef FREERTOS_STDINT 3 | #define FREERTOS_STDINT 4 | 5 | /******************************************************************************* 6 | * THIS IS NOT A FULL stdint.h IMPLEMENTATION - It only contains the definitions 7 | * necessary to build the FreeRTOS code. It is provided to allow FreeRTOS to be 8 | * built using compilers that do not provide their own stdint.h definition. 9 | * 10 | * To use this file: 11 | * 12 | * 1) Copy this file into the directory that contains your FreeRTOSConfig.h 13 | * header file, as that directory will already be in the compilers include 14 | * path. 15 | * 16 | * 2) Rename the copied file stdint.h. 17 | * 18 | */ 19 | 20 | typedef signed char int8_t; 21 | typedef unsigned char uint8_t; 22 | typedef short int16_t; 23 | typedef unsigned short uint16_t; 24 | typedef long int32_t; 25 | typedef unsigned long uint32_t; 26 | 27 | #endif /* FREERTOS_STDINT */ 28 | -------------------------------------------------------------------------------- /FreeRTOS/Source/portable/Keil/See-also-the-RVDS-directory.txt: -------------------------------------------------------------------------------- 1 | Nothing to see here. -------------------------------------------------------------------------------- /FreeRTOS/Source/portable/MemMang/ReadMe.url: -------------------------------------------------------------------------------- 1 | [{000214A0-0000-0000-C000-000000000046}] 2 | Prop3=19,2 3 | [InternetShortcut] 4 | URL=http://www.freertos.org/a00111.html 5 | IDList= 6 | -------------------------------------------------------------------------------- /FreeRTOS/Source/portable/MemMang/heap_3.c: -------------------------------------------------------------------------------- 1 | /* 2 | * FreeRTOS Kernel V10.0.1 3 | * Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved. 4 | * 5 | * Permission is hereby granted, free of charge, to any person obtaining a copy of 6 | * this software and associated documentation files (the "Software"), to deal in 7 | * the Software without restriction, including without limitation the rights to 8 | * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of 9 | * the Software, and to permit persons to whom the Software is furnished to do so, 10 | * subject to the following conditions: 11 | * 12 | * The above copyright notice and this permission notice shall be included in all 13 | * copies or substantial portions of the Software. 14 | * 15 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS 17 | * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR 18 | * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER 19 | * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 20 | * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 21 | * 22 | * http://www.FreeRTOS.org 23 | * http://aws.amazon.com/freertos 24 | * 25 | * 1 tab == 4 spaces! 26 | */ 27 | 28 | 29 | /* 30 | * Implementation of pvPortMalloc() and vPortFree() that relies on the 31 | * compilers own malloc() and free() implementations. 32 | * 33 | * This file can only be used if the linker is configured to to generate 34 | * a heap memory area. 35 | * 36 | * See heap_1.c, heap_2.c and heap_4.c for alternative implementations, and the 37 | * memory management pages of http://www.FreeRTOS.org for more information. 38 | */ 39 | 40 | #include 41 | 42 | /* Defining MPU_WRAPPERS_INCLUDED_FROM_API_FILE prevents task.h from redefining 43 | all the API functions to use the MPU wrappers. That should only be done when 44 | task.h is included from an application file. */ 45 | #define MPU_WRAPPERS_INCLUDED_FROM_API_FILE 46 | 47 | #include "FreeRTOS.h" 48 | #include "task.h" 49 | 50 | #undef MPU_WRAPPERS_INCLUDED_FROM_API_FILE 51 | 52 | #if( configSUPPORT_DYNAMIC_ALLOCATION == 0 ) 53 | #error This file must not be used if configSUPPORT_DYNAMIC_ALLOCATION is 0 54 | #endif 55 | 56 | /*-----------------------------------------------------------*/ 57 | 58 | void *pvPortMalloc( size_t xWantedSize ) 59 | { 60 | void *pvReturn; 61 | 62 | vTaskSuspendAll(); 63 | { 64 | pvReturn = malloc( xWantedSize ); 65 | traceMALLOC( pvReturn, xWantedSize ); 66 | } 67 | ( void ) xTaskResumeAll(); 68 | 69 | #if( configUSE_MALLOC_FAILED_HOOK == 1 ) 70 | { 71 | if( pvReturn == NULL ) 72 | { 73 | extern void vApplicationMallocFailedHook( void ); 74 | vApplicationMallocFailedHook(); 75 | } 76 | } 77 | #endif 78 | 79 | return pvReturn; 80 | } 81 | /*-----------------------------------------------------------*/ 82 | 83 | void vPortFree( void *pv ) 84 | { 85 | if( pv ) 86 | { 87 | vTaskSuspendAll(); 88 | { 89 | free( pv ); 90 | traceFREE( pv, 0 ); 91 | } 92 | ( void ) xTaskResumeAll(); 93 | } 94 | } 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /FreeRTOS/Source/portable/RVDS/ARM_CM7/ReadMe.txt: -------------------------------------------------------------------------------- 1 | There are two options for running FreeRTOS on ARM Cortex-M7 microcontrollers. 2 | The best option depends on the revision of the ARM Cortex-M7 core in use. The 3 | revision is specified by an 'r' number, and a 'p' number, so will look something 4 | like 'r0p1'. Check the documentation for the microcontroller in use to find the 5 | revision of the Cortex-M7 core used in that microcontroller. If in doubt, use 6 | the FreeRTOS port provided specifically for r0p1 revisions, as that can be used 7 | with all core revisions. 8 | 9 | The first option is to use the ARM Cortex-M4F port, and the second option is to 10 | use the Cortex-M7 r0p1 port - the latter containing a minor errata workaround. 11 | 12 | If the revision of the ARM Cortex-M7 core is not r0p1 then either option can be 13 | used, but it is recommended to use the FreeRTOS ARM Cortex-M4F port located in 14 | the /FreeRTOS/Source/portable/RVDS/ARM_CM4F directory. 15 | 16 | If the revision of the ARM Cortex-M7 core is r0p1 then use the FreeRTOS ARM 17 | Cortex-M7 r0p1 port located in the /FreeRTOS/Source/portable/RVDS/ARM_CM7/r0p1 18 | directory. -------------------------------------------------------------------------------- /FreeRTOS/Source/readme.txt: -------------------------------------------------------------------------------- 1 | Each real time kernel port consists of three files that contain the core kernel 2 | components and are common to every port, and one or more files that are 3 | specific to a particular microcontroller and or compiler. 4 | 5 | + The FreeRTOS/Source directory contains the three files that are common to 6 | every port - list.c, queue.c and tasks.c. The kernel is contained within these 7 | three files. croutine.c implements the optional co-routine functionality - which 8 | is normally only used on very memory limited systems. 9 | 10 | + The FreeRTOS/Source/Portable directory contains the files that are specific to 11 | a particular microcontroller and or compiler. 12 | 13 | + The FreeRTOS/Source/include directory contains the real time kernel header 14 | files. 15 | 16 | See the readme file in the FreeRTOS/Source/Portable directory for more 17 | information. -------------------------------------------------------------------------------- /HARDWARE/天穹飞控V3原理图.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/HARDWARE/天穹飞控V3原理图.pdf -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /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 "Tue Jul 24 2018" 11 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 12 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 13 | 14 | #endif // MAVLINK_VERSION_H 15 | -------------------------------------------------------------------------------- /PIC/BlueSkyPilot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/PIC/BlueSkyPilot.jpg -------------------------------------------------------------------------------- /PIC/QGroudControl.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/PIC/QGroudControl.jpg -------------------------------------------------------------------------------- /PIC/飞控配置.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/PIC/飞控配置.jpg -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 天穹飞控 2 | 3 | ## 项目暂停更新说明 4 | 5 | 本人最近新入职某无人机公司(非DJI),重返无人机行业,重新开始一段学习和修炼的旅程,而此开源飞控项目将无限期暂停更新。 6 | 7 | ## 飞控简介 8 | 9 | 近10年来,小型多旋翼飞行器发展迅猛,国外涌现出无数开源项目,其中不乏一些耳熟能详的优秀开源飞控项目。而在中国,消费级多旋翼更是发展得如火如荼,其中DJI一枝独秀,占据了国内外80%的市场。然而有一点值得我们深思的是,即使到今天,国内仍然没有一个像样的开源飞控项目,只有少部分存在于淘宝上的收费开源项目,而且质量极其堪忧,其中大部分连最基本的定点悬停功能都无法实现,且代码架构混乱,缺少文档和资料,少数带有定点功能的,其效果也远远不及普通的商业飞控,更不用说和DJI之类的进行对比。更糟糕的一点是,这些收费开源飞控项目基本上都是一锤子买卖,缺少后续更新维护及性能优化,少数一些更新仅仅是小BUG修复。 10 | 11 | 在这个背景下,本飞控项目诞生了,其目的不仅仅是为了做出一款性能逼近甚至超越DJI的飞控,而是想让更多的中国人参与进飞控项目的开发,包括大学生们,各行各业的爱好者,还有各地区的行业从业者等。国外的APM/PX4飞控也是经过数百人多年的维护和优化,才有了今天的成果,但是也不得不说国外的大部分开源飞控在性能细节上的优化还没有到位,性能难以与DJI之类经过深度优化的飞控同台竞技。 12 | 13 | 而本项目的最终目标,是打造出一套易于使用和开发,且飞行性能比肩商业产品的中国本土化开源多旋翼飞控平台,为中国的多旋翼无人机产业添砖加瓦。 14 | 15 | >飞控交流Q群:472648354 16 | 17 | >[飞控交流论坛](http://bbs.loveuav.com/forum-68-1.html) 18 | 19 | ## 飞控特色 20 | 21 | - 功能丰富,除了最基础的自稳、定高、定点飞行之外,还支持自动降落、自动返航、航线规划、兴趣点环绕等高级功能,其余功能如编队飞行、智能避障还在陆续开发中 22 | 23 | - 飞控上使用硬件恒温,传感器零漂更加稳定 24 | 25 | - 飞控的姿态估计中使用了运动加速度补偿和自适应卡尔曼滤波器,极大提升了飞机在各种机动飞行状况下的姿态精度,并直接提升了惯导性能。导航方面使用了基于飞行状态误差模型的自适应卡尔曼滤波器,提升动态刹车精度以及悬停精度。飞行性能方面还在持续优化中 26 | 27 | - 加速度与地磁传感器的校准均使用Levenberg-Marquardt算法拟合椭球误差方程,校准精度基本达到了廉价MEMS传感器的极限 28 | 29 | - 位置环与姿态环均使用串级PID控制算法,悬停姿态控制精度可达0.1°,且控制方面将算法实现与操控逻辑实现分开,方便代码理解及后期扩展开发 30 | 31 | - 使用了轻量级RTOS:FreeRTOS,实现抢占式任务调度,提高系统实时性 32 | 33 | - 使用自定义的轻量级通信协议bsklink,配合自主开发的天穹地面站软件,可实现飞控参数设置、传感器校准、波形显示分析和航线规划等功能 34 | 35 | - 兼容mavlink协议,可使用QGroudControl及Mission Planner地面站的大部分功能如传感器校准,数据波形分析,航线规划等 36 | 37 | - 支持FatFs文件系统和飞行日志功能(ulog格式),可使用FlightPlot工具查看并分析飞行日志 38 | 39 | - 飞控整体架构极其清晰,代码逐行注释,功能模块化且尽可能降低各模块之间的耦合程度,方便新手理解及代码移植 40 | 41 | ## 下载与编译 42 | 43 | - 使用git克隆项目到本地 44 | 45 | - 安装Keil MDK 5.25,编译飞控工程 46 | 47 | >[详细图文说明](http://bbs.loveuav.com/thread-11422-1-1.html) 48 | 49 | 50 | ## 飞控硬件配置 51 | 52 | ![飞控配置](https://github.com/loveuav/BlueSkyFlightControl/blob/master/PIC/飞控配置.jpg) 53 | 54 | [购买飞控板](https://item.taobao.com/item.htm?spm=a1z10.3-c-s.w4002-1694068282.10.3d006ada1kJAJG&id=580800534157) 55 | 56 | ## 天穹地面站 57 | 58 | ![天穹地面站](https://github.com/loveuav/BlueSkyFlightControl/blob/master/PIC/BlueSkyPilot.jpg) 59 | 60 | 简介及下载:[天穹地面站——专为飞控研发而生](https://blog.csdn.net/loveuav/article/details/83592374) 61 | 62 | ## QGroundControl地面站 63 | 64 | QGroundControl是一个在QT跨平台环境下开发的开源无人机地面站项目,支持windows,linux,android、ios等,使用mavlink协议。 65 | 66 | >[【天穹飞控】QGroundControl地面站使用教程](http://bbs.loveuav.com/thread-11450-1-1.html) 67 | 68 | ![QGroundControl](https://github.com/loveuav/BlueSkyFlightControl/blob/master/PIC/QGroudControl.jpg) 69 | 70 | ## 作者相关 71 | 72 | BlueSky 73 | 74 | QQ:352707983 75 | 76 | ## 【深入浅出多旋翼飞控开发】 77 | 78 | 针对飞控初学者编写的一系列教程,可配合开源飞控项目:[天穹飞控](https://github.com/loveuav/BlueSkyFlightControl)一起学习,效率更高。 79 | 80 | 作者:**BlueSky** 81 | 82 | 欢迎斧正和指导,QQ:352707983 83 | 84 | 先挖个坑,再慢慢来填······ 85 | 86 | ### 目录 87 | 88 | #### 【概述篇】 89 | 90 | ##### [一.多旋翼飞控发展史](http://bbs.loveuav.com/thread-693-1-1.html) 91 | 92 | ##### [二.多旋翼飞控技术综述](https://blog.csdn.net/loveuav/article/details/81605417) 93 | 94 | #### 【预备篇】 95 | 96 | ##### [一.元器件选型及飞控电路设计](http://bbs.loveuav.com/thread-11314-1-1.html) 97 | ##### [二.飞控代码下载与编译](http://bbs.loveuav.com/thread-11422-1-1.html) 98 | ##### 三.天穹地面站的使用 99 | ##### [四.QGroundControl地面站的使用](http://bbs.loveuav.com/thread-11450-1-1.html) 100 | 101 | #### 【姿态篇】 102 | ##### [一.初识姿态估计](https://blog.csdn.net/loveuav/article/details/81713015) 103 | ##### 二.卡尔曼滤波 104 | ##### 三.最优估计与卡尔曼滤波推导 105 | ##### [四.非线性最小二乘与飞控传感器校准](https://blog.csdn.net/loveuav/article/details/81592870) 106 | ##### 五.运动中的传感器误差分析和优化 107 | 108 | #### 【导航篇】 109 | 110 | #### 【控制篇】 111 | 112 | #### 【系统篇】 113 | 114 | #### 【诊断篇】 115 | -------------------------------------------------------------------------------- /SRC/CONTROL/flightControl.h: -------------------------------------------------------------------------------- 1 | #ifndef _FLIGHTCONTROL_H_ 2 | #define _FLIGHTCONTROL_H_ 3 | 4 | #include "mathTool.h" 5 | #include "pid.h" 6 | 7 | #define MAXANGLE 400 //最大飞行角度:40° 8 | #define MAXRCDATA 450 9 | 10 | enum 11 | { 12 | ROLL_INNER, 13 | PITCH_INNER, 14 | YAW_INNER, 15 | ROLL_OUTER, 16 | PITCH_OUTER, 17 | YAW_OUTER, 18 | VEL_X, 19 | VEL_Y, 20 | VEL_Z, 21 | POS_X, 22 | POS_Y, 23 | POS_Z, 24 | PIDNUM 25 | }; 26 | 27 | typedef struct 28 | { 29 | float roll; 30 | float pitch; 31 | float yaw; 32 | float throttle; 33 | } RCTARGET_t; 34 | 35 | typedef struct 36 | { 37 | PID_t pid[PIDNUM]; //PID参数结构体 38 | 39 | RCTARGET_t rcTarget; //摇杆控制量 40 | Vector3f_t angleLpf; 41 | 42 | Vector3f_t attInnerCtlValue; //姿态内环控制量 43 | float altInnerCtlValue; //高度内环控制量 44 | 45 | Vector3f_t attInnerTarget; //姿态内环(角速度)控制目标 46 | Vector3f_t attOuterTarget; //姿态外环(角度)控制目标 47 | Vector3f_t posInnerTarget; //位置内环(速度)控制目标 48 | Vector3f_t posOuterTarget; //位置外环(位置)控制目标 49 | 50 | Vector3f_t attInnerError; //姿态内环(角速度)控制误差 51 | Vector3f_t attOuterError; //姿态外环(角度)控制误差 52 | Vector3f_t posInnerError; //位置内环(速度)控制误差 53 | Vector3f_t posOuterError; //位置外环(位置)控制误差 54 | 55 | uint8_t altCtlFlag; //高度控制使能标志位 56 | uint8_t posCtlFlag; //位置控制使能标志位 57 | uint8_t yawHoldFlag; //航向锁定控制使能标志位 58 | 59 | int16_t maxBrakeAngle; //最大刹车角度 60 | int16_t maxPosOuterCtl; //位置控制的最大输出 61 | int16_t maxAltOuterCtl; //高度控制的最大输出 62 | } FLIGHTCONTROL_t; 63 | 64 | void FlightControlInit(void); 65 | 66 | void SetRcTarget(RCTARGET_t rcTarget); 67 | void FlightControlInnerLoop(Vector3f_t gyro); 68 | void AttitudeOuterControl(void); 69 | void AltitudeOuterControl(void); 70 | void PositionInnerControl(void); 71 | void PositionOuterControl(void); 72 | 73 | void SetYawCtlTarget(float target); 74 | 75 | void SetAltInnerCtlTarget(float target); 76 | void SetAltOuterCtlTarget(float target); 77 | void SetPosInnerCtlTarget(Vector3f_t target); 78 | void SetPosOuterCtlTarget(Vector3f_t target); 79 | 80 | void SetAltCtlStatus(uint8_t status); 81 | void SetPosCtlStatus(uint8_t status); 82 | void SetYawHoldStatus(uint8_t status); 83 | 84 | Vector3f_t GetAttInnerCtlError(void); 85 | Vector3f_t GetAttOuterCtlError(void); 86 | Vector3f_t GetPosInnerCtlError(void); 87 | Vector3f_t GetPosOuterCtlError(void); 88 | 89 | Vector3f_t GetAttInnerCtlTarget(void); 90 | Vector3f_t GetAttOuterCtlTarget(void); 91 | Vector3f_t GetPosInnerCtlTarget(void); 92 | Vector3f_t GetPosOuterCtlTarget(void); 93 | 94 | void FlightControlReset(void); 95 | PID_t FcGetPID(uint8_t id); 96 | void FcSetPID(uint8_t id, PID_t pid); 97 | bool PIDReadFromFlash(void); 98 | 99 | void SetMaxBrakeAngle(int16_t angle); 100 | void SetMaxPosOuterCtl(int16_t vel); 101 | void SetMaxAltOuterCtl(int16_t vel); 102 | 103 | #endif 104 | 105 | 106 | 107 | -------------------------------------------------------------------------------- /SRC/CONTROL/missionControl.h: -------------------------------------------------------------------------------- 1 | #ifndef _MISSIONCONTROL_H_ 2 | #define _MISSIONCONTROL_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | enum 7 | { 8 | RTH_STEP_START = 0, 9 | RTH_STEP_TURN, 10 | RTH_STEP_CLIMB, 11 | RTH_STEP_FLIGHT, 12 | RTH_STEP_TURN_BACK, 13 | RTH_STEP_LOITER 14 | }; 15 | 16 | void MissionControl(void); 17 | 18 | #endif 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /SRC/CONTROL/motor.h: -------------------------------------------------------------------------------- 1 | #ifndef _MOTOR_H_ 2 | #define _MOTOR_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct 7 | { 8 | float throttle; //油门 9 | float roll; //横滚 10 | float pitch; //俯仰 11 | float yaw; //偏航 12 | } MOTOR_MIXER_t; 13 | 14 | typedef struct 15 | { 16 | int8_t motorNum; 17 | MOTOR_MIXER_t motorMixer[8]; 18 | } MOTOR_TYPE_t; 19 | 20 | void MotorInit(void); 21 | void MotorControl(int16_t roll, int16_t pitch, int16_t yaw, int16_t throttle); 22 | void EscCalibrateEnable(void); 23 | void MotorStop(void); 24 | int16_t* GetMotorValue(void); 25 | int8_t GetMotorNum(void); 26 | 27 | #endif 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /SRC/CONTROL/rc.h: -------------------------------------------------------------------------------- 1 | #ifndef _RC_H_ 2 | #define _RC_H_ 3 | 4 | #include "mathTool.h" 5 | #include "board.h" 6 | 7 | enum 8 | { 9 | AUX1, 10 | AUX2, 11 | AUX3, 12 | AUX4, 13 | AUX5, 14 | AUX6, 15 | AUX7, 16 | AUX8, 17 | AUX9, 18 | AUX10, 19 | AUX11, 20 | AUX12 21 | }; 22 | 23 | enum 24 | { 25 | LOW, 26 | MID, 27 | HIGH 28 | }; 29 | 30 | enum 31 | { 32 | ROLL, 33 | PITCH, 34 | YAW, 35 | THROTTLE 36 | }; 37 | 38 | typedef struct 39 | { 40 | int16_t roll; //横滚 41 | int16_t pitch; //俯仰 42 | int16_t yaw; //偏航 43 | int16_t throttle; //油门 44 | } RCCOMMAND_t; 45 | 46 | void RcInit(void); 47 | void RcCheck(void); 48 | RCDATA_t GetRcData(void); 49 | RCCOMMAND_t GetRcCommad(void); 50 | 51 | void FlightStatusUpdate(void); 52 | 53 | #endif 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /SRC/CONTROL/safeControl.h: -------------------------------------------------------------------------------- 1 | #ifndef _SAFECONTROL_H_ 2 | #define _SAFECONTROL_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void SafeControl(void); 7 | 8 | #endif 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /SRC/CONTROL/userControl.h: -------------------------------------------------------------------------------- 1 | #ifndef _USERCONTROL_H_ 2 | #define _USERCONTROL_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void UserControl(void); 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /SRC/CONTROL/waypointControl.h: -------------------------------------------------------------------------------- 1 | #ifndef _WAYPOINTCONTROL_H_ 2 | #define _WAYPOINTCONTROL_H_ 3 | 4 | #include "mathTool.h" 5 | #include "common/mavlink.h" 6 | 7 | void WaypointControl(void); 8 | 9 | uint16_t GetWaypointCount(void); 10 | void SetWaypointCount(uint16_t count); 11 | uint16_t GetWaypointRecvCount(void); 12 | void SetWaypointRecvCount(uint16_t count); 13 | uint16_t GetWaypointSendCount(void); 14 | void SetWaypointSendCount(uint16_t count); 15 | 16 | void SetWaypointItem(uint16_t count, mavlink_mission_item_t item); 17 | void ClearAllWaypointItem(void); 18 | mavlink_mission_item_t GetWaypointItem(uint16_t count); 19 | 20 | #endif 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /SRC/DRIVER/board.h: -------------------------------------------------------------------------------- 1 | #ifndef __BOARD_H__ 2 | #define __BOARD_H__ 3 | 4 | #include "stm32f4xx.h" 5 | 6 | /********************************************************************************************************** 7 | *飞控软件版本 8 | **********************************************************************************************************/ 9 | #define SOFTWARE_VERSION_HIGH 0 10 | #define SOFTWARE_VERSION_MID 4 11 | #define SOFTWARE_VERSION_LOW 27 12 | 13 | /********************************************************************************************************** 14 | *飞控硬件类型 15 | **********************************************************************************************************/ 16 | enum 17 | { 18 | BOARD_BLUESKY_V3 = 1, 19 | BOARD_FUTURESKY 20 | }; 21 | 22 | /********************************************************************************************************** 23 | *定义硬件类型 24 | **********************************************************************************************************/ 25 | #define BLUESKY_V3 26 | 27 | //硬件配置头文件包含 28 | #ifdef BLUESKY_V3 29 | #include "boardConfigBlueSkyV3.h" 30 | #define BOARD_TYPE BOARD_BLUESKY_V3 31 | #endif 32 | 33 | typedef struct 34 | { 35 | int16_t roll; 36 | int16_t pitch; 37 | int16_t yaw; 38 | int16_t throttle; 39 | int16_t aux1; 40 | int16_t aux2; 41 | int16_t aux3; 42 | int16_t aux4; 43 | int16_t aux5; 44 | int16_t aux6; 45 | int16_t aux7; 46 | int16_t aux8; 47 | int16_t aux9; 48 | int16_t aux10; 49 | int16_t aux11; 50 | int16_t aux12; 51 | } RCDATA_t; 52 | 53 | enum 54 | { 55 | MPU6000, 56 | MPU6500, 57 | ICM20602, 58 | ICM20689 59 | }; 60 | 61 | enum 62 | { 63 | BMP280, 64 | MS5611, 65 | _2SMPB, 66 | LPS22HB 67 | }; 68 | 69 | enum 70 | { 71 | HMC5883, 72 | QMC5883, 73 | IST8310, 74 | MMC3630 75 | }; 76 | 77 | enum 78 | { 79 | SBUS, 80 | PPM 81 | }; 82 | 83 | void BoardInit(void); 84 | void SoftDelayMs(uint32_t ms); 85 | void SoftDelayUs(uint32_t us); 86 | void OsDelayMs(uint32_t ms); 87 | 88 | uint64_t GetSysTimeUs(void); 89 | uint32_t GetSysTimeMs(void); 90 | 91 | #endif 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_adc.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 drv_adc.c 6 | * @说明 ADC驱动 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.06 11 | **********************************************************************************************************/ 12 | #include "drv_adc.h" 13 | 14 | /********************************************************************************************************** 15 | *函 数 名: Adc_Init 16 | *功能说明: ADC初始化 17 | *形 参: 无 18 | *返 回 值: 无 19 | **********************************************************************************************************/ 20 | void Adc_Init(void) 21 | { 22 | GPIO_InitTypeDef GPIO_InitStructure; 23 | ADC_CommonInitTypeDef ADC_CommonInitStructure; 24 | ADC_InitTypeDef ADC_InitStructyre; 25 | 26 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; 27 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 28 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; 29 | 30 | GPIO_InitStructure.GPIO_Pin = ADC_VOLTAGE_PIN; 31 | GPIO_Init(ADC_VOLTAGE_GPIO, &GPIO_InitStructure); 32 | 33 | GPIO_InitStructure.GPIO_Pin = ADC_CURRENT_PIN; 34 | GPIO_Init(ADC_CURRENT_GPIO, &GPIO_InitStructure); 35 | 36 | ADC_DeInit(); 37 | 38 | ADC_CommonInitStructure.ADC_DMAAccessMode = ADC_DMAAccessMode_Disabled; 39 | ADC_CommonInitStructure.ADC_Mode = ADC_Mode_Independent; 40 | ADC_CommonInitStructure.ADC_Prescaler = ADC_Prescaler_Div8; 41 | ADC_CommonInitStructure.ADC_TwoSamplingDelay = ADC_TwoSamplingDelay_20Cycles; 42 | ADC_CommonInit(&ADC_CommonInitStructure); 43 | 44 | ADC_InitStructyre.ADC_ContinuousConvMode = ENABLE; 45 | ADC_InitStructyre.ADC_DataAlign = ADC_DataAlign_Right; 46 | ADC_InitStructyre.ADC_ExternalTrigConvEdge = ADC_ExternalTrigConvEdge_None; 47 | ADC_InitStructyre.ADC_NbrOfConversion = 1; 48 | ADC_InitStructyre.ADC_Resolution = ADC_Resolution_12b; 49 | ADC_InitStructyre.ADC_ScanConvMode = ENABLE; 50 | 51 | ADC_Init(ADC_VOLTAGE, &ADC_InitStructyre); 52 | ADC_Cmd(ADC_VOLTAGE, ENABLE); 53 | ADC_RegularChannelConfig(ADC_VOLTAGE, ADC_VOLTAGE_CHAN, 1, ADC_SampleTime_144Cycles); 54 | ADC_SoftwareStartConv(ADC_VOLTAGE); 55 | 56 | ADC_Init(ADC_CURRENT, &ADC_InitStructyre); 57 | ADC_Cmd(ADC_CURRENT, ENABLE); 58 | ADC_RegularChannelConfig(ADC_CURRENT, ADC_CURRENT_CHAN, 1, ADC_SampleTime_144Cycles); 59 | ADC_SoftwareStartConv(ADC_CURRENT); 60 | } 61 | 62 | /********************************************************************************************************** 63 | *函 数 名: GetVoltageAdcValue 64 | *功能说明: 获取电压ADC采样值 65 | *形 参: 无 66 | *返 回 值: ADC采样值 67 | **********************************************************************************************************/ 68 | uint16_t GetVoltageAdcValue(void) 69 | { 70 | static uint16_t adcTemp; 71 | 72 | adcTemp = ADC_GetConversionValue(ADC_VOLTAGE) >> 4; 73 | 74 | return (adcTemp * 330 / 0xFFF); 75 | } 76 | 77 | /********************************************************************************************************** 78 | *函 数 名: GetCurrentAdcValue 79 | *功能说明: 获取电流ADC采样值 80 | *形 参: 无 81 | *返 回 值: ADC采样值 82 | **********************************************************************************************************/ 83 | uint16_t GetCurrentAdcValue(void) 84 | { 85 | static uint16_t adcTemp; 86 | 87 | adcTemp = ADC_GetConversionValue(ADC_CURRENT) >> 4; 88 | 89 | return (adcTemp * 330 / 0xFFF); 90 | } 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_adc.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_ADC_H__ 2 | #define __DRV_ADC_H__ 3 | 4 | #include "board.h" 5 | 6 | void Adc_Init(void); 7 | uint16_t GetVoltageAdcValue(void); 8 | uint16_t GetCurrentAdcValue(void); 9 | 10 | #endif 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_can.h: -------------------------------------------------------------------------------- 1 | #ifndef _DRV_CAN_H 2 | #define _DRV_CAN_H 3 | 4 | #include "board.h" 5 | 6 | void Can_Open(uint8_t deviceNum); 7 | void CAN1_SendTest(void); 8 | 9 | #endif 10 | 11 | 12 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_flash.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_FLASH_H__ 2 | #define __DRV_FLASH_H__ 3 | 4 | #include "board.h" 5 | 6 | uint8_t Flash_ReadByte(uint32_t start_addr, uint16_t cnt); 7 | bool Flash_WriteByte(uint32_t dest,uint8_t *src,uint32_t length); 8 | 9 | #endif 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_i2c_soft.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_I2C_SOFT_H__ 2 | #define __DRV_I2C_SOFT_H__ 3 | 4 | #include "board.h" 5 | 6 | void Soft_I2c_Open(uint8_t deviceNum); 7 | bool Soft_I2c_Single_Write(uint8_t deviceNum, u8 SlaveAddress,u8 REG_Address,u8 REG_data); 8 | uint8_t Soft_I2C_Single_Read(uint8_t deviceNum, u8 SlaveAddress,u8 REG_Address); 9 | bool Soft_I2C_Multi_Read(uint8_t deviceNum, u8 SlaveAddress,u8 REG_Address,u8 * ptChar,u8 size); 10 | 11 | #endif 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_ppm.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_PPM_H__ 2 | #define __DRV_PPM_H__ 3 | 4 | #include "board.h" 5 | 6 | typedef void (*RcDataCallback)(RCDATA_t data); 7 | 8 | void PPM_Init(void); 9 | void PPM_Disable(void); 10 | void PPM_SetRcDataCallback(RcDataCallback rcDataCallback); 11 | 12 | #endif 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_pwm.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_PWM_H__ 2 | #define __DRV_PWM_H__ 3 | 4 | #include "board.h" 5 | 6 | void PWM_Init(void); 7 | void TempControlPWMSet(int32_t pwmValue); 8 | void MotorPWMSet(uint8_t motor, uint16_t pwmValue); 9 | 10 | #endif 11 | 12 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_sbus.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_SBUS_H__ 2 | #define __DRV_SBUS_H__ 3 | 4 | #include "board.h" 5 | 6 | typedef void (*RcDataCallback)(RCDATA_t data); 7 | 8 | void Sbus_Init(void); 9 | void Sbus_SetRcDataCallback(RcDataCallback rcDataCallback); 10 | void Sbus_Disable(void); 11 | 12 | #endif 13 | 14 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_spi.h: -------------------------------------------------------------------------------- 1 | #ifndef __DRV_SPI_H__ 2 | #define __DRV_SPI_H__ 3 | 4 | #include "board.h" 5 | 6 | void Spi_GPIO_Init(void); 7 | void Spi_Open(uint8_t deviceNum); 8 | uint8_t Spi_SingleWirteAndRead(uint8_t deviceNum, uint8_t dat); 9 | void SPI_MultiWriteAndRead(uint8_t deviceNum, uint8_t *out, uint8_t *in, int len); 10 | 11 | void Spi_GyroSingleWrite(uint8_t reg, uint8_t value); 12 | void Spi_GyroMultiRead(uint8_t reg,uint8_t *data, uint8_t length); 13 | void Spi_BaroSingleWrite(uint8_t reg, uint8_t value); 14 | void Spi_BaroMultiRead(uint8_t reg,uint8_t *data, uint8_t length); 15 | 16 | #endif 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_usart.h: -------------------------------------------------------------------------------- 1 | #ifndef _DRV_USART_H 2 | #define _DRV_USART_H 3 | 4 | #include "board.h" 5 | 6 | typedef void (*UsartCallback)(uint8_t data); 7 | 8 | void Usart_Open(uint8_t deviceNum, uint32_t baudrate); 9 | void Usart_SetIRQCallback(uint8_t deviceNum, UsartCallback usartCallback); 10 | void Usart_SendData(uint8_t deviceNum, uint8_t *DataToSend,uint8_t length); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /SRC/DRIVER/drv_usb.h: -------------------------------------------------------------------------------- 1 | #ifndef _DRV_USB_H 2 | #define _DRV_USB_H 3 | 4 | #include "board.h" 5 | 6 | typedef void (*UsbCallback)(uint8_t data); 7 | 8 | void Usb_Init(void); 9 | void Usb_Send(uint8_t *dataToSend, uint8_t length); 10 | void Usb_SetRecvCallback(UsbCallback usbCallback); 11 | 12 | #endif 13 | -------------------------------------------------------------------------------- /SRC/LOG/logger.h: -------------------------------------------------------------------------------- 1 | #ifndef _LOGGER_H_ 2 | #define _LOGGER_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | #define LOG_RATE 100 //日志记录频率 单位:Hz 7 | 8 | void LoggerInit(void); 9 | void LoggerLoop(void); 10 | void LoggerWrite(void *data, uint16_t size); 11 | 12 | #endif 13 | 14 | 15 | -------------------------------------------------------------------------------- /SRC/LOG/ulog.h: -------------------------------------------------------------------------------- 1 | #ifndef _ULOG_H_ 2 | #define _ULOG_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | enum ULogMessageType 7 | { 8 | FORMAT = 'F', 9 | DATA = 'D', 10 | INFO = 'I', 11 | INFO_MULTIPLE = 'M', 12 | PARAMETER = 'P', 13 | ADD_LOGGED_MSG = 'A', 14 | REMOVE_LOGGED_MSG = 'R', 15 | SYNC = 'S', 16 | DROPOUT = 'O', 17 | LOGGING = 'L', 18 | FLAG_BITS = 'B', 19 | }; 20 | 21 | #pragma pack (1) 22 | 23 | typedef struct 24 | { 25 | uint8_t magic[8]; 26 | uint64_t timestamp; 27 | } ulog_file_header_s; 28 | 29 | #define ULOG_MSG_HEADER_LEN 3 30 | typedef struct 31 | { 32 | uint16_t msg_size; 33 | uint8_t msg_type; 34 | } ulog_message_header_s; 35 | 36 | typedef struct 37 | { 38 | uint16_t msg_size; 39 | uint8_t msg_type; 40 | char format[2096]; 41 | } ulog_message_format_s; 42 | 43 | typedef struct 44 | { 45 | uint16_t msg_size; 46 | uint8_t msg_type; 47 | uint8_t multi_id; 48 | uint16_t msg_id; 49 | char message_name[255]; 50 | } ulog_message_add_logged_s; 51 | 52 | typedef struct { 53 | uint16_t msg_size; 54 | uint8_t msg_type; 55 | uint16_t msg_id; 56 | } ulog_message_remove_logged_s; 57 | 58 | typedef struct 59 | { 60 | uint16_t msg_size; 61 | uint8_t msg_type; 62 | uint8_t sync_magic[8]; 63 | } ulog_message_sync_s; 64 | 65 | typedef struct 66 | { 67 | uint16_t msg_size; 68 | uint8_t msg_type; 69 | uint16_t duration; 70 | } ulog_message_dropout_s; 71 | 72 | typedef struct 73 | { 74 | uint16_t msg_size; 75 | uint8_t msg_type; 76 | uint16_t msg_id; 77 | } ulog_message_data_header_s; 78 | 79 | typedef struct 80 | { 81 | uint16_t msg_size; 82 | uint8_t msg_type; 83 | uint8_t key_len; 84 | char key[255]; 85 | } ulog_message_info_header_s; 86 | 87 | typedef struct 88 | { 89 | uint16_t msg_size; 90 | uint8_t msg_type ; 91 | uint8_t is_continued; 92 | uint8_t key_len; 93 | char key[255]; 94 | } ulog_message_info_multiple_header_s; 95 | 96 | typedef struct 97 | { 98 | uint16_t msg_size; 99 | uint8_t msg_type; 100 | uint8_t log_level; 101 | uint64_t timestamp; 102 | char message[128]; 103 | } ulog_message_logging_s; 104 | 105 | typedef struct 106 | { 107 | uint16_t msg_size; 108 | uint8_t msg_type; 109 | uint8_t key_len; 110 | char key[255]; 111 | } ulog_message_parameter_header_s; 112 | 113 | 114 | #define ULOG_INCOMPAT_FLAG0_DATA_APPENDED_MASK (1<<0) 115 | 116 | typedef struct 117 | { 118 | uint16_t msg_size; 119 | uint8_t msg_type; 120 | uint8_t compat_flags[8]; 121 | uint8_t incompat_flags[8]; 122 | uint64_t appended_offsets[3]; 123 | } ulog_message_flag_bits_s; 124 | 125 | typedef struct 126 | { 127 | char* data_type; 128 | char* data_name; 129 | } ULOG_FORMAT_t; 130 | 131 | #pragma pack () 132 | 133 | void UlogWriteHeader(void); 134 | void UlogWriteFlag(void); 135 | void UlogWriteFormat(char* formatType, ULOG_FORMAT_t* ulog_format, int16_t dataNum); 136 | void UlogWriteAddLogged(char* logType, uint8_t msg_id); 137 | 138 | void UlogWriteData_Flight(void); 139 | void UlogWriteData_GPS(void); 140 | 141 | #endif 142 | 143 | 144 | -------------------------------------------------------------------------------- /SRC/LOG/ulog_data.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 ulog_data.c 6 | * @说明 ulog数据格式定义 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.08 11 | **********************************************************************************************************/ 12 | #include "ulog_data.h" 13 | 14 | ULOG_FORMAT_t ulog_data_flight[ULOG_DATA_FLIGHT_NUM] = 15 | { 16 | { 17 | .data_type = "uint64_t", 18 | .data_name = "timestamp" 19 | }, 20 | { 21 | .data_type = "int16_t", 22 | .data_name = "roll_rate" 23 | }, 24 | { 25 | .data_type = "int16_t", 26 | .data_name = "pitch_rate" 27 | }, 28 | { 29 | .data_type = "int16_t", 30 | .data_name = "yaw_rate" 31 | }, 32 | { 33 | .data_type = "int16_t", 34 | .data_name = "roll_rate_sp" 35 | }, 36 | { 37 | .data_type = "int16_t", 38 | .data_name = "pitch_rate_sp" 39 | }, 40 | { 41 | .data_type = "int16_t", 42 | .data_name = "yaw_rate_sp" 43 | }, 44 | { 45 | .data_type = "int16_t", 46 | .data_name = "roll" 47 | }, 48 | { 49 | .data_type = "int16_t", 50 | .data_name = "pitch" 51 | }, 52 | { 53 | .data_type = "int16_t", 54 | .data_name = "yaw" 55 | }, 56 | { 57 | .data_type = "int16_t", 58 | .data_name = "roll_sp" 59 | }, 60 | { 61 | .data_type = "int16_t", 62 | .data_name = "pitch_sp" 63 | }, 64 | { 65 | .data_type = "int16_t", 66 | .data_name = "yaw_sp" 67 | }, 68 | { 69 | .data_type = "int16_t[3]", 70 | .data_name = "accel" 71 | }, 72 | { 73 | .data_type = "int16_t[3]", 74 | .data_name = "velocity" 75 | }, 76 | { 77 | .data_type = "int16_t[3]", 78 | .data_name = "velocity_sp" 79 | }, 80 | { 81 | .data_type = "int32_t[3]", 82 | .data_name = "position" 83 | }, 84 | { 85 | .data_type = "int32_t[3]", 86 | .data_name = "position_sp" 87 | }, 88 | }; 89 | 90 | ULOG_FORMAT_t ulog_data_gps[ULOG_DATA_GPS_NUM] = 91 | { 92 | { 93 | .data_type = "uint64_t", 94 | .data_name = "timestamp" 95 | }, 96 | { 97 | .data_type = "int32_t", 98 | .data_name = "latitude" 99 | }, 100 | { 101 | .data_type = "int32_t", 102 | .data_name = "longitude" 103 | }, 104 | { 105 | .data_type = "int32_t", 106 | .data_name = "altitude" 107 | }, 108 | { 109 | .data_type = "int32_t", 110 | .data_name = "velN" 111 | }, 112 | { 113 | .data_type = "int32_t", 114 | .data_name = "velE" 115 | }, 116 | { 117 | .data_type = "int32_t", 118 | .data_name = "velD" 119 | }, 120 | { 121 | .data_type = "int16_t", 122 | .data_name = "heading" 123 | }, 124 | { 125 | .data_type = "int16_t", 126 | .data_name = "hAcc" 127 | }, 128 | { 129 | .data_type = "int16_t", 130 | .data_name = "vAcc" 131 | }, 132 | { 133 | .data_type = "int16_t", 134 | .data_name = "sAcc" 135 | }, 136 | { 137 | .data_type = "int16_t", 138 | .data_name = "cAcc" 139 | }, 140 | { 141 | .data_type = "uint8_t", 142 | .data_name = "fixState" 143 | }, 144 | { 145 | .data_type = "uint8_t", 146 | .data_name = "numSV" 147 | }, 148 | }; 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | -------------------------------------------------------------------------------- /SRC/LOG/ulog_data.h: -------------------------------------------------------------------------------- 1 | #ifndef _ULOG_DATA_H_ 2 | #define _ULOG_DATA_H_ 3 | 4 | #include "mathTool.h" 5 | #include "ulog.h" 6 | 7 | #define ULOG_DATA_FLIGHT_NUM 18 8 | #define ULOG_DATA_GPS_NUM 14 9 | 10 | #define ULOG_DATA_FLIGHT_ID 0 11 | #define ULOG_DATA_GPS_ID 1 12 | 13 | typedef struct 14 | { 15 | uint64_t timestamp; 16 | int16_t roll_rate; 17 | int16_t pitch_rate; 18 | int16_t yaw_rate; 19 | int16_t roll_rate_sp; 20 | int16_t pitch_rate_sp; 21 | int16_t yaw_rate_sp; 22 | int16_t roll; 23 | int16_t pitch; 24 | int16_t yaw; 25 | int16_t roll_sp; 26 | int16_t pitch_sp; 27 | int16_t yaw_sp; 28 | int16_t accel[3]; 29 | int16_t velocity[3]; 30 | int16_t velocity_sp[3]; 31 | int32_t position[3]; 32 | int32_t position_sp[3]; 33 | } ULOG_DATA_FLIGHT_t; 34 | 35 | typedef struct 36 | { 37 | uint64_t timestamp; 38 | int32_t latitude; //纬度 39 | int32_t longitude; //经度 40 | int32_t altitude; //高度 41 | int32_t velN; //北向速度 42 | int32_t velE; //东向速度 43 | int32_t velD; //天向速度 44 | int16_t heading; //航向 45 | int16_t hAcc; //水平定位精度 46 | int16_t vAcc; //垂直定位精度 47 | int16_t sAcc; //速度精度 48 | int16_t cAcc; //航向精度 49 | uint8_t fixStatus; //定位状态 50 | uint8_t numSV; //卫星数量 51 | } ULOG_DATA_GPS_t; 52 | 53 | extern ULOG_FORMAT_t ulog_data_flight[ULOG_DATA_FLIGHT_NUM]; 54 | extern ULOG_FORMAT_t ulog_data_gps[ULOG_DATA_GPS_NUM]; 55 | 56 | #endif 57 | 58 | 59 | -------------------------------------------------------------------------------- /SRC/MATH/LevenbergMarquardt.h: -------------------------------------------------------------------------------- 1 | #ifndef _LEVENBERGMARQUARDT_H 2 | #define _LEVENBERGMARQUARDT_H 3 | 4 | #include "mathTool.h" 5 | 6 | void LevenbergMarquardt(Vector3f_t inputData[6], Vector3f_t* offset, Vector3f_t* scale, float initBeta[6], float length); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /SRC/MATH/declination.h: -------------------------------------------------------------------------------- 1 | #ifndef _DECLINATION_H 2 | #define _DECLINATION_H 3 | 4 | #include "mathTool.h" 5 | 6 | float CompassGetDeclination(float lat, float lon); 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /SRC/MATH/kalman3.h: -------------------------------------------------------------------------------- 1 | #ifndef _KALMAN3_H_ 2 | #define _KALMAN3_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct { 7 | //状态矩阵 8 | Vector3f_t state; 9 | //滑动窗口大小 10 | int16_t slidWindowSize; 11 | //状态滑动窗口 12 | Vector3f_t* statusSlidWindow; 13 | //观测信号相位补偿值 14 | Vector3i_t fuseDelay; 15 | //残差矩阵 16 | Vector3f_t residual; 17 | //状态转移矩阵 18 | float f[9]; 19 | //状态转移矩阵的转置 20 | float f_t[9]; 21 | //观测映射矩阵 22 | float h[9]; 23 | //观测映射矩阵的转置 24 | float h_t[9]; 25 | //控制输入矩阵 26 | float b[9]; 27 | //卡尔曼增益矩阵 28 | float gain[9]; 29 | //误差协方差矩阵 30 | float covariance[9]; 31 | //过程噪声协方差矩阵 32 | float q[9]; 33 | //测量噪声协方差矩阵 34 | float r[9]; 35 | } Kalman_t; 36 | 37 | void KalmanUpdate(Kalman_t* kalman, Vector3f_t input, Vector3f_t observe, bool flag); 38 | 39 | void KalmanStateTransMatSet(Kalman_t* kalman, float* f); 40 | void KalmanObserveMapMatSet(Kalman_t* kalman, float* h); 41 | void KalmanCovarianceMatSet(Kalman_t* kalman, float* p); 42 | void KalmanQMatSet(Kalman_t* kalman, float* q); 43 | void KalmanRMatSet(Kalman_t* kalman, float* r); 44 | void KalmanBMatSet(Kalman_t* kalman, float* b); 45 | 46 | #endif 47 | -------------------------------------------------------------------------------- /SRC/MATH/kalmanVel.h: -------------------------------------------------------------------------------- 1 | #ifndef _KALMANVEL_H_ 2 | #define _KALMANVEL_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct { 7 | //状态矩阵 8 | //状态量为:x轴速度 y轴速度 z轴速度 x轴加速度bias y轴加速度bias z轴加速度bias 9 | float state[6]; 10 | //滑动窗口大小 11 | int16_t slidWindowSize; 12 | //状态滑动窗口 13 | Vector3f_t* stateSlidWindow; 14 | //观测信号相位补偿值 15 | int16_t fuseDelay[6]; 16 | //残差矩阵 17 | float residual[6]; 18 | //状态转移矩阵 19 | float f[6][6]; 20 | //状态转移矩阵的转置 21 | float f_t[6][6]; 22 | //观测映射矩阵 23 | float h[6][6]; 24 | //观测映射矩阵的转置 25 | float h_t[6][6]; 26 | //控制输入矩阵 27 | float b[6][6]; 28 | //卡尔曼增益矩阵 29 | float gain[6][6]; 30 | //误差协方差矩阵 31 | float covariance[6][6]; 32 | //过程噪声协方差矩阵 33 | float q[6][6]; 34 | //测量噪声协方差矩阵 35 | float r[6][6]; 36 | } KalmanVel_t; 37 | 38 | void KalmanVelUpdate(KalmanVel_t* kalman, Vector3f_t* velocity, Vector3f_t* bias, Vector3f_t accel, 39 | float observe[6], float deltaT, bool fuseFlag); 40 | 41 | void KalmanVelStateTransMatSet(KalmanVel_t* kalman, float f[6][6]); 42 | void KalmanVelObserveMapMatSet(KalmanVel_t* kalman, float h[6][6]); 43 | void KalmanVelCovarianceMatSet(KalmanVel_t* kalman, float p[6][6]); 44 | void KalmanVelQMatSet(KalmanVel_t* kalman, float q[6][6]); 45 | void KalmanVelRMatSet(KalmanVel_t* kalman, float r[6][6]); 46 | void KalmanVelBMatSet(KalmanVel_t* kalman, float b[6][6]); 47 | 48 | void KalmanVelUseMeasurement(KalmanVel_t* kalman, uint8_t num, bool flag); 49 | 50 | #endif 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /SRC/MATH/lowPassFilter.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 lowPassFilter.c 6 | * @说明 低通滤波器 7 | * @版本 V1.1 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.07 11 | **********************************************************************************************************/ 12 | #include "lowPassFilter.h" 13 | 14 | /********************************************************************************************************** 15 | *函 数 名: LowPassFilter1st 16 | *功能说明: 一阶低通滤波器 17 | *形 参: 数据指针 新数据 滤波系数(新数据权重) 18 | *返 回 值: 无 19 | **********************************************************************************************************/ 20 | void LowPassFilter1st(Vector3f_t* data, Vector3f_t newData, float coff) 21 | { 22 | data->x = data->x * (1 - coff) + newData.x * coff; 23 | data->y = data->y * (1 - coff) + newData.y * coff; 24 | data->z = data->z * (1 - coff) + newData.z * coff; 25 | } 26 | 27 | /********************************************************************************************************** 28 | *函 数 名: LowPassFilter2ndFactorCal 29 | *功能说明: 二阶IIR低通滤波器系数计算,该滤波器由二阶模拟低通滤波电路数字化推导而来 30 | *形 参: 滤波器运行频率 截止频率 滤波器结构体指针 31 | *返 回 值: 无 32 | **********************************************************************************************************/ 33 | void LowPassFilter2ndFactorCal(float deltaT, float Fcut, LPF2ndData_t* lpf_data) 34 | { 35 | float a = 1 / (2 * M_PI * Fcut * deltaT); 36 | lpf_data->b0 = 1 / (a*a + 3*a + 1); 37 | lpf_data->a1 = (2*a*a + 3*a) / (a*a + 3*a + 1); 38 | lpf_data->a2 = (a*a) / (a*a + 3*a + 1); 39 | } 40 | 41 | /********************************************************************************************************** 42 | *函 数 名: LowPassFilter2nd 43 | *功能说明: 二阶IIR低通滤波器过程实现 44 | *形 参: 滤波器结构体指针 原始数据 45 | *返 回 值: 经过滤波的数据 46 | **********************************************************************************************************/ 47 | Vector3f_t LowPassFilter2nd(LPF2ndData_t* lpf_2nd, Vector3f_t rawData) 48 | { 49 | Vector3f_t lpf_2nd_data; 50 | 51 | lpf_2nd_data.x = rawData.x * lpf_2nd->b0 + lpf_2nd->lastout.x * lpf_2nd->a1 - lpf_2nd->preout.x * lpf_2nd->a2; 52 | lpf_2nd_data.y = rawData.y * lpf_2nd->b0 + lpf_2nd->lastout.y * lpf_2nd->a1 - lpf_2nd->preout.y * lpf_2nd->a2; 53 | lpf_2nd_data.z = rawData.z * lpf_2nd->b0 + lpf_2nd->lastout.z * lpf_2nd->a1 - lpf_2nd->preout.z * lpf_2nd->a2; 54 | 55 | lpf_2nd->preout.x = lpf_2nd->lastout.x; 56 | lpf_2nd->preout.y = lpf_2nd->lastout.y; 57 | lpf_2nd->preout.z = lpf_2nd->lastout.z; 58 | 59 | lpf_2nd->lastout.x = lpf_2nd_data.x; 60 | lpf_2nd->lastout.y = lpf_2nd_data.y; 61 | lpf_2nd->lastout.z = lpf_2nd_data.z; 62 | 63 | return lpf_2nd_data; 64 | } 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /SRC/MATH/lowPassFilter.h: -------------------------------------------------------------------------------- 1 | #ifndef _LOWPASSFILTER_H_ 2 | #define _LOWPASSFILTER_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct 7 | { 8 | float b0; 9 | float a1; 10 | float a2; 11 | Vector3f_t preout; 12 | Vector3f_t lastout; 13 | } LPF2ndData_t; 14 | 15 | void LowPassFilter1st(Vector3f_t* data, Vector3f_t newData, float coff); 16 | void LowPassFilter2ndFactorCal(float deltaT, float Fcut, LPF2ndData_t* lpf_data); 17 | Vector3f_t LowPassFilter2nd(LPF2ndData_t* lpf_2nd, Vector3f_t rawData); 18 | 19 | #endif 20 | 21 | 22 | -------------------------------------------------------------------------------- /SRC/MATH/mathTool.h: -------------------------------------------------------------------------------- 1 | #ifndef __MATHTOOL_H__ 2 | #define __MATHTOOL_H__ 3 | 4 | #include 5 | #include 6 | #include "stdbool.h" 7 | #include 8 | 9 | #include "vector3.h" 10 | 11 | #define min(a, b) ((a) < (b) ? (a) : (b)) 12 | #define max(a, b) ((a) > (b) ? (a) : (b)) 13 | #define abs(x) ((x) > 0 ? (x) : -(x)) 14 | 15 | #define M_PI 3.141592653f //圆周率 16 | #define DEG_TO_RAD 0.01745329f //角度转弧度 17 | #define RAD_TO_DEG 57.29577951f //弧度转角度 18 | 19 | #define EARTH_RADIUS 6371.004f //km 20 | #define GRAVITY_ACCEL 9.8f //重力加速度 单位:m/s^2 21 | 22 | #define HALF_SQRT_2 0.70710678118654757f //根号2的值 23 | 24 | float SafeArcsin(float v); 25 | float ConstrainFloat(float amt, float low, float high); 26 | int16_t ConstrainInt16(int16_t amt, int16_t low, int16_t high); 27 | uint16_t ConstrainUint16(uint16_t amt, uint16_t low, uint16_t high); 28 | int32_t ConstrainInt32(int32_t amt, int32_t low, int32_t high); 29 | 30 | int32_t ApplyDeadbandInt(int32_t value, int32_t deadband); 31 | float ApplyDeadbandFloat(float value, float deadband); 32 | 33 | float Radians(float deg); 34 | float Degrees(float rad); 35 | 36 | float Sq(float v); 37 | float Pythagorous2(float a, float b); 38 | float Pythagorous3(float a, float b, float c); 39 | float Pythagorous4(float a, float b, float c, float d); 40 | 41 | float WrapDegree360(float angle); 42 | 43 | int32_t GetRandom(void); 44 | 45 | #endif 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /SRC/MATH/matrix3.h: -------------------------------------------------------------------------------- 1 | #ifndef _MATRIX3_H_ 2 | #define _MATRIX3_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void Matrix3_Add(float* a,float* b,float* c); 7 | void Matrix3_Sub(float* a,float* b,float* c); 8 | void Matrix3_Mul(float* a,float* b,float* c); 9 | void Matrix3_Copy(float* a, float* b); 10 | void Matrix3_Tran(float* a, float* b); 11 | void Matrix3_Det(float* a,float* b); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /SRC/MATH/matrix6.h: -------------------------------------------------------------------------------- 1 | #ifndef _MATRIX6_H_ 2 | #define _MATRIX6_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void Matrix6_Add(float a[6][6], float b[6][6], float c[6][6]); 7 | void Matrix6_Sub(float a[6][6], float b[6][6], float c[6][6]); 8 | void Matrix6_Mul(float a[6][6], float b[6][6], float c[6][6]); 9 | void Matrix6_Copy(float a[6][6], float b[6][6]); 10 | void Matrix6_Tran(float a[6][6], float b[6][6]); 11 | bool Matrix6_Det(float a[6][6], float b[6][6]); 12 | 13 | void Vector6f_Add(float v1[6], float v2[6], float v3[6]); 14 | void Vector6f_Sub(float v1[6], float v2[6], float v3[6]); 15 | 16 | void Matrix6MulVector6(float m[6][6], float v[6], float result[6]); 17 | 18 | #endif 19 | 20 | -------------------------------------------------------------------------------- /SRC/MATH/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef _PID_H 2 | #define _PID_H 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct 7 | { 8 | float kP; 9 | float kI; 10 | float kD; 11 | 12 | float imax; 13 | float integrator; 14 | float lastError; 15 | float lastDerivative; 16 | float dFilter; 17 | } PID_t; 18 | 19 | float PID_GetP(PID_t* pid, float error); 20 | float PID_GetI(PID_t* pid, float error, float dt); 21 | void PID_ResetI(PID_t* pid); 22 | float PID_GetD(PID_t* pid, float error, float dt); 23 | float PID_GetPI(PID_t* pid, float error, float dt); 24 | float PID_GetPID(PID_t* pid, float error, float dt); 25 | void PID_SetParam(PID_t* pid, float p, float i, float d, float imaxval, float dCutFreq) ; 26 | 27 | #endif 28 | 29 | -------------------------------------------------------------------------------- /SRC/MATH/quaternion.h: -------------------------------------------------------------------------------- 1 | #ifndef __QUATERNION_H__ 2 | #define __QUATERNION_H__ 3 | 4 | #include "mathTool.h" 5 | 6 | void EulerAngleToQuaternion(Vector3f_t angle, float q[4]); 7 | void QuaternionToDCM(float q[4], float dcM[9]); 8 | void QuaternionToDCM_T(float q[4], float dcM[9]); 9 | Vector3f_t QuaternionRotateToEarthFrame(float q[4], Vector3f_t vector); 10 | Vector3f_t QuaternionRotateToBodyFrame(float q[4], Vector3f_t vector); 11 | void QuaternionToEulerAngle(float q[4], Vector3f_t* angle); 12 | void QuaternionNormalize(float q[4]); 13 | 14 | #endif 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /SRC/MATH/rotation.h: -------------------------------------------------------------------------------- 1 | #ifndef _ROTATION_H 2 | #define _ROTATION_H 3 | 4 | #include "mathTool.h" 5 | 6 | enum Rotation 7 | { 8 | ROTATION_NONE = 0, 9 | ROTATION_YAW_45 = 1, 10 | ROTATION_YAW_90 = 2, 11 | ROTATION_YAW_135 = 3, 12 | ROTATION_YAW_180 = 4, 13 | ROTATION_YAW_225 = 5, 14 | ROTATION_YAW_270 = 6, 15 | ROTATION_YAW_315 = 7, 16 | ROTATION_ROLL_180 = 8, 17 | ROTATION_ROLL_180_YAW_45 = 9, 18 | ROTATION_ROLL_180_YAW_90 = 10, 19 | ROTATION_ROLL_180_YAW_135 = 11, 20 | ROTATION_PITCH_180 = 12, 21 | ROTATION_ROLL_180_YAW_225 = 13, 22 | ROTATION_ROLL_180_YAW_270 = 14, 23 | ROTATION_ROLL_180_YAW_315 = 15, 24 | ROTATION_ROLL_90 = 16, 25 | ROTATION_ROLL_90_YAW_45 = 17, 26 | ROTATION_ROLL_90_YAW_90 = 18, 27 | ROTATION_ROLL_90_YAW_135 = 19, 28 | ROTATION_ROLL_270 = 20, 29 | ROTATION_ROLL_270_YAW_45 = 21, 30 | ROTATION_ROLL_270_YAW_90 = 22, 31 | ROTATION_ROLL_270_YAW_135 = 23, 32 | ROTATION_PITCH_90 = 24, 33 | ROTATION_PITCH_270 = 25, 34 | ROTATION_ROLL_270_YAW_270 = 26, 35 | ROTATION_ROLL_180_PITCH_270 = 27, 36 | ROTATION_PITCH_90_YAW_180 = 28, 37 | ROTATION_PITCH_90_ROLL_90 = 29, 38 | ROTATION_YAW_293_PITCH_68_ROLL_90 = 30, 39 | ROTATION_PITCH_90_ROLL_270 = 31, 40 | ROTATION_MAX 41 | }; 42 | 43 | void RotateVector3f(enum Rotation rot, Vector3f_t* v); 44 | 45 | #endif 46 | 47 | -------------------------------------------------------------------------------- /SRC/MATH/vector3.h: -------------------------------------------------------------------------------- 1 | #ifndef __VECTOR3_H__ 2 | #define __VECTOR3_H__ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct { 7 | float x; 8 | float y; 9 | float z; 10 | } Vector3f_t; 11 | 12 | typedef struct { 13 | double x; 14 | double y; 15 | double z; 16 | } Vector3d_t; 17 | 18 | typedef struct { 19 | int16_t x; 20 | int16_t y; 21 | int16_t z; 22 | } Vector3i_t; 23 | 24 | typedef struct { 25 | int32_t x; 26 | int32_t y; 27 | int32_t z; 28 | } Vector3l_t; 29 | 30 | void Vector3f_Normalize(Vector3f_t* vector); 31 | 32 | Vector3f_t Vector3iTo3f(Vector3i_t vector); 33 | Vector3i_t Vector3fTo3i(Vector3f_t vector); 34 | Vector3f_t Vector3f_Add(Vector3f_t v1, Vector3f_t v2); 35 | Vector3f_t Vector3f_Sub(Vector3f_t v1, Vector3f_t v2); 36 | 37 | Vector3f_t VectorCrossProduct(Vector3f_t a, Vector3f_t b); 38 | Vector3f_t Matrix3MulVector3(float* m, Vector3f_t vector); 39 | Vector3f_t VectorRotateToBodyFrame(Vector3f_t vector, Vector3f_t deltaAngle); 40 | Vector3f_t VectorRotateToEarthFrame(Vector3f_t vector, Vector3f_t deltaAngle); 41 | 42 | void EulerAngleToDCM(Vector3f_t angle, float* dcM); 43 | 44 | void AccVectorToRollPitchAngle(Vector3f_t* angle, Vector3f_t vector); 45 | void MagVectorToYawAngle(Vector3f_t* angle, Vector3f_t vector); 46 | 47 | #endif 48 | 49 | 50 | -------------------------------------------------------------------------------- /SRC/MESSAGE/bsklinkDecode.h: -------------------------------------------------------------------------------- 1 | #ifndef _BSKLINKDECODE_H_ 2 | #define _BSKLINKDECODE_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void BsklinkDecode(uint8_t data); 7 | 8 | #endif 9 | -------------------------------------------------------------------------------- /SRC/MESSAGE/bsklinkSend.h: -------------------------------------------------------------------------------- 1 | #ifndef _BSKLINKSEND_H_ 2 | #define _BSKLINKSEND_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void BsklinkSendFlightData(uint8_t* sendFlag); 7 | void BsklinkSendFlightStatus(uint8_t* sendFlag); 8 | void BsklinkSendSensor(uint8_t* sendFlag); 9 | void BsklinkSendSensorCaliData(uint8_t* sendFlag); 10 | void BsklinkSendSensorCaliCmd(uint8_t* sendFlag, uint8_t type, uint8_t step, uint8_t success); 11 | void BsklinkSendGps(uint8_t* sendFlag); 12 | void BsklinkSendRcData(uint8_t* sendFlag); 13 | void BsklinkSendMotor(uint8_t* sendFlag); 14 | void BsklinkSendBattery(uint8_t* sendFlag); 15 | void BsklinkSendPidAtt(uint8_t* sendFlag); 16 | void BsklinkSendPidPos(uint8_t* sendFlag); 17 | void BsklinkSendPidAck(uint8_t* sendFlag); 18 | void BsklinkSetPidAck(uint8_t ack); 19 | void BsklinkSendSysError(uint8_t* sendFlag); 20 | void BsklinkSendSysWarning(uint8_t* sendFlag); 21 | void BsklinkSendAttAnalyse(uint8_t* sendFlag); 22 | void BsklinkSendVelAnalyse(uint8_t* sendFlag); 23 | void BsklinkSendPosAnalyse(uint8_t* sendFlag); 24 | void BsklinkSendUserDefine(uint8_t* sendFlag); 25 | void BsklinkSendFreqSetup(uint8_t* sendFlag); 26 | void BsklinkSendHeartBeat(uint8_t* sendFlag); 27 | 28 | #endif 29 | 30 | 31 | -------------------------------------------------------------------------------- /SRC/MESSAGE/mavlinkDecode.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAVLINKDECODE_H_ 2 | #define _MAVLINKDECODE_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | void MavlinkDecode(uint8_t data); 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /SRC/MESSAGE/mavlinkNotice.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAVLINKNOTICE_H_ 2 | #define _MAVLINKNOTICE_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | enum MAV_NOTICE_TYPE 7 | { 8 | CAL_START_GYRO, 9 | CAL_START_ACC, 10 | CAL_START_MAG, 11 | CAL_START_LEVEL, 12 | CAL_DONE, 13 | CAL_FAILED, 14 | CAL_PROGRESS_0, 15 | CAL_PROGRESS_10, 16 | CAL_PROGRESS_20, 17 | CAL_PROGRESS_30, 18 | CAL_PROGRESS_40, 19 | CAL_PROGRESS_50, 20 | CAL_PROGRESS_60, 21 | CAL_PROGRESS_70, 22 | CAL_PROGRESS_80, 23 | CAL_PROGRESS_90, 24 | CAL_PROGRESS_100, 25 | CAL_UP_DETECTED, 26 | CAL_DOWN_DETECTED, 27 | CAL_LEFT_DETECTED, 28 | CAL_RIGHT_DETECTED, 29 | CAL_FRONT_DETECTED, 30 | CAL_BACK_DETECTED, 31 | CAL_UP_DONE, 32 | CAL_DOWN_DONE, 33 | CAL_LEFT_DONE, 34 | CAL_RIGHT_DONE, 35 | CAL_FRONT_DONE, 36 | CAL_BACK_DONE, 37 | INIT_WELCOME, 38 | INIT_INIT_START, 39 | INIT_INIT_FINISH, 40 | INIT_HEATING, 41 | INIT_HEAT_FINISH, 42 | INIT_HEAT_DISABLE, 43 | SENSOR_CHECK_START, 44 | SENSOR_CHECK_ERROR, 45 | SENSOR_CHECK_FINISH, 46 | SENSOR_GYRO_UNDETECTED, 47 | SENSOR_GYRO_UNCALI, 48 | SENSOR_GYRO_NEED_CALI, 49 | SENSOR_GYRO_OK, 50 | SENSOR_ACC_UNCALI, 51 | SENSOR_ACC_NEED_CALI, 52 | SENSOR_ACC_OK, 53 | SENSOR_MAG_UNDETECTED, 54 | SENSOR_MAG_UNCALI, 55 | SENSOR_MAG_NEED_CALI, 56 | SENSOR_MAG_OK, 57 | SENSOR_BARO_UNDETECTED, 58 | SENSOR_BARO_OK, 59 | SENSOR_GPS_UNDETECTED, 60 | SENSOR_GPS_OK, 61 | MAV_NOTICE_NUM 62 | }; 63 | 64 | typedef struct { 65 | uint8_t severity; 66 | char* strings; 67 | } MAV_NOTICE_t; 68 | 69 | MAV_NOTICE_t GetMavNoticeValue(uint8_t noticeNum); 70 | 71 | #endif 72 | 73 | -------------------------------------------------------------------------------- /SRC/MESSAGE/mavlinkSend.h: -------------------------------------------------------------------------------- 1 | #ifndef _MAVLINKSEND_H_ 2 | #define _MAVLINKSEND_H_ 3 | 4 | #include "mathTool.h" 5 | #include "mavlinkNotice.h" 6 | 7 | void MavlinkSendHeartbeat(uint8_t* sendFlag); 8 | void MavlinkSendSysStatus(uint8_t* sendFlag); 9 | void MavlinkSendParamValue(uint8_t* sendFlag); 10 | void MavlinkSendGpsRawInt(uint8_t* sendFlag); 11 | void MavlinkSendScaledImu(uint8_t* sendFlag); 12 | void MavlinkSendAttitude(uint8_t* sendFlag); 13 | void MavlinkSendLocalPositionNed(uint8_t* sendFlag); 14 | void MavlinkSendRcChannels(uint8_t* sendFlag); 15 | void MavlinkSendCommandAck(uint8_t* sendFlag); 16 | void MavlinkSendMissionRequest(uint8_t* sendFlag); 17 | void MavlinkSendMissionAck(uint8_t* sendFlag); 18 | void MavlinkSendMissionCount(uint8_t* sendFlag); 19 | void MavlinkSendMissionItem(uint8_t* sendFlag); 20 | void MavlinkSendHomePosition(uint8_t* sendFlag); 21 | void MavlinkSendVfrHud(uint8_t* sendFlag); 22 | void MavlinkSendStatusText(uint8_t* sendFlag); 23 | 24 | void MavlinkCurrentParamSet(uint16_t num); 25 | void MavlinkSetCommandAck(uint16_t command, uint8_t result); 26 | bool MavStatusTextSendCheck(void); 27 | void MavlinkSendNoticeEnable(uint16_t noticeNum); 28 | void MavlinkSendNotice(uint16_t noticeNum); 29 | void MavlinkSendNoticeProgress(uint8_t progress); 30 | 31 | #endif 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /SRC/MESSAGE/message.h: -------------------------------------------------------------------------------- 1 | #ifndef _MESSAGE_H_ 2 | #define _MESSAGE_H_ 3 | 4 | #include "mathTool.h" 5 | #include "bsklink.h" 6 | #include "common/mavlink.h" 7 | 8 | #define MAX_SEND_FREQ 100 //最大发送频率 单位:Hz 9 | 10 | #define MAVLINK_SYSTEM_ID 1 11 | #define MAVLINK_COMPONENT_ID MAV_COMP_ID_AUTOPILOT1 12 | 13 | typedef union 14 | { 15 | int8_t i8; 16 | int16_t i16; 17 | int32_t i32; 18 | float f32; 19 | uint8_t byte[4]; 20 | } DATA_TYPE_t; 21 | 22 | void MessageInit(void); 23 | void MessageSendLoop(void); 24 | void MessageSensorCaliFeedbackEnable(uint8_t type, uint8_t step, uint8_t success); 25 | void BsklinkSetMsgFreq(BSKLINK_MSG_ID_FREQ_SETUP_t payload); 26 | void BsklinkSendEnable(uint8_t msgid); 27 | void MavlinkSendEnable(uint8_t msgid); 28 | void DataSend(uint8_t *data, uint8_t length); 29 | bool GetMessageStatus(void); 30 | 31 | #endif 32 | 33 | 34 | -------------------------------------------------------------------------------- /SRC/MODULE/2smpb.h: -------------------------------------------------------------------------------- 1 | #ifndef __2SMPB_H 2 | #define __2SMPB_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool _2SMPB_Detect(void); 7 | void _2SMPB_Init(void); 8 | 9 | void _2SMPB_Update(void); 10 | void _2SMPB_Read(int32_t* baroAlt); 11 | void _2SMPB_ReadTemp(float* temp); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /SRC/MODULE/battery.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 battery.c 6 | * @说明 电池电压电流检测 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.06 11 | **********************************************************************************************************/ 12 | #include "battery.h" 13 | #include "drv_adc.h" 14 | 15 | #define VOLTAGE_LOW 360 * 4 16 | #define VOLTAGE_CRITICAL_LOW 340 * 4 17 | 18 | static float batVoltage; 19 | static float batCurrent; 20 | 21 | /********************************************************************************************************** 22 | *函 数 名: BatteryVoltageUpdate 23 | *功能说明: 电池电压采样更新 24 | *形 参: 无 25 | *返 回 值: 无 26 | **********************************************************************************************************/ 27 | void BatteryVoltageUpdate(void) 28 | { 29 | if(batVoltage == 0) 30 | { 31 | batVoltage = GetVoltageAdcValue() * ADC_VOLTAGE_COEF; 32 | } 33 | else 34 | { 35 | batVoltage = batVoltage * 0.999f + GetVoltageAdcValue() * ADC_VOLTAGE_COEF * 0.001f; 36 | } 37 | } 38 | 39 | /********************************************************************************************************** 40 | *函 数 名: BatteryCurrentUpdate 41 | *功能说明: 电池电流采样更新 42 | *形 参: 无 43 | *返 回 值: 无 44 | **********************************************************************************************************/ 45 | void BatteryCurrentUpdate(void) 46 | { 47 | if(batCurrent == 0) 48 | { 49 | batCurrent = GetCurrentAdcValue() * ADC_CURRENT_COEF; 50 | } 51 | else 52 | { 53 | batCurrent = batCurrent * 0.99f + GetCurrentAdcValue() * ADC_CURRENT_COEF * 0.01f; 54 | } 55 | } 56 | 57 | /********************************************************************************************************** 58 | *函 数 名: GetBatteryVoltage 59 | *功能说明: 获取电池电压 60 | *形 参: 无 61 | *返 回 值: 电压值 62 | **********************************************************************************************************/ 63 | int16_t GetBatteryVoltage(void) 64 | { 65 | return batVoltage; 66 | } 67 | 68 | /********************************************************************************************************** 69 | *函 数 名: GetBatteryCurrent 70 | *功能说明: 获取电池电流 71 | *形 参: 无 72 | *返 回 值: 电压值 73 | **********************************************************************************************************/ 74 | int16_t GetBatteryCurrent(void) 75 | { 76 | return batCurrent; 77 | } 78 | 79 | /********************************************************************************************************** 80 | *函 数 名: GetBatteryStatus 81 | *功能说明: 获取电池状态 82 | *形 参: 无 83 | *返 回 值: 状态 84 | **********************************************************************************************************/ 85 | uint8_t GetBatteryStatus(void) 86 | { 87 | if(batVoltage < VOLTAGE_CRITICAL_LOW) 88 | { 89 | return BATTERY_CRITICAL_LOW; 90 | } 91 | else if(batVoltage < VOLTAGE_LOW) 92 | { 93 | return BATTERY_LOW; 94 | } 95 | else 96 | { 97 | return BATTERY_NORMAL; 98 | } 99 | } 100 | -------------------------------------------------------------------------------- /SRC/MODULE/battery.h: -------------------------------------------------------------------------------- 1 | #ifndef __BATTERY_H 2 | #define __BATTERY_H 3 | 4 | #include "mathTool.h" 5 | 6 | enum 7 | { 8 | BATTERY_NORMAL, 9 | BATTERY_LOW, 10 | BATTERY_CRITICAL_LOW 11 | }; 12 | 13 | void BatteryVoltageUpdate(void); 14 | void BatteryCurrentUpdate(void); 15 | 16 | int16_t GetBatteryVoltage(void); 17 | int16_t GetBatteryCurrent(void); 18 | uint8_t GetBatteryStatus(void); 19 | 20 | #endif 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /SRC/MODULE/icm20602.h: -------------------------------------------------------------------------------- 1 | #ifndef __ICM20602_H 2 | #define __ICM20602_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool ICM20602_Detect(void); 7 | void ICM20602_Init(void); 8 | 9 | void ICM20602_ReadAcc(Vector3f_t* acc); 10 | void ICM20602_ReadGyro(Vector3f_t* gyro); 11 | void ICM20602_ReadTemp(float* temp); 12 | 13 | #endif 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /SRC/MODULE/icm20689.h: -------------------------------------------------------------------------------- 1 | #ifndef __ICM20689_H 2 | #define __ICM20689_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool ICM20689_Detect(void); 7 | void ICM20689_Init(void); 8 | 9 | void ICM20689_ReadAcc(Vector3f_t* acc); 10 | void ICM20689_ReadGyro(Vector3f_t* gyro); 11 | void ICM20689_ReadTemp(float* temp); 12 | 13 | #endif 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /SRC/MODULE/ist8310.h: -------------------------------------------------------------------------------- 1 | #ifndef __IST8310_H 2 | #define __IST8310_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool IST8310_Detect(void); 7 | void IST8310_Init(void); 8 | void IST8310_Update(void); 9 | void IST8310_Read(Vector3f_t* mag); 10 | 11 | 12 | #endif 13 | 14 | 15 | -------------------------------------------------------------------------------- /SRC/MODULE/mmc3630.h: -------------------------------------------------------------------------------- 1 | #ifndef __MMC3630_H 2 | #define __MMC3630_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool MMC3630_Detect(void); 7 | void MMC3630_Init(void); 8 | void MMC3630_Update(void); 9 | void MMC3630_Read(Vector3f_t* mag); 10 | 11 | #endif 12 | 13 | 14 | -------------------------------------------------------------------------------- /SRC/MODULE/module.h: -------------------------------------------------------------------------------- 1 | #ifndef __MODULE_H__ 2 | #define __MODULE_H__ 3 | 4 | #include "mathTool.h" 5 | 6 | void GyroSensorInit(void); 7 | void MagSensorInit(void); 8 | void BaroSensorInit(void); 9 | void GPSModuleInit(void); 10 | 11 | void GyroSensorRead(Vector3f_t* gyro); 12 | void AccSensorRead(Vector3f_t* acc); 13 | void TempSensorRead(float* temp); 14 | 15 | void MagSensorUpdate(void); 16 | void MagSensorRead(Vector3f_t* mag); 17 | void BaroSensorRead(int32_t* baroAlt); 18 | void BaroSensorUpdate(void); 19 | void BaroTemperatureRead(float* temp); 20 | 21 | void TempControlSet(int16_t value); 22 | 23 | #endif 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /SRC/MODULE/mpu6000.h: -------------------------------------------------------------------------------- 1 | #ifndef __MPU6000_H 2 | #define __MPU6000_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool MPU6000_Detect(void); 7 | void MPU6000_Init(void); 8 | 9 | void MPU6000_ReadAcc(Vector3f_t* acc); 10 | void MPU6000_ReadGyro(Vector3f_t* gyro); 11 | void MPU6000_ReadTemp(float* temp); 12 | 13 | #endif 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /SRC/MODULE/mpu6500.h: -------------------------------------------------------------------------------- 1 | #ifndef __MPU6500_H 2 | #define __MPU6500_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool MPU6500_Detect(void); 7 | void MPU6500_Init(void); 8 | 9 | void MPU6500_ReadAcc(Vector3f_t* acc); 10 | void MPU6500_ReadGyro(Vector3f_t* gyro); 11 | void MPU6500_ReadTemp(float* temp); 12 | 13 | #endif 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /SRC/MODULE/ms5611.h: -------------------------------------------------------------------------------- 1 | #ifndef __MS5611_H 2 | #define __MS5611_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool MS5611_Detect(void); 7 | void MS5611_Init(void); 8 | 9 | void MS5611_Update(void); 10 | void MS5611_Read(int32_t* baroAlt); 11 | void MS5611_ReadTemp(float* temp); 12 | 13 | #endif 14 | -------------------------------------------------------------------------------- /SRC/MODULE/qmc5883.h: -------------------------------------------------------------------------------- 1 | #ifndef __QMC5883_H 2 | #define __QMC5883_H 3 | 4 | #include "mathTool.h" 5 | 6 | bool QMC5883_Detect(void); 7 | void QMC5883_Init(void); 8 | void QMC5883_Update(void); 9 | void QMC5883_Read(Vector3f_t* mag); 10 | 11 | 12 | #endif 13 | 14 | 15 | -------------------------------------------------------------------------------- /SRC/MODULE/rgb.h: -------------------------------------------------------------------------------- 1 | #ifndef __RGB_H 2 | #define __RGB_H 3 | 4 | #include "mathTool.h" 5 | 6 | void RGB_Init(void); 7 | void RGB_Flash(void); 8 | 9 | #endif 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /SRC/NAVIGATION/ahrs.h: -------------------------------------------------------------------------------- 1 | #ifndef _AHRS_H_ 2 | #define _AHRS_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct { 7 | Vector3f_t angle; 8 | Vector3f_t angleError; 9 | Vector3f_t angleMeasure; 10 | 11 | Vector3f_t accEf; 12 | Vector3f_t accEfLpf; 13 | Vector3f_t accBfOffset; 14 | 15 | Vector3f_t gyroEf; 16 | 17 | Vector3f_t centripetalAcc; 18 | Vector3f_t centripetalAccBf; 19 | } AHRS_t; 20 | 21 | void AHRSInit(void); 22 | void AttitudeEstimate(Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag); 23 | void BodyFrameToEarthFrame(Vector3f_t angle, Vector3f_t vector, Vector3f_t* vectorEf); 24 | void EarthFrameToBodyFrame(Vector3f_t angle, Vector3f_t vector, Vector3f_t* vectorBf); 25 | 26 | void AttCovarianceSelfAdaptation(void); 27 | 28 | Vector3f_t GetCopterAngle(void); 29 | Vector3f_t GetCopterAccEf(void); 30 | Vector3f_t GetCopterAccEfLpf(void); 31 | Vector3f_t GetCentripetalAcc(void); 32 | Vector3f_t GetCentripetalAccBf(void); 33 | Vector3f_t GetAngleMeasure(void); 34 | Vector3f_t GetAngleEstError(void); 35 | 36 | uint8_t RollOverDetect(void); 37 | 38 | #endif 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /SRC/NAVIGATION/ahrsAux.h: -------------------------------------------------------------------------------- 1 | #ifndef _AHRSAUX_H_ 2 | #define _AHRSAUX_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | typedef struct { 7 | Vector3f_t angle; 8 | float q[4]; 9 | Vector3f_t angleError; 10 | 11 | Vector3f_t vectorRollPitch; 12 | 13 | Vector3f_t accEf; 14 | } AHRSAUX_t; 15 | 16 | void AHRSAuxInit(void); 17 | void AttitudeAuxEstimate(Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag); 18 | 19 | void RollPitchUpdateByKF(Vector3f_t* angle, Vector3f_t gyro, Vector3f_t acc, float deltaT); 20 | void QuaternionUpdateByCF(float q[4], Vector3f_t* angle, Vector3f_t gyro, Vector3f_t acc, Vector3f_t mag, float deltaT); 21 | 22 | Vector3f_t GetSportAccEf(void); 23 | Vector3f_t GetAuxAngle(void); 24 | 25 | #endif 26 | 27 | 28 | -------------------------------------------------------------------------------- /SRC/NAVIGATION/navigation.h: -------------------------------------------------------------------------------- 1 | #ifndef _NAVIGATION_H_ 2 | #define _NAVIGATION_H_ 3 | 4 | #include "mathTool.h" 5 | 6 | enum 7 | { 8 | GPS_VEL_X = 0, 9 | GPS_VEL_Y, 10 | GPS_VEL_Z, 11 | BARO_VEL, 12 | TOF_VEL 13 | }; 14 | 15 | typedef struct { 16 | Vector3f_t accel; 17 | Vector3f_t accel_bias; 18 | 19 | Vector3f_t velocity; 20 | float velMeasure[6]; 21 | 22 | Vector3f_t position; 23 | Vector3f_t posMeasure; 24 | } NAVGATION_t; 25 | 26 | void NavigationInit(void); 27 | void VelocityEstimate(void); 28 | void PositionEstimate(void); 29 | 30 | void AltCovarianceSelfAdaptation(void); 31 | void PosCovarianceSelfAdaptation(void); 32 | 33 | Vector3f_t GetCopterAccel(void); 34 | Vector3f_t GetAccelBias(void); 35 | Vector3f_t GetCopterVelocity(void); 36 | float* GetCopterVelMeasure(void); 37 | Vector3f_t GetCopterPosition(void); 38 | Vector3f_t GetCopterPosMeasure(void); 39 | 40 | void NavigationReset(void); 41 | 42 | #endif 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /SRC/SENSOR/accelerometer.h: -------------------------------------------------------------------------------- 1 | #ifndef __ACCELEROMETER_H 2 | #define __ACCELEROMETER_H 3 | 4 | #include "sensor.h" 5 | 6 | typedef struct { 7 | Vector3f_t data; 8 | Vector3f_t dataLpf; 9 | float mag; 10 | float vibraCoef; 11 | LPF2ndData_t lpf_2nd; 12 | SENSOR_CALI_t cali; 13 | SENSOR_CALI_t levelCali; 14 | } ACCELEROMETER_t; 15 | 16 | void AccPreTreatInit(void); 17 | void AccDataPreTreat(Vector3f_t accRaw, Vector3f_t* accData); 18 | void AccCalibration(Vector3f_t accRaw); 19 | void AccScaleCalibrate(Vector3f_t* acc); 20 | void ImuLevelCalibration(void); 21 | 22 | Vector3f_t GetAccOffsetCaliData(void); 23 | Vector3f_t GetAccScaleCaliData(void); 24 | Vector3f_t GetLevelCalibraData(void); 25 | 26 | void AccCalibrateEnable(void); 27 | void LevelCalibrateEnable(void); 28 | 29 | float GetAccMag(void); 30 | Vector3f_t AccGetData(void); 31 | Vector3f_t AccLpfGetData(void); 32 | 33 | #endif 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /SRC/SENSOR/barometer.h: -------------------------------------------------------------------------------- 1 | #ifndef __BAROMETER_H 2 | #define __BAROMETER_H 3 | 4 | #include "sensor.h" 5 | 6 | void BaroDataPreTreat(void); 7 | int32_t BaroGetAlt(void); 8 | float BaroGetTemp(void); 9 | float BaroGetVelocity(void); 10 | 11 | #endif 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /SRC/SENSOR/gps.h: -------------------------------------------------------------------------------- 1 | #ifndef __GPS_H 2 | #define __GPS_H 3 | 4 | #include "sensor.h" 5 | 6 | void GpsDataPreTreat(void); 7 | void TransVelToBodyFrame(Vector3f_t velEf, Vector3f_t* velBf, float yaw); 8 | void TransVelToEarthFrame(Vector3f_t velBf, Vector3f_t* velEf, float yaw); 9 | float GetMagDeclination(void); 10 | bool GpsGetFixStatus(void); 11 | float GpsGetAccuracy(void); 12 | Vector3f_t GpsGetVelocity(void); 13 | Vector3f_t GpsGetPosition(void); 14 | 15 | void GpsResetHomePosition(void); 16 | void GpsTransToLocalPosition(Vector3f_t* position, double lat, double lon); 17 | 18 | float GetDirectionToHome(Vector3f_t position); 19 | float GetDistanceToHome(Vector3f_t position); 20 | float GetDirectionOfTwoPoint(Vector3f_t point1, Vector3f_t point2); 21 | Vector3f_t GetHomePosition(void); 22 | void GetHomeLatitudeAndLongitude(double* lat, double* lon); 23 | 24 | #endif 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /SRC/SENSOR/gyroscope.h: -------------------------------------------------------------------------------- 1 | #ifndef __GYROSCOPE_H 2 | #define __GYROSCOPE_H 3 | 4 | #include "sensor.h" 5 | 6 | typedef struct { 7 | Vector3f_t data; 8 | Vector3f_t dataLpf; 9 | float temperature; 10 | LPF2ndData_t lpf_2nd; 11 | SENSOR_CALI_t cali; 12 | } GYROSCOPE_t; 13 | 14 | void GyroPreTreatInit(void); 15 | void GyroDataPreTreat(Vector3f_t gyroRaw, float temperature, Vector3f_t* gyroData, Vector3f_t* gyroLpfData); 16 | void GyroCalibration(Vector3f_t gyroRaw); 17 | 18 | uint8_t GetGyroCaliStatus(void); 19 | void GyroCalibrateEnable(void); 20 | 21 | Vector3f_t GyroGetData(void); 22 | Vector3f_t GyroLpfGetData(void); 23 | float GyroGetTemp(void); 24 | Vector3f_t GetGyroOffsetCaliData(void); 25 | 26 | #endif 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /SRC/SENSOR/magnetometer.h: -------------------------------------------------------------------------------- 1 | #ifndef __MAGNETOMETER_H 2 | #define __MAGNETOMETER_H 3 | 4 | #include "sensor.h" 5 | 6 | typedef struct { 7 | Vector3f_t data; 8 | float mag; 9 | SENSOR_CALI_t cali; 10 | float earthMag; 11 | 12 | } MAGNETOMETER_t; 13 | 14 | void MagCaliDataInit(void); 15 | void MagDataPreTreat(void); 16 | void MagCalibration(void); 17 | Vector3f_t MagGetData(void); 18 | void MagCalibrateEnable(void); 19 | 20 | Vector3f_t GetMagOffsetCaliData(void); 21 | Vector3f_t GetMagScaleCaliData(void); 22 | 23 | #endif 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /SRC/SENSOR/sensor.h: -------------------------------------------------------------------------------- 1 | #ifndef __SENSOR_H 2 | #define __SENSOR_H 3 | 4 | #include "mathTool.h" 5 | #include "lowPassFilter.h" 6 | 7 | //默认设置传感器自带低通滤波频率为42Hz 8 | 9 | //陀螺仪低通滤波截止频率 10 | //二次低通滤波的陀螺仪数据用于角速度环控制 11 | //频率过低,信号延迟大,会导致控制发散 12 | //频率过高,信号噪声大,会增加高频抖动 13 | //要达到最佳效果,需经调试选取最佳的截止频率 14 | //飞行器越小,系统惯性越小,所需的控制频率越高,对信号实时性的要求也就越高 15 | //450轴距左右的飞机,截止频率不低于50就好,要根据实际情况选取,比如有时过软的硬件减震措施也会增加实际信号延迟 16 | #define GYRO_LPF_CUT 88 17 | 18 | //加速度低通滤波截止频率 19 | //二次低通滤波的加速度数据主要用于计算模值 20 | #define ACC_LPF_CUT 30 21 | 22 | //传感器恒温目标值,单位为摄氏度 23 | #define SENSOR_TEMP_KEPT 50 24 | 25 | //传感器类型 26 | enum 27 | { 28 | GYRO, 29 | ACC, 30 | MAG, 31 | ANGLE, 32 | BARO, 33 | ESC 34 | }; 35 | 36 | //飞控放置方向 37 | enum ORIENTATION_STATUS 38 | { 39 | ORIENTATION_UP, 40 | ORIENTATION_DOWN, 41 | ORIENTATION_LEFT, 42 | ORIENTATION_RIGHT, 43 | ORIENTATION_FRONT, 44 | ORIENTATION_BACK, 45 | }; 46 | 47 | //传感器健康状态 48 | enum SENSOR_HEALTH 49 | { 50 | SENSOR_NORMAL, 51 | SENSOR_UNHEALTH 52 | }; 53 | 54 | typedef struct 55 | { 56 | Vector3f_t offset; //零偏误差 57 | Vector3f_t scale; //比例误差 58 | bool should_cali; //传感器校准标志位 59 | bool success; //校准成功标志位 60 | uint8_t step; //步骤标志位 61 | } SENSOR_CALI_t; 62 | 63 | typedef struct 64 | { 65 | float gyro_offset; 66 | float acc_offset; 67 | float mag_offset; 68 | enum SENSOR_HEALTH gyro; 69 | enum SENSOR_HEALTH acc; 70 | enum SENSOR_HEALTH mag; 71 | } SENSOR_HEALTH_t; 72 | 73 | void ImuTempControlInit(void); 74 | bool SensorCheckStatus(void); 75 | void SensorHealthCheck(void); 76 | void ImuTempControl(float tempMeasure); 77 | void ImuOrientationDetect(void); 78 | 79 | enum ORIENTATION_STATUS GetImuOrientation(void); 80 | enum SENSOR_HEALTH GetGyroHealthStatus(void); 81 | enum SENSOR_HEALTH GetAccHealthStatus(void); 82 | 83 | #endif 84 | 85 | 86 | -------------------------------------------------------------------------------- /SRC/SYSTEM/faultDetect.h: -------------------------------------------------------------------------------- 1 | #ifndef __FAULTDETECT_H__ 2 | #define __FAULTDETECT_H__ 3 | 4 | #include "mathTool.h" 5 | 6 | //错误类型 7 | enum ERROR_TYPE 8 | { 9 | ERROR_RESERVE1, 10 | GYRO_UNDETECTED, //未检测到陀螺仪传感器 11 | ACC_UNDETECTED, //未检测到加速度传感器 12 | MAG_UNDETECTED, //未检测到地磁传感器 13 | BARO_UNDETECTED, //未检测到气压传感器 14 | GPS_UNDETECTED, //未检测到GPS 15 | OPTICALFLOW_UNDETECTED, //未检测到光流 16 | ULTRASONIC_UNDETECTED, //未检测到超声波传感器 17 | TOF_UNDETECTED, //未检测到TOF传感器 18 | ERROR_RESERVE2, 19 | ERROR_RESERVE3, 20 | ERROR_RESERVE4, 21 | ERROR_RESERVE5, 22 | GYRO_UNCALIBRATED, //陀螺仪传感器未校准 23 | ACC_UNCALIBRATED, //加速度传感器未校准 24 | MAG_UNCALIBRATED, //地磁传感器未校准 25 | TOF_UNCALIBRATED, //TOF传感器未校准 26 | ERROR_RESERVE6, 27 | ERROR_RESERVE7, 28 | ERROR_RESERVE8, 29 | ERROR_RESERVE9, 30 | SENSOR_HEAT_ERROR, //IMU传感器恒温不工作 31 | BMS_UNDETECTED, //未检测到电池管理系统 32 | CRITICAL_LOW_BATTERY, //严重低电量/电压 33 | ERROR_NUM 34 | }; 35 | 36 | //警告类型 37 | enum WARNNING_TYPE 38 | { 39 | WARNNING_RESERVE1, 40 | SYSTEM_INITIALIZING, //系统正在初始化 41 | WARNNING_RESERVE2, 42 | GYRO_NEED_CALIBRATED, //建议校准陀螺仪传感器 43 | ACC_NEED_CALIBRATED, //建议校准加速度传感器 44 | MAG_NEED_CALIBRATED, //建议校准地磁传感器 45 | WARNNING_RESERVE3, 46 | WARNNING_RESERVE4, 47 | WARNNING_RESERVE5, 48 | WARNNING_RESERVE6, 49 | MAG_DISTURBING, //地磁传感器受干扰 50 | WARNNING_RESERVE7, 51 | LOW_BATTERY, //低电量/电压 52 | LOW_BATTERY_FOR_RETURN, //电量/电压低于返航所需 53 | WARNNING_NUM 54 | }; 55 | 56 | 57 | void FaultDetectInit(void); 58 | void FaultDetectSetError(uint16_t error_type); 59 | void FaultDetectResetError(uint16_t error_type); 60 | void FaultDetectSetWarnning(uint16_t warnning_type); 61 | void FaultDetectResetWarnning(uint16_t warnning_type); 62 | uint8_t FaultDetectGetErrorStatus(uint16_t error_type); 63 | uint8_t FaultDetectGetWarnningStatus(uint16_t warnning_type); 64 | uint8_t* FaultDetectGetError(void); 65 | uint8_t* FaultDetectGetWarnning(void); 66 | 67 | #endif 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /SRC/SYSTEM/flightStatus.h: -------------------------------------------------------------------------------- 1 | #ifndef __FLIGHTSTATUS_H__ 2 | #define __FLIGHTSTATUS_H__ 3 | 4 | #include "mathTool.h" 5 | 6 | //飞行状态 7 | enum 8 | { 9 | STANDBY, //待机 10 | TAKE_OFF, //起飞 11 | IN_AIR, //在空中 12 | LANDING, //降落 13 | FINISH_LANDING //降落完成 14 | }; 15 | 16 | //初始化状态 17 | enum 18 | { 19 | HEATING, //加热中 20 | HEAT_FINISH, //加热完成 21 | INIT_FINISH //初始化完成 (完成加速度零偏计算) 22 | }; 23 | 24 | //放置状态 25 | enum 26 | { 27 | STATIC, //静止 28 | MOTIONAL //运动 29 | }; 30 | 31 | //电机锁定状态 32 | enum 33 | { 34 | DISARMED, //上锁 35 | ARMED //解锁 36 | }; 37 | 38 | //水平方向控制状态 39 | enum 40 | { 41 | POS_HOLD, //悬停 42 | POS_CHANGED, //飞行 43 | POS_BRAKE, //刹车 44 | POS_BRAKE_FINISH //刹车完成 45 | }; 46 | 47 | //垂直方向控制状态 48 | enum 49 | { 50 | ALT_HOLD, //悬停 51 | ALT_CHANGED, //高度改变 52 | ALT_CHANGED_FINISH //高度改变完成 53 | }; 54 | 55 | //飞行模式 56 | enum 57 | { 58 | MANUAL = 0, //手动 (不带定高不带定点) 59 | SEMIAUTO, //半自动 (带定高不带定点) 60 | AUTO, //自动 (带定高带定点) 61 | SPORT, //运动模式 62 | COMMAND, //命令模式 (用于第三方控制) 63 | AUTOTAKEOFF, //自动起飞 64 | AUTOLAND, //自动降落 65 | RETURNTOHOME, //自动返航 66 | AUTOCIRCLE, //自动绕圈 67 | AUTOPILOT, //自动航线 68 | FOLLOWME //自动跟随 69 | }; 70 | 71 | void SystemInitCheck(void); 72 | 73 | void SetAltControlStatus(uint8_t status); 74 | uint8_t GetAltControlStatus(void); 75 | void SetPosControlStatus(uint8_t status); 76 | uint8_t GetPosControlStatus(void); 77 | 78 | bool SetArmedStatus(uint8_t status); 79 | uint8_t GetArmedStatus(void); 80 | 81 | void PlaceStausCheck(Vector3f_t gyro); 82 | uint8_t GetPlaceStatus(void); 83 | 84 | void SetInitStatus(uint8_t status); 85 | uint8_t GetInitStatus(void); 86 | uint32_t GetInitFinishTime(void); 87 | 88 | void SetFlightStatus(uint8_t status); 89 | uint8_t GetFlightStatus(void); 90 | 91 | void SetFlightMode(uint8_t mode); 92 | uint8_t GetFlightMode(void); 93 | 94 | void WindEstimate(void); 95 | float GetWindSpeed(void); 96 | float GetWindSpeedAcc(void); 97 | 98 | void SetFailSafeStatus(bool status); 99 | bool GetFailSafeStatus(void); 100 | 101 | #endif 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /SRC/SYSTEM/parameter.h: -------------------------------------------------------------------------------- 1 | #ifndef __PARAMETER_H__ 2 | #define __PARAMETER_H__ 3 | 4 | #include "board.h" 5 | 6 | enum PARAM_TYPE 7 | { 8 | PARAM_CHECK_NUM, 9 | PARAM_CHECK_SUM, 10 | /*******陀螺仪校准参数*********/ 11 | PARAM_GYRO_OFFSET_X, 12 | PARAM_GYRO_OFFSET_Y, 13 | PARAM_GYRO_OFFSET_Z, 14 | PARAM_GYRO_SCALE_X, 15 | PARAM_GYRO_SCALE_Y, 16 | PARAM_GYRO_SCALE_Z, 17 | /*******加速度校准参数*********/ 18 | PARAM_ACC_OFFSET_X, 19 | PARAM_ACC_OFFSET_Y, 20 | PARAM_ACC_OFFSET_Z, 21 | PARAM_ACC_SCALE_X, 22 | PARAM_ACC_SCALE_Y, 23 | PARAM_ACC_SCALE_Z, 24 | /*******磁力计校准参数*********/ 25 | PARAM_MAG_OFFSET_X, 26 | PARAM_MAG_OFFSET_Y, 27 | PARAM_MAG_OFFSET_Z, 28 | PARAM_MAG_SCALE_X, 29 | PARAM_MAG_SCALE_Y, 30 | PARAM_MAG_SCALE_Z, 31 | PARAM_MAG_EARTH_MAG, 32 | /*********水平校准参数*********/ 33 | PARAM_IMU_LEVEL_X, 34 | PARAM_IMU_LEVEL_Y, 35 | PARAM_IMU_LEVEL_Z, 36 | /*********姿态PID参数*********/ 37 | PARAM_PID_ATT_INNER_X_KP, 38 | PARAM_PID_ATT_INNER_X_KI, 39 | PARAM_PID_ATT_INNER_X_KD, 40 | PARAM_PID_ATT_INNER_Y_KP, 41 | PARAM_PID_ATT_INNER_Y_KI, 42 | PARAM_PID_ATT_INNER_Y_KD, 43 | PARAM_PID_ATT_INNER_Z_KP, 44 | PARAM_PID_ATT_INNER_Z_KI, 45 | PARAM_PID_ATT_INNER_Z_KD, 46 | PARAM_PID_ATT_OUTER_X_KP, 47 | PARAM_PID_ATT_OUTER_Y_KP, 48 | PARAM_PID_ATT_OUTER_Z_KP, 49 | /*********位置PID参数*********/ 50 | PARAM_PID_POS_INNER_X_KP, 51 | PARAM_PID_POS_INNER_X_KI, 52 | PARAM_PID_POS_INNER_X_KD, 53 | PARAM_PID_POS_INNER_Y_KP, 54 | PARAM_PID_POS_INNER_Y_KI, 55 | PARAM_PID_POS_INNER_Y_KD, 56 | PARAM_PID_POS_INNER_Z_KP, 57 | PARAM_PID_POS_INNER_Z_KI, 58 | PARAM_PID_POS_INNER_Z_KD, 59 | PARAM_PID_POS_OUTER_X_KP, 60 | PARAM_PID_POS_OUTER_Y_KP, 61 | PARAM_PID_POS_OUTER_Z_KP, 62 | /*********电调校准标志*********/ 63 | PARAM_ESC_CALI_FLAG, 64 | /******************************/ 65 | PARAM_NUM 66 | }; 67 | 68 | void ParamInit(void); 69 | void ParamSaveToFlash(void); 70 | void ParamUpdateData(uint16_t dataNum, const void * data); 71 | void ParamGetData(uint16_t dataNum, void *data, uint8_t length); 72 | const char* ParamGetString(uint8_t paramNum); 73 | 74 | #endif 75 | 76 | -------------------------------------------------------------------------------- /SRC/TASK/TaskConfig.h: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 TaskConfig.h 6 | * @说明 RTOS的任务配置文件,任务调度模式为抢占式,共有15个优先级(最多可设置为32) 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.05 11 | **********************************************************************************************************/ 12 | #ifndef __TASKCONFIG_H__ 13 | #define __TASKCONFIG_H__ 14 | 15 | #include "FreeRTOS.h" 16 | #include "task.h" 17 | #include "queue.h" 18 | 19 | #include "board.h" 20 | #include "mathTool.h" 21 | 22 | #include "module_task.h" 23 | #include "sensor_task.h" 24 | #include "navigation_task.h" 25 | #include "control_task.h" 26 | #include "message_task.h" 27 | #include "log_task.h" 28 | 29 | //任务堆栈大小 30 | #define IMU_SENSOR_READ_TASK_STACK 256 31 | #define SENSOR_UPDATE_TASK_STACK 256 32 | #define IMU_DATA_PRETREAT_TASK_STACK 256 33 | #define OTHER_SENSOR_TASK_STACK 256 34 | #define NAVIGATION_TASK_STACK 512 35 | #define FLIGHT_STATUS_TASK_STACK 256 36 | #define FLIGHTCONTROL_TASK_STACK 256 37 | #define MESSAGE_TASK_STACK 512 38 | #define LOG_TASK_STACK 1024 39 | 40 | //任务优先级 41 | #define IMU_SENSOR_READ_TASK_PRIORITY 13 42 | #define IMU_DATA_PRETREAT_TASK_PRIORITY 12 43 | #define FLIGHTCONTROL_TASK_PRIORITY 10 44 | #define NAVIGATION_TASK_PRIORITY 11 45 | #define SENSOR_UPDATE_TASK_PRIORITY 8 46 | #define OTHER_SENSOR_TASK_PRIORITY 7 47 | #define MESSAGE_TASK_PRIORITY 6 48 | #define FLIGHT_STATUS_TASK_PRIORITY 5 49 | #define LOG_TASK_PRIORITY 3 50 | 51 | enum { 52 | GYRO_SENSOR_READ, 53 | ACC_SENSOR_READ, 54 | TEMP_SENSOR_READ, 55 | GYRO_DATA_PRETREAT, 56 | ACC_DATA_PRETREAT, 57 | GYRO_FOR_CONTROL, 58 | QUEUE_NUM 59 | }; 60 | 61 | extern QueueHandle_t messageQueue[QUEUE_NUM]; 62 | 63 | #endif 64 | -------------------------------------------------------------------------------- /SRC/TASK/control_task.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 control_task.c 6 | * @说明 飞行控制相关任务 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.05 11 | **********************************************************************************************************/ 12 | #include "TaskConfig.h" 13 | 14 | #include "flightControl.h" 15 | #include "userControl.h" 16 | #include "missionControl.h" 17 | #include "safeControl.h" 18 | #include "rc.h" 19 | 20 | xTaskHandle flightControlTask; 21 | 22 | /********************************************************************************************************** 23 | *函 数 名: vFlightControlTask 24 | *功能说明: 飞行控制相关任务 25 | *形 参: 无 26 | *返 回 值: 无 27 | **********************************************************************************************************/ 28 | portTASK_FUNCTION(vFlightControlTask, pvParameters) 29 | { 30 | Vector3f_t* gyro; 31 | static uint32_t count = 0; 32 | 33 | //遥控相关功能初始化 34 | RcInit(); 35 | 36 | //控制参数初始化 37 | FlightControlInit(); 38 | 39 | for(;;) 40 | { 41 | //从消息队列中获取数据 42 | xQueueReceive(messageQueue[GYRO_FOR_CONTROL], &gyro, (3 / portTICK_RATE_MS)); 43 | 44 | //100Hz 45 | if(count % 10 == 0) 46 | { 47 | //遥控器各通道数据及失控检测 48 | RcCheck(); 49 | 50 | //安全保护控制 51 | SafeControl(); 52 | } 53 | 54 | //200Hz 55 | if(count % 5 == 0) 56 | { 57 | //位置外环控制 58 | PositionOuterControl(); 59 | } 60 | 61 | //500Hz 62 | if(count % 2 == 0) 63 | { 64 | //用户控制模式下的操控逻辑处理 65 | UserControl(); 66 | 67 | //自主控制任务实现(自动起飞、自动降落、自动返航、自动航线等) 68 | MissionControl(); 69 | 70 | //位置内环控制 71 | PositionInnerControl(); 72 | 73 | //姿态外环控制 74 | AttitudeOuterControl(); 75 | 76 | //高度外环控制 77 | AltitudeOuterControl(); 78 | } 79 | 80 | //飞行内环控制,包括姿态内环和高度内环 81 | FlightControlInnerLoop(*gyro); 82 | 83 | count++; 84 | } 85 | } 86 | 87 | /********************************************************************************************************** 88 | *函 数 名: ControlTaskCreate 89 | *功能说明: 控制相关任务创建 90 | *形 参: 无 91 | *返 回 值: 无 92 | **********************************************************************************************************/ 93 | void ControlTaskCreate(void) 94 | { 95 | xTaskCreate(vFlightControlTask, "flightControl", FLIGHTCONTROL_TASK_STACK, NULL, FLIGHTCONTROL_TASK_PRIORITY, &flightControlTask); 96 | } 97 | 98 | /********************************************************************************************************** 99 | *函 数 名: GetFlightControlTaskStackRemain 100 | *功能说明: 获取任务堆栈使用剩余 101 | *形 参: 无 102 | *返 回 值: 无 103 | **********************************************************************************************************/ 104 | int16_t GetFlightControlTaskStackRemain(void) 105 | { 106 | return uxTaskGetStackHighWaterMark(flightControlTask); 107 | } 108 | 109 | 110 | 111 | 112 | -------------------------------------------------------------------------------- /SRC/TASK/control_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __CONTROL_TASK_H__ 2 | #define __CONTROL_TASK_H__ 3 | 4 | void ControlTaskCreate(void); 5 | 6 | int16_t GetFlightControlTaskStackRemain(void); 7 | 8 | #endif 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /SRC/TASK/log_task.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 log_task.c 6 | * @说明 飞行日志相关任务 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.08 11 | **********************************************************************************************************/ 12 | #include "TaskConfig.h" 13 | 14 | #include "logger.h" 15 | 16 | xTaskHandle logTask; 17 | 18 | /********************************************************************************************************** 19 | *函 数 名: vLogTask 20 | *功能说明: 飞行日志任务 21 | *形 参: 无 22 | *返 回 值: 无 23 | **********************************************************************************************************/ 24 | portTASK_FUNCTION(vLogTask, pvParameters) 25 | { 26 | portTickType xLastWakeTime; 27 | 28 | //飞行日志初始化 29 | LoggerInit(); 30 | 31 | xLastWakeTime = xTaskGetTickCount(); 32 | for(;;) 33 | { 34 | //记录飞行日志 35 | LoggerLoop(); 36 | 37 | vTaskDelayUntil(&xLastWakeTime, ((1000 / LOG_RATE) / portTICK_RATE_MS)); 38 | } 39 | } 40 | 41 | /********************************************************************************************************** 42 | *函 数 名: LogTaskCreate 43 | *功能说明: 飞行日志任务创建 44 | *形 参: 无 45 | *返 回 值: 无 46 | **********************************************************************************************************/ 47 | void LogTaskCreate(void) 48 | { 49 | xTaskCreate(vLogTask, "log", LOG_TASK_STACK, NULL, LOG_TASK_PRIORITY, &logTask); 50 | } 51 | 52 | /********************************************************************************************************** 53 | *函 数 名: GetLogTaskStackRemain 54 | *功能说明: 获取任务堆栈使用剩余 55 | *形 参: 无 56 | *返 回 值: 无 57 | **********************************************************************************************************/ 58 | int16_t GetLogTaskStackRemain(void) 59 | { 60 | return uxTaskGetStackHighWaterMark(logTask); 61 | } 62 | 63 | 64 | -------------------------------------------------------------------------------- /SRC/TASK/log_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __LOG_TASK_H__ 2 | #define __LOG_TASK_H__ 3 | 4 | void LogTaskCreate(void); 5 | 6 | int16_t GetLogTaskStackRemain(void); 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /SRC/TASK/messageQueue.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 messageQueue.c 6 | * @说明 消息队列,主要用于对数据传递实时性要求较高的任务间通信 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.05 11 | **********************************************************************************************************/ 12 | #include "messageQueue.h" 13 | 14 | //声明消息队列句柄 15 | QueueHandle_t messageQueue[QUEUE_NUM]; 16 | 17 | /********************************************************************************************************** 18 | *函 数 名: MessageQueueCreate 19 | *功能说明: 消息队列创建 20 | *形 参: 无 21 | *返 回 值: 无 22 | **********************************************************************************************************/ 23 | void MessageQueueCreate(void) 24 | { 25 | messageQueue[ACC_SENSOR_READ] = xQueueCreate(2, sizeof(Vector3f_t *)); 26 | messageQueue[GYRO_SENSOR_READ] = xQueueCreate(2, sizeof(Vector3f_t *)); 27 | messageQueue[TEMP_SENSOR_READ] = xQueueCreate(2, sizeof(float *)); 28 | 29 | messageQueue[GYRO_DATA_PRETREAT] = xQueueCreate(2, sizeof(Vector3f_t *)); 30 | messageQueue[ACC_DATA_PRETREAT] = xQueueCreate(2, sizeof(Vector3f_t *)); 31 | messageQueue[GYRO_FOR_CONTROL] = xQueueCreate(2, sizeof(Vector3f_t *)); 32 | } 33 | 34 | 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /SRC/TASK/messageQueue.h: -------------------------------------------------------------------------------- 1 | #ifndef __MESSAGE_QUEUE_H__ 2 | #define __MESSAGE_QUEUE_H__ 3 | 4 | #include "TaskConfig.h" 5 | 6 | void MessageQueueCreate(void); 7 | 8 | #endif 9 | 10 | -------------------------------------------------------------------------------- /SRC/TASK/message_task.c: -------------------------------------------------------------------------------- 1 | /********************************************************************************************************** 2 | 天穹飞控 —— 致力于打造中国最好的多旋翼开源飞控 3 | Github: github.com/loveuav/BlueSkyFlightControl 4 | 技术讨论:bbs.loveuav.com/forum-68-1.html 5 | * @文件 message_task.c 6 | * @说明 飞控数据通信相关任务 7 | * @版本 V1.0 8 | * @作者 BlueSky 9 | * @网站 bbs.loveuav.com 10 | * @日期 2018.06 11 | **********************************************************************************************************/ 12 | #include "TaskConfig.h" 13 | 14 | #include "message.h" 15 | 16 | xTaskHandle messageTask; 17 | 18 | /********************************************************************************************************** 19 | *函 数 名: vMessageTask 20 | *功能说明: 数据通信任务 21 | *形 参: 无 22 | *返 回 值: 无 23 | **********************************************************************************************************/ 24 | portTASK_FUNCTION(vMessageTask, pvParameters) 25 | { 26 | portTickType xLastWakeTime; 27 | 28 | //飞控数据通信初始化 29 | MessageInit(); 30 | 31 | xLastWakeTime = xTaskGetTickCount(); 32 | for(;;) 33 | { 34 | //发送飞控数据 35 | MessageSendLoop(); 36 | 37 | vTaskDelayUntil(&xLastWakeTime, ((1000 / MAX_SEND_FREQ) / portTICK_RATE_MS)); 38 | } 39 | } 40 | 41 | /********************************************************************************************************** 42 | *函 数 名: MessageTaskCreate 43 | *功能说明: 数据通信任务创建 44 | *形 参: 无 45 | *返 回 值: 无 46 | **********************************************************************************************************/ 47 | void MessageTaskCreate(void) 48 | { 49 | xTaskCreate(vMessageTask, "message", MESSAGE_TASK_STACK, NULL, MESSAGE_TASK_PRIORITY, &messageTask); 50 | } 51 | 52 | /********************************************************************************************************** 53 | *函 数 名: GetMessageTaskStackRemain 54 | *功能说明: 获取任务堆栈使用剩余 55 | *形 参: 无 56 | *返 回 值: 无 57 | **********************************************************************************************************/ 58 | int16_t GetMessageTaskStackRemain(void) 59 | { 60 | return uxTaskGetStackHighWaterMark(messageTask); 61 | } 62 | 63 | 64 | -------------------------------------------------------------------------------- /SRC/TASK/message_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __MESSAGE_TASK_H__ 2 | #define __MESSAGE_TASK_H__ 3 | 4 | void MessageTaskCreate(void); 5 | 6 | int16_t GetMessageTaskStackRemain(void); 7 | 8 | #endif 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /SRC/TASK/module_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __MODULE_TASK_H__ 2 | #define __MODULE_TASK_H__ 3 | 4 | void ModuleTaskCreate(void); 5 | int16_t GetImuSensorReadTaskStackRemain(void); 6 | int16_t GetSensorUpdateTaskStackRemain(void); 7 | 8 | #endif 9 | 10 | 11 | -------------------------------------------------------------------------------- /SRC/TASK/navigation_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __NAVIGATION_TASK_H__ 2 | #define __NAVIGATION_TASK_H__ 3 | 4 | 5 | void NavigationTaskCreate(void); 6 | 7 | int16_t GetNavigationTaskStackRemain(void); 8 | int16_t GetFlightStatusTaskStackRemain(void); 9 | 10 | #endif 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /SRC/TASK/sensor_task.h: -------------------------------------------------------------------------------- 1 | #ifndef __SENSOR_TASK_H__ 2 | #define __SENSOR_TASK_H__ 3 | 4 | void SensorTaskCreate(void); 5 | 6 | int16_t GetImuDataPreTreatTaskStackRemain(void); 7 | int16_t GetOtherSensorTaskStackRemain(void); 8 | 9 | #endif 10 | 11 | 12 | -------------------------------------------------------------------------------- /STMLIB/Include/core_cmFunc.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************//** 2 | * @file core_cmFunc.h 3 | * @brief CMSIS Cortex-M Core Function Access Header File 4 | * @version V4.30 5 | * @date 20. October 2015 6 | ******************************************************************************/ 7 | /* Copyright (c) 2009 - 2015 ARM LIMITED 8 | 9 | All rights reserved. 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | - Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | - Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | - Neither the name of ARM nor the names of its contributors may be used 18 | to endorse or promote products derived from this software without 19 | specific prior written permission. 20 | * 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | ---------------------------------------------------------------------------*/ 33 | 34 | 35 | #if defined ( __ICCARM__ ) 36 | #pragma system_include /* treat file as system include file for MISRA check */ 37 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 38 | #pragma clang system_header /* treat file as system include file */ 39 | #endif 40 | 41 | #ifndef __CORE_CMFUNC_H 42 | #define __CORE_CMFUNC_H 43 | 44 | 45 | /* ########################### Core Function Access ########################### */ 46 | /** \ingroup CMSIS_Core_FunctionInterface 47 | \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions 48 | @{ 49 | */ 50 | 51 | /*------------------ RealView Compiler -----------------*/ 52 | #if defined ( __CC_ARM ) 53 | #include "cmsis_armcc.h" 54 | 55 | /*------------------ ARM Compiler V6 -------------------*/ 56 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 57 | #include "cmsis_armcc_V6.h" 58 | 59 | /*------------------ GNU Compiler ----------------------*/ 60 | #elif defined ( __GNUC__ ) 61 | #include "cmsis_gcc.h" 62 | 63 | /*------------------ ICC Compiler ----------------------*/ 64 | #elif defined ( __ICCARM__ ) 65 | #include 66 | 67 | /*------------------ TI CCS Compiler -------------------*/ 68 | #elif defined ( __TMS470__ ) 69 | #include 70 | 71 | /*------------------ TASKING Compiler ------------------*/ 72 | #elif defined ( __TASKING__ ) 73 | /* 74 | * The CMSIS functions have been implemented as intrinsics in the compiler. 75 | * Please use "carm -?i" to get an up to date list of all intrinsics, 76 | * Including the CMSIS ones. 77 | */ 78 | 79 | /*------------------ COSMIC Compiler -------------------*/ 80 | #elif defined ( __CSMC__ ) 81 | #include 82 | 83 | #endif 84 | 85 | /*@} end of CMSIS_Core_RegAccFunctions */ 86 | 87 | #endif /* __CORE_CMFUNC_H */ 88 | -------------------------------------------------------------------------------- /STMLIB/Include/core_cmInstr.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************//** 2 | * @file core_cmInstr.h 3 | * @brief CMSIS Cortex-M Core Instruction Access Header File 4 | * @version V4.30 5 | * @date 20. October 2015 6 | ******************************************************************************/ 7 | /* Copyright (c) 2009 - 2015 ARM LIMITED 8 | 9 | All rights reserved. 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | - Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | - Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | - Neither the name of ARM nor the names of its contributors may be used 18 | to endorse or promote products derived from this software without 19 | specific prior written permission. 20 | * 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | ---------------------------------------------------------------------------*/ 33 | 34 | 35 | #if defined ( __ICCARM__ ) 36 | #pragma system_include /* treat file as system include file for MISRA check */ 37 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 38 | #pragma clang system_header /* treat file as system include file */ 39 | #endif 40 | 41 | #ifndef __CORE_CMINSTR_H 42 | #define __CORE_CMINSTR_H 43 | 44 | 45 | /* ########################## Core Instruction Access ######################### */ 46 | /** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface 47 | Access to dedicated instructions 48 | @{ 49 | */ 50 | 51 | /*------------------ RealView Compiler -----------------*/ 52 | #if defined ( __CC_ARM ) 53 | #include "cmsis_armcc.h" 54 | 55 | /*------------------ ARM Compiler V6 -------------------*/ 56 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 57 | #include "cmsis_armcc_V6.h" 58 | 59 | /*------------------ GNU Compiler ----------------------*/ 60 | #elif defined ( __GNUC__ ) 61 | #include "cmsis_gcc.h" 62 | 63 | /*------------------ ICC Compiler ----------------------*/ 64 | #elif defined ( __ICCARM__ ) 65 | #include 66 | 67 | /*------------------ TI CCS Compiler -------------------*/ 68 | #elif defined ( __TMS470__ ) 69 | #include 70 | 71 | /*------------------ TASKING Compiler ------------------*/ 72 | #elif defined ( __TASKING__ ) 73 | /* 74 | * The CMSIS functions have been implemented as intrinsics in the compiler. 75 | * Please use "carm -?i" to get an up to date list of all intrinsics, 76 | * Including the CMSIS ones. 77 | */ 78 | 79 | /*------------------ COSMIC Compiler -------------------*/ 80 | #elif defined ( __CSMC__ ) 81 | #include 82 | 83 | #endif 84 | 85 | /*@}*/ /* end of group CMSIS_Core_InstructionInterface */ 86 | 87 | #endif /* __CORE_CMINSTR_H */ 88 | -------------------------------------------------------------------------------- /STMLIB/Include/core_cmSimd.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************//** 2 | * @file core_cmSimd.h 3 | * @brief CMSIS Cortex-M SIMD Header File 4 | * @version V4.30 5 | * @date 20. October 2015 6 | ******************************************************************************/ 7 | /* Copyright (c) 2009 - 2015 ARM LIMITED 8 | 9 | All rights reserved. 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions are met: 12 | - Redistributions of source code must retain the above copyright 13 | notice, this list of conditions and the following disclaimer. 14 | - Redistributions in binary form must reproduce the above copyright 15 | notice, this list of conditions and the following disclaimer in the 16 | documentation and/or other materials provided with the distribution. 17 | - Neither the name of ARM nor the names of its contributors may be used 18 | to endorse or promote products derived from this software without 19 | specific prior written permission. 20 | * 21 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 22 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 23 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 24 | ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS AND CONTRIBUTORS BE 25 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 26 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 27 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 28 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 29 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 30 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | ---------------------------------------------------------------------------*/ 33 | 34 | 35 | #if defined ( __ICCARM__ ) 36 | #pragma system_include /* treat file as system include file for MISRA check */ 37 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 38 | #pragma clang system_header /* treat file as system include file */ 39 | #endif 40 | 41 | #ifndef __CORE_CMSIMD_H 42 | #define __CORE_CMSIMD_H 43 | 44 | #ifdef __cplusplus 45 | extern "C" { 46 | #endif 47 | 48 | 49 | /* ################### Compiler specific Intrinsics ########################### */ 50 | /** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics 51 | Access to dedicated SIMD instructions 52 | @{ 53 | */ 54 | 55 | /*------------------ RealView Compiler -----------------*/ 56 | #if defined ( __CC_ARM ) 57 | #include "cmsis_armcc.h" 58 | 59 | /*------------------ ARM Compiler V6 -------------------*/ 60 | #elif defined(__ARMCC_VERSION) && (__ARMCC_VERSION >= 6010050) 61 | #include "cmsis_armcc_V6.h" 62 | 63 | /*------------------ GNU Compiler ----------------------*/ 64 | #elif defined ( __GNUC__ ) 65 | #include "cmsis_gcc.h" 66 | 67 | /*------------------ ICC Compiler ----------------------*/ 68 | #elif defined ( __ICCARM__ ) 69 | #include 70 | 71 | /*------------------ TI CCS Compiler -------------------*/ 72 | #elif defined ( __TMS470__ ) 73 | #include 74 | 75 | /*------------------ TASKING Compiler ------------------*/ 76 | #elif defined ( __TASKING__ ) 77 | /* 78 | * The CMSIS functions have been implemented as intrinsics in the compiler. 79 | * Please use "carm -?i" to get an up to date list of all intrinsics, 80 | * Including the CMSIS ones. 81 | */ 82 | 83 | /*------------------ COSMIC Compiler -------------------*/ 84 | #elif defined ( __CSMC__ ) 85 | #include 86 | 87 | #endif 88 | 89 | /*@} end of group CMSIS_SIMD_intrinsics */ 90 | 91 | 92 | #ifdef __cplusplus 93 | } 94 | #endif 95 | 96 | #endif /* __CORE_CMSIMD_H */ 97 | -------------------------------------------------------------------------------- /STMLIB/SDIO/stm32f4_sdio_sd_LowLevel.h: -------------------------------------------------------------------------------- 1 | //****************************************************************************** 2 | /* Define to prevent recursive inclusion -------------------------------------*/ 3 | #ifndef __STM32F4_SDIO_SD_LOWLEVEL_H 4 | #define __STM32F4_SDIO_SD_LOWLEVEL_H 5 | 6 | #include "stm32f4xx.h" 7 | 8 | #define SDIO_FIFO_ADDRESS ((uint32_t)0x40012C80) 9 | /** 10 | * @brief SDIO Intialization Frequency (400KHz max) 11 | */ 12 | #define SDIO_INIT_CLK_DIV ((uint8_t)0x76) 13 | /** 14 | * @brief SDIO Data Transfer Frequency (25MHz max) 15 | */ 16 | #define SDIO_TRANSFER_CLK_DIV ((uint8_t)0x0) 17 | 18 | #define SD_SDIO_DMA DMA2 19 | #define SD_SDIO_DMA_CLK RCC_AHB1Periph_DMA2 20 | 21 | #define SD_SDIO_DMA_STREAM3 3 22 | //#define SD_SDIO_DMA_STREAM6 6 23 | 24 | #ifdef SD_SDIO_DMA_STREAM3 25 | #define SD_SDIO_DMA_STREAM DMA2_Stream3 26 | #define SD_SDIO_DMA_CHANNEL DMA_Channel_4 27 | #define SD_SDIO_DMA_FLAG_FEIF DMA_FLAG_FEIF3 28 | #define SD_SDIO_DMA_FLAG_DMEIF DMA_FLAG_DMEIF3 29 | #define SD_SDIO_DMA_FLAG_TEIF DMA_FLAG_TEIF3 30 | #define SD_SDIO_DMA_FLAG_HTIF DMA_FLAG_HTIF3 31 | #define SD_SDIO_DMA_FLAG_TCIF DMA_FLAG_TCIF3 32 | #define SD_SDIO_DMA_IRQn DMA2_Stream3_IRQn 33 | #define SD_SDIO_DMA_IRQHANDLER DMA2_Stream3_IRQHandler 34 | #elif defined SD_SDIO_DMA_STREAM6 35 | #define SD_SDIO_DMA_STREAM DMA2_Stream6 36 | #define SD_SDIO_DMA_CHANNEL DMA_Channel_4 37 | #define SD_SDIO_DMA_FLAG_FEIF DMA_FLAG_FEIF6 38 | #define SD_SDIO_DMA_FLAG_DMEIF DMA_FLAG_DMEIF6 39 | #define SD_SDIO_DMA_FLAG_TEIF DMA_FLAG_TEIF6 40 | #define SD_SDIO_DMA_FLAG_HTIF DMA_FLAG_HTIF6 41 | #define SD_SDIO_DMA_FLAG_TCIF DMA_FLAG_TCIF6 42 | #define SD_SDIO_DMA_IRQn DMA2_Stream6_IRQn 43 | #define SD_SDIO_DMA_IRQHANDLER DMA2_Stream6_IRQHandler 44 | #endif /* SD_SDIO_DMA_STREAM3 */ 45 | 46 | 47 | void SD_LowLevel_DeInit(void); 48 | void SD_LowLevel_Init(void); 49 | void SD_LowLevel_DMA_TxConfig(uint32_t *BufferSRC, uint32_t BufferSize); 50 | void SD_LowLevel_DMA_RxConfig(uint32_t *BufferDST, uint32_t BufferSize); 51 | 52 | #endif /* __STM32F4_DISCOVERY_SDIO_SD_LOWLEVEL_H */ 53 | //****************************************************************************** 54 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/audio/inc/usbd_audio_out_if.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_audio_out_if.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header file for the usbd_audio_out_if.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | 30 | #ifndef __USB_AUDIO_OUT_IF_H_ 31 | #define __USB_AUDIO_OUT_IF_H_ 32 | 33 | #ifdef STM32F2XX 34 | #include "stm322xg_usb_audio_codec.h" 35 | #elif defined(STM32F4XX) 36 | #include "stm324xg_usb_audio_codec.h" 37 | #elif defined(STM32F10X_CL) 38 | #include "stm3210c_usb_audio_codec.h" 39 | #endif /* STM32F2XX */ 40 | 41 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 42 | * @{ 43 | */ 44 | 45 | /** @defgroup usbd_audio 46 | * @brief This file is the Header file for USBD_audio.c 47 | * @{ 48 | */ 49 | 50 | 51 | /** @defgroup usbd_audio_Exported_Defines 52 | * @{ 53 | */ 54 | /* Audio Commands enmueration */ 55 | typedef enum 56 | { 57 | AUDIO_CMD_PLAY = 1, 58 | AUDIO_CMD_PAUSE, 59 | AUDIO_CMD_STOP, 60 | }AUDIO_CMD_TypeDef; 61 | 62 | /* Mute commands */ 63 | #define AUDIO_MUTE 0x01 64 | #define AUDIO_UNMUTE 0x00 65 | 66 | /* Functions return value */ 67 | #define AUDIO_OK 0x00 68 | #define AUDIO_FAIL 0xFF 69 | 70 | /* Audio Machine States */ 71 | #define AUDIO_STATE_INACTIVE 0x00 72 | #define AUDIO_STATE_ACTIVE 0x01 73 | #define AUDIO_STATE_PLAYING 0x02 74 | #define AUDIO_STATE_PAUSED 0x03 75 | #define AUDIO_STATE_STOPPED 0x04 76 | #define AUDIO_STATE_ERROR 0x05 77 | 78 | /** 79 | * @} 80 | */ 81 | 82 | 83 | /** @defgroup USBD_CORE_Exported_TypesDefinitions 84 | * @{ 85 | */ 86 | /** 87 | * @} 88 | */ 89 | 90 | 91 | 92 | /** @defgroup USBD_CORE_Exported_Macros 93 | * @{ 94 | */ 95 | /** 96 | * @} 97 | */ 98 | 99 | /** @defgroup USBD_CORE_Exported_Variables 100 | * @{ 101 | */ 102 | 103 | extern AUDIO_FOPS_TypeDef AUDIO_OUT_fops; 104 | 105 | /** 106 | * @} 107 | */ 108 | 109 | /** @defgroup USB_CORE_Exported_Functions 110 | * @{ 111 | */ 112 | /** 113 | * @} 114 | */ 115 | 116 | #endif /* __USB_AUDIO_OUT_IF_H_ */ 117 | /** 118 | * @} 119 | */ 120 | 121 | /** 122 | * @} 123 | */ 124 | 125 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 126 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/cdc/inc/usbd_cdc_if_template.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_cdc_if_template.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for dfu_mal.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBD_CDC_IF_TEMPLATE_H 30 | #define __USBD_CDC_IF_TEMPLATE_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | #include "usbd_conf.h" 35 | #include "usbd_cdc_core.h" 36 | 37 | /* Exported types ------------------------------------------------------------*/ 38 | /* Exported constants --------------------------------------------------------*/ 39 | 40 | extern CDC_IF_Prop_TypeDef TEMPLATE_fops; 41 | 42 | /* Exported macro ------------------------------------------------------------*/ 43 | /* Exported functions ------------------------------------------------------- */ 44 | #endif /* __USBD_CDC_IF_TEMPLATE_H */ 45 | 46 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 47 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/dfu/inc/usbd_dfu_mal.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_dfu_mal.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for usbd_dfu_mal.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __DFU_MAL_H 30 | #define __DFU_MAL_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | 35 | #include "usbd_conf.h" 36 | #include "usbd_dfu_core.h" 37 | 38 | /* Exported types ------------------------------------------------------------*/ 39 | typedef struct _DFU_MAL_PROP 40 | { 41 | const uint8_t* pStrDesc; 42 | uint16_t (*pMAL_Init) (void); 43 | uint16_t (*pMAL_DeInit) (void); 44 | uint16_t (*pMAL_Erase) (uint32_t Add); 45 | uint16_t (*pMAL_Write) (uint32_t Add, uint32_t Len); 46 | uint8_t *(*pMAL_Read) (uint32_t Add, uint32_t Len); 47 | uint16_t (*pMAL_CheckAdd) (uint32_t Add); 48 | const uint32_t EraseTiming; 49 | const uint32_t WriteTiming; 50 | } 51 | DFU_MAL_Prop_TypeDef; 52 | 53 | 54 | /* Exported constants --------------------------------------------------------*/ 55 | #define MAL_OK 0 56 | #define MAL_FAIL 1 57 | 58 | /* utils macro ---------------------------------------------------------------*/ 59 | #define _1st_BYTE(x) (uint8_t)((x)&0xFF) /* 1st addressing cycle */ 60 | #define _2nd_BYTE(x) (uint8_t)(((x)&0xFF00)>>8) /* 2nd addressing cycle */ 61 | #define _3rd_BYTE(x) (uint8_t)(((x)&0xFF0000)>>16) /* 3rd addressing cycle */ 62 | #define _4th_BYTE(x) (uint8_t)(((x)&0xFF000000)>>24) /* 4th addressing cycle */ 63 | 64 | /* Exported macro ------------------------------------------------------------*/ 65 | #define SET_POLLING_TIMING(x) buffer[1] = _1st_BYTE(x);\ 66 | buffer[2] = _2nd_BYTE(x);\ 67 | buffer[3] = _3rd_BYTE(x); 68 | 69 | /* Exported functions ------------------------------------------------------- */ 70 | 71 | uint16_t MAL_Init (void); 72 | uint16_t MAL_DeInit (void); 73 | uint16_t MAL_Erase (uint32_t SectorAddress); 74 | uint16_t MAL_Write (uint32_t SectorAddress, uint32_t DataLength); 75 | uint8_t *MAL_Read (uint32_t SectorAddress, uint32_t DataLength); 76 | uint16_t MAL_GetStatus(uint32_t SectorAddress ,uint8_t Cmd, uint8_t *buffer); 77 | 78 | extern uint8_t MAL_Buffer[XFERSIZE]; /* RAM Buffer for Downloaded Data */ 79 | #endif /* __DFU_MAL_H */ 80 | 81 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 82 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/dfu/inc/usbd_flash_if.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_flash_if.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for usbd_flash_if.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __FLASH_IF_MAL_H 30 | #define __FLASH_IF_MAL_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbd_dfu_mal.h" 34 | 35 | /* Exported types ------------------------------------------------------------*/ 36 | /* Exported constants --------------------------------------------------------*/ 37 | #define FLASH_START_ADD 0x08000000 38 | 39 | #ifdef STM32F2XX 40 | #define FLASH_END_ADD 0x08100000 41 | #define FLASH_IF_STRING "@Internal Flash /0x08000000/03*016Ka,01*016Kg,01*064Kg,07*128Kg" 42 | #elif defined(STM32F4XX) 43 | #define FLASH_END_ADD 0x08100000 44 | #define FLASH_IF_STRING "@Internal Flash /0x08000000/03*016Ka,01*016Kg,01*064Kg,07*128Kg" 45 | #elif defined(STM32F10X_CL) 46 | #define FLASH_END_ADD 0x08040000 47 | #define FLASH_IF_STRING "@Internal Flash /0x08000000/06*002Ka,122*002Kg" 48 | #endif /* STM32F2XX */ 49 | 50 | 51 | extern DFU_MAL_Prop_TypeDef DFU_Flash_cb; 52 | 53 | /* Exported macro ------------------------------------------------------------*/ 54 | /* Exported functions ------------------------------------------------------- */ 55 | 56 | #endif /* __FLASH_IF_MAL_H */ 57 | 58 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 59 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/dfu/inc/usbd_mem_if_template.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_mem_if_template.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for usbd_mem_if_template.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __MEM_IF_MAL_H 30 | #define __MEM_IF_MAL_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | 35 | #include "usbd_dfu_mal.h" 36 | 37 | /* Exported types ------------------------------------------------------------*/ 38 | /* Exported constants --------------------------------------------------------*/ 39 | #define MEM_START_ADD 0x00000000 /* Dummy start address */ 40 | #define MEM_END_ADD (uint32_t)(MEM_START_ADD + (5 * 1024)) /* Dummy Size = 5KB */ 41 | 42 | #define MEM_IF_STRING "@Dummy Memory /0x00000000/01*002Kg,03*001Kg" 43 | 44 | extern DFU_MAL_Prop_TypeDef DFU_Mem_cb; 45 | 46 | /* Exported macro ------------------------------------------------------------*/ 47 | /* Exported functions ------------------------------------------------------- */ 48 | 49 | #endif /* __MEM_IF_MAL_H */ 50 | 51 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 52 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/dfu/inc/usbd_otp_if.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_otp_if.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for usbd_otp_if.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __OTP_IF_MAL_H 30 | #define __OTP_IF_MAL_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbd_dfu_mal.h" 34 | 35 | /* Exported types ------------------------------------------------------------*/ 36 | /* Exported constants --------------------------------------------------------*/ 37 | #define OTP_START_ADD 0x1FFF7800 38 | #define OTP_END_ADD (uint32_t)(OTP_START_ADD + 528) 39 | 40 | #define OTP_IF_STRING "@OTP Area /0x1FFF7800/01*512 g,01*016 g" 41 | 42 | extern DFU_MAL_Prop_TypeDef DFU_Otp_cb; 43 | 44 | /* Exported macro ------------------------------------------------------------*/ 45 | /* Exported functions ------------------------------------------------------- */ 46 | 47 | #endif /* __OTP_IF_MAL_H */ 48 | 49 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 50 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/hid/inc/usbd_hid_core.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_hid_core.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header file for the usbd_hid_core.c file. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | 30 | #ifndef __USB_HID_CORE_H_ 31 | #define __USB_HID_CORE_H_ 32 | 33 | #include "usbd_ioreq.h" 34 | 35 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 36 | * @{ 37 | */ 38 | 39 | /** @defgroup USBD_HID 40 | * @brief This file is the Header file for USBD_msc.c 41 | * @{ 42 | */ 43 | 44 | 45 | /** @defgroup USBD_HID_Exported_Defines 46 | * @{ 47 | */ 48 | #define USB_HID_CONFIG_DESC_SIZ 34 49 | #define USB_HID_DESC_SIZ 9 50 | #define HID_MOUSE_REPORT_DESC_SIZE 74 51 | 52 | #define HID_DESCRIPTOR_TYPE 0x21 53 | #define HID_REPORT_DESC 0x22 54 | 55 | 56 | #define HID_REQ_SET_PROTOCOL 0x0B 57 | #define HID_REQ_GET_PROTOCOL 0x03 58 | 59 | #define HID_REQ_SET_IDLE 0x0A 60 | #define HID_REQ_GET_IDLE 0x02 61 | 62 | #define HID_REQ_SET_REPORT 0x09 63 | #define HID_REQ_GET_REPORT 0x01 64 | /** 65 | * @} 66 | */ 67 | 68 | 69 | /** @defgroup USBD_CORE_Exported_TypesDefinitions 70 | * @{ 71 | */ 72 | 73 | 74 | /** 75 | * @} 76 | */ 77 | 78 | 79 | 80 | /** @defgroup USBD_CORE_Exported_Macros 81 | * @{ 82 | */ 83 | 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup USBD_CORE_Exported_Variables 89 | * @{ 90 | */ 91 | 92 | extern USBD_Class_cb_TypeDef USBD_HID_cb; 93 | /** 94 | * @} 95 | */ 96 | 97 | /** @defgroup USB_CORE_Exported_Functions 98 | * @{ 99 | */ 100 | uint8_t USBD_HID_SendReport (USB_OTG_CORE_HANDLE *pdev, 101 | uint8_t *report, 102 | uint16_t len); 103 | /** 104 | * @} 105 | */ 106 | 107 | #endif // __USB_HID_CORE_H_ 108 | /** 109 | * @} 110 | */ 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 117 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/msc/inc/usbd_msc_core.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_msc_core.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header for the usbd_msc_core.c file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef _USB_MSC_CORE_H_ 30 | #define _USB_MSC_CORE_H_ 31 | 32 | #include "usbd_ioreq.h" 33 | 34 | /** @addtogroup USBD_MSC_BOT 35 | * @{ 36 | */ 37 | 38 | /** @defgroup USBD_MSC 39 | * @brief This file is the Header file for USBD_msc.c 40 | * @{ 41 | */ 42 | 43 | 44 | /** @defgroup USBD_BOT_Exported_Defines 45 | * @{ 46 | */ 47 | 48 | 49 | #define BOT_GET_MAX_LUN 0xFE 50 | #define BOT_RESET 0xFF 51 | #define USB_MSC_CONFIG_DESC_SIZ 32 52 | 53 | #define MSC_EPIN_SIZE MSC_MAX_PACKET 54 | #define MSC_EPOUT_SIZE MSC_MAX_PACKET 55 | 56 | /** 57 | * @} 58 | */ 59 | 60 | /** @defgroup USB_CORE_Exported_Types 61 | * @{ 62 | */ 63 | 64 | extern USBD_Class_cb_TypeDef USBD_MSC_cb; 65 | /** 66 | * @} 67 | */ 68 | 69 | /** 70 | * @} 71 | */ 72 | #endif // _USB_MSC_CORE_H_ 73 | /** 74 | * @} 75 | */ 76 | 77 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 78 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/msc/inc/usbd_msc_data.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_msc_data.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header for the usbd_msc_data.c file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | 30 | #ifndef _USBD_MSC_DATA_H_ 31 | #define _USBD_MSC_DATA_H_ 32 | 33 | /* Includes ------------------------------------------------------------------*/ 34 | #include "usbd_conf.h" 35 | 36 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USB_INFO 41 | * @brief general defines for the usb device library file 42 | * @{ 43 | */ 44 | 45 | /** @defgroup USB_INFO_Exported_Defines 46 | * @{ 47 | */ 48 | #define MODE_SENSE6_LEN 8 49 | #define MODE_SENSE10_LEN 8 50 | #define LENGTH_INQUIRY_PAGE00 7 51 | #define LENGTH_FORMAT_CAPACITIES 20 52 | 53 | /** 54 | * @} 55 | */ 56 | 57 | 58 | /** @defgroup USBD_INFO_Exported_TypesDefinitions 59 | * @{ 60 | */ 61 | /** 62 | * @} 63 | */ 64 | 65 | 66 | 67 | /** @defgroup USBD_INFO_Exported_Macros 68 | * @{ 69 | */ 70 | 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup USBD_INFO_Exported_Variables 76 | * @{ 77 | */ 78 | extern const uint8_t MSC_Page00_Inquiry_Data[]; 79 | extern const uint8_t MSC_Mode_Sense6_data[]; 80 | extern const uint8_t MSC_Mode_Sense10_data[] ; 81 | 82 | /** 83 | * @} 84 | */ 85 | 86 | /** @defgroup USBD_INFO_Exported_FunctionsPrototype 87 | * @{ 88 | */ 89 | 90 | /** 91 | * @} 92 | */ 93 | 94 | #endif /* _USBD_MSC_DATA_H_ */ 95 | 96 | /** 97 | * @} 98 | */ 99 | 100 | /** 101 | * @} 102 | */ 103 | 104 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 105 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/msc/inc/usbd_msc_mem.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_msc_mem.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header for the STORAGE DISK file file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | 30 | #ifndef __USBD_MEM_H 31 | #define __USBD_MEM_H 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbd_def.h" 34 | 35 | 36 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USBD_MEM 41 | * @brief header file for the storage disk file 42 | * @{ 43 | */ 44 | 45 | /** @defgroup USBD_MEM_Exported_Defines 46 | * @{ 47 | */ 48 | #define USBD_STD_INQUIRY_LENGTH 36 49 | /** 50 | * @} 51 | */ 52 | 53 | 54 | /** @defgroup USBD_MEM_Exported_TypesDefinitions 55 | * @{ 56 | */ 57 | 58 | typedef struct _USBD_STORAGE 59 | { 60 | int8_t (* Init) (uint8_t lun); 61 | int8_t (* GetCapacity) (uint8_t lun, uint32_t *block_num, uint32_t *block_size); 62 | int8_t (* IsReady) (uint8_t lun); 63 | int8_t (* IsWriteProtected) (uint8_t lun); 64 | int8_t (* Read) (uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len); 65 | int8_t (* Write)(uint8_t lun, uint8_t *buf, uint32_t blk_addr, uint16_t blk_len); 66 | int8_t (* GetMaxLun)(void); 67 | int8_t *pInquiry; 68 | 69 | }USBD_STORAGE_cb_TypeDef; 70 | /** 71 | * @} 72 | */ 73 | 74 | 75 | 76 | /** @defgroup USBD_MEM_Exported_Macros 77 | * @{ 78 | */ 79 | 80 | /** 81 | * @} 82 | */ 83 | 84 | /** @defgroup USBD_MEM_Exported_Variables 85 | * @{ 86 | */ 87 | 88 | /** 89 | * @} 90 | */ 91 | 92 | /** @defgroup USBD_MEM_Exported_FunctionsPrototype 93 | * @{ 94 | */ 95 | extern USBD_STORAGE_cb_TypeDef *USBD_STORAGE_fops; 96 | /** 97 | * @} 98 | */ 99 | 100 | #endif /* __USBD_MEM_H */ 101 | /** 102 | * @} 103 | */ 104 | 105 | /** 106 | * @} 107 | */ 108 | 109 | /** 110 | * @} 111 | */ 112 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 113 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/msc/src/usbd_msc_data.c: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_msc_data.c 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief This file provides all the vital inquiry pages and sense data. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Includes ------------------------------------------------------------------*/ 29 | #include "usbd_msc_data.h" 30 | 31 | 32 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 33 | * @{ 34 | */ 35 | 36 | 37 | /** @defgroup MSC_DATA 38 | * @brief Mass storage info/data module 39 | * @{ 40 | */ 41 | 42 | /** @defgroup MSC_DATA_Private_TypesDefinitions 43 | * @{ 44 | */ 45 | /** 46 | * @} 47 | */ 48 | 49 | 50 | /** @defgroup MSC_DATA_Private_Defines 51 | * @{ 52 | */ 53 | /** 54 | * @} 55 | */ 56 | 57 | 58 | /** @defgroup MSC_DATA_Private_Macros 59 | * @{ 60 | */ 61 | /** 62 | * @} 63 | */ 64 | 65 | 66 | /** @defgroup MSC_DATA_Private_Variables 67 | * @{ 68 | */ 69 | 70 | 71 | /* USB Mass storage Page 0 Inquiry Data */ 72 | const uint8_t MSC_Page00_Inquiry_Data[] = {//7 73 | 0x00, 74 | 0x00, 75 | 0x00, 76 | (LENGTH_INQUIRY_PAGE00 - 4), 77 | 0x00, 78 | 0x80, 79 | 0x83 80 | }; 81 | /* USB Mass storage sense 6 Data */ 82 | const uint8_t MSC_Mode_Sense6_data[] = { 83 | 0x00, 84 | 0x00, 85 | 0x00, 86 | 0x00, 87 | 0x00, 88 | 0x00, 89 | 0x00, 90 | 0x00 91 | }; 92 | /* USB Mass storage sense 10 Data */ 93 | const uint8_t MSC_Mode_Sense10_data[] = { 94 | 0x00, 95 | 0x06, 96 | 0x00, 97 | 0x00, 98 | 0x00, 99 | 0x00, 100 | 0x00, 101 | 0x00 102 | }; 103 | /** 104 | * @} 105 | */ 106 | 107 | 108 | /** @defgroup MSC_DATA_Private_FunctionPrototypes 109 | * @{ 110 | */ 111 | /** 112 | * @} 113 | */ 114 | 115 | 116 | /** @defgroup MSC_DATA_Private_Functions 117 | * @{ 118 | */ 119 | 120 | /** 121 | * @} 122 | */ 123 | 124 | 125 | /** 126 | * @} 127 | */ 128 | 129 | 130 | /** 131 | * @} 132 | */ 133 | 134 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 135 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Class/msc/src/usbd_msc_scsi.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/STMLIB/USB/STM32_USB_Device_Library/Class/msc/src/usbd_msc_scsi.c -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Core/inc/usbd_conf_template.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_conf_template.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief usb device configuration template file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBD_CONF__H__ 30 | #define __USBD_CONF__H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | 35 | /** @defgroup USB_CONF_Exported_Defines 36 | * @{ 37 | */ 38 | #define USE_USB_OTG_HS 39 | 40 | #define USBD_CFG_MAX_NUM 1 41 | #define USB_MAX_STR_DESC_SIZ 64 42 | #define USBD_EP0_MAX_PACKET_SIZE 64 43 | 44 | /** 45 | * @} 46 | */ 47 | 48 | 49 | /** @defgroup USB_CONF_Exported_Types 50 | * @{ 51 | */ 52 | /** 53 | * @} 54 | */ 55 | 56 | 57 | /** @defgroup USB_CONF_Exported_Macros 58 | * @{ 59 | */ 60 | /** 61 | * @} 62 | */ 63 | 64 | /** @defgroup USB_CONF_Exported_Variables 65 | * @{ 66 | */ 67 | /** 68 | * @} 69 | */ 70 | 71 | /** @defgroup USB_CONF_Exported_FunctionsPrototype 72 | * @{ 73 | */ 74 | /** 75 | * @} 76 | */ 77 | 78 | 79 | #endif //__USBD_CONF__H__ 80 | 81 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 82 | 83 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Core/inc/usbd_core.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_core.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header file for usbd_core.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBD_CORE_H 30 | #define __USBD_CORE_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_dcd.h" 34 | #include "usbd_def.h" 35 | #include "usbd_conf.h" 36 | 37 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 38 | * @{ 39 | */ 40 | 41 | /** @defgroup USBD_CORE 42 | * @brief This file is the Header file for usbd_core.c file 43 | * @{ 44 | */ 45 | 46 | 47 | /** @defgroup USBD_CORE_Exported_Defines 48 | * @{ 49 | */ 50 | 51 | typedef enum { 52 | USBD_OK = 0, 53 | USBD_BUSY, 54 | USBD_FAIL, 55 | }USBD_Status; 56 | /** 57 | * @} 58 | */ 59 | 60 | 61 | /** @defgroup USBD_CORE_Exported_TypesDefinitions 62 | * @{ 63 | */ 64 | 65 | 66 | /** 67 | * @} 68 | */ 69 | 70 | 71 | 72 | /** @defgroup USBD_CORE_Exported_Macros 73 | * @{ 74 | */ 75 | 76 | /** 77 | * @} 78 | */ 79 | 80 | /** @defgroup USBD_CORE_Exported_Variables 81 | * @{ 82 | */ 83 | 84 | /** 85 | * @} 86 | */ 87 | 88 | /** @defgroup USBD_CORE_Exported_FunctionsPrototype 89 | * @{ 90 | */ 91 | void USBD_Init(USB_OTG_CORE_HANDLE *pdev, 92 | USB_OTG_CORE_ID_TypeDef coreID, 93 | USBD_DEVICE *pDevice, 94 | USBD_Class_cb_TypeDef *class_cb, 95 | USBD_Usr_cb_TypeDef *usr_cb); 96 | 97 | USBD_Status USBD_DeInit(USB_OTG_CORE_HANDLE *pdev); 98 | 99 | USBD_Status USBD_ClrCfg(USB_OTG_CORE_HANDLE *pdev, uint8_t cfgidx); 100 | 101 | USBD_Status USBD_SetCfg(USB_OTG_CORE_HANDLE *pdev, uint8_t cfgidx); 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | #endif /* __USBD_CORE_H */ 108 | 109 | /** 110 | * @} 111 | */ 112 | 113 | /** 114 | * @} 115 | */ 116 | 117 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 118 | 119 | 120 | 121 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Core/inc/usbd_ioreq.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_ioreq.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header file for the usbd_ioreq.c file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | 30 | #ifndef __USBD_IOREQ_H_ 31 | #define __USBD_IOREQ_H_ 32 | 33 | /* Includes ------------------------------------------------------------------*/ 34 | #include "usbd_def.h" 35 | #include "usbd_core.h" 36 | 37 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 38 | * @{ 39 | */ 40 | 41 | /** @defgroup USBD_IOREQ 42 | * @brief header file for the usbd_ioreq.c file 43 | * @{ 44 | */ 45 | 46 | /** @defgroup USBD_IOREQ_Exported_Defines 47 | * @{ 48 | */ 49 | /** 50 | * @} 51 | */ 52 | 53 | 54 | /** @defgroup USBD_IOREQ_Exported_Types 55 | * @{ 56 | */ 57 | 58 | 59 | /** 60 | * @} 61 | */ 62 | 63 | 64 | 65 | /** @defgroup USBD_IOREQ_Exported_Macros 66 | * @{ 67 | */ 68 | 69 | /** 70 | * @} 71 | */ 72 | 73 | /** @defgroup USBD_IOREQ_Exported_Variables 74 | * @{ 75 | */ 76 | 77 | /** 78 | * @} 79 | */ 80 | 81 | /** @defgroup USBD_IOREQ_Exported_FunctionsPrototype 82 | * @{ 83 | */ 84 | 85 | USBD_Status USBD_CtlSendData (USB_OTG_CORE_HANDLE *pdev, 86 | uint8_t *buf, 87 | uint16_t len); 88 | 89 | USBD_Status USBD_CtlContinueSendData (USB_OTG_CORE_HANDLE *pdev, 90 | uint8_t *pbuf, 91 | uint16_t len); 92 | 93 | USBD_Status USBD_CtlPrepareRx (USB_OTG_CORE_HANDLE *pdev, 94 | uint8_t *pbuf, 95 | uint16_t len); 96 | 97 | USBD_Status USBD_CtlContinueRx (USB_OTG_CORE_HANDLE *pdev, 98 | uint8_t *pbuf, 99 | uint16_t len); 100 | 101 | USBD_Status USBD_CtlSendStatus (USB_OTG_CORE_HANDLE *pdev); 102 | 103 | USBD_Status USBD_CtlReceiveStatus (USB_OTG_CORE_HANDLE *pdev); 104 | 105 | uint16_t USBD_GetRxCount (USB_OTG_CORE_HANDLE *pdev , 106 | uint8_t epnum); 107 | 108 | /** 109 | * @} 110 | */ 111 | 112 | #endif /* __USBD_IOREQ_H_ */ 113 | 114 | /** 115 | * @} 116 | */ 117 | 118 | /** 119 | * @} 120 | */ 121 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 122 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Core/inc/usbd_req.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_req.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header file for the usbd_req.c file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | 30 | #ifndef __USB_REQUEST_H_ 31 | #define __USB_REQUEST_H_ 32 | 33 | /* Includes ------------------------------------------------------------------*/ 34 | #include "usbd_def.h" 35 | #include "usbd_core.h" 36 | #include "usbd_conf.h" 37 | 38 | 39 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 40 | * @{ 41 | */ 42 | 43 | /** @defgroup USBD_REQ 44 | * @brief header file for the usbd_ioreq.c file 45 | * @{ 46 | */ 47 | 48 | /** @defgroup USBD_REQ_Exported_Defines 49 | * @{ 50 | */ 51 | /** 52 | * @} 53 | */ 54 | 55 | 56 | /** @defgroup USBD_REQ_Exported_Types 57 | * @{ 58 | */ 59 | /** 60 | * @} 61 | */ 62 | 63 | 64 | 65 | /** @defgroup USBD_REQ_Exported_Macros 66 | * @{ 67 | */ 68 | /** 69 | * @} 70 | */ 71 | 72 | /** @defgroup USBD_REQ_Exported_Variables 73 | * @{ 74 | */ 75 | /** 76 | * @} 77 | */ 78 | 79 | /** @defgroup USBD_REQ_Exported_FunctionsPrototype 80 | * @{ 81 | */ 82 | 83 | USBD_Status USBD_StdDevReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 84 | USBD_Status USBD_StdItfReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 85 | USBD_Status USBD_StdEPReq (USB_OTG_CORE_HANDLE *pdev, USB_SETUP_REQ *req); 86 | void USBD_ParseSetupRequest( USB_OTG_CORE_HANDLE *pdev, 87 | USB_SETUP_REQ *req); 88 | 89 | void USBD_CtlError( USB_OTG_CORE_HANDLE *pdev, 90 | USB_SETUP_REQ *req); 91 | 92 | void USBD_GetString(uint8_t *desc, uint8_t *unicode, uint16_t *len); 93 | /** 94 | * @} 95 | */ 96 | 97 | #endif /* __USB_REQUEST_H_ */ 98 | 99 | /** 100 | * @} 101 | */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | 108 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 109 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_Device_Library/Core/inc/usbd_usr.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_usr.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header file for usbd_usr.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBD_USR_H__ 30 | #define __USBD_USR_H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbd_core.h" 34 | 35 | 36 | /** @addtogroup USBD_USER 37 | * @{ 38 | */ 39 | 40 | /** @addtogroup USBD_MSC_DEMO_USER_CALLBACKS 41 | * @{ 42 | */ 43 | 44 | /** @defgroup USBD_USR 45 | * @brief This file is the Header file for usbd_usr.c 46 | * @{ 47 | */ 48 | 49 | 50 | /** @defgroup USBD_USR_Exported_Types 51 | * @{ 52 | */ 53 | 54 | extern USBD_Usr_cb_TypeDef USR_cb; 55 | extern USBD_Usr_cb_TypeDef USR_FS_cb; 56 | extern USBD_Usr_cb_TypeDef USR_HS_cb; 57 | 58 | 59 | 60 | /** 61 | * @} 62 | */ 63 | 64 | 65 | 66 | /** @defgroup USBD_USR_Exported_Defines 67 | * @{ 68 | */ 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** @defgroup USBD_USR_Exported_Macros 75 | * @{ 76 | */ 77 | /** 78 | * @} 79 | */ 80 | 81 | /** @defgroup USBD_USR_Exported_Variables 82 | * @{ 83 | */ 84 | 85 | void USBD_USR_Init(void); 86 | void USBD_USR_DeviceReset (uint8_t speed); 87 | void USBD_USR_DeviceConfigured (void); 88 | void USBD_USR_DeviceSuspended(void); 89 | void USBD_USR_DeviceResumed(void); 90 | 91 | void USBD_USR_DeviceConnected(void); 92 | void USBD_USR_DeviceDisconnected(void); 93 | 94 | void USBD_USR_FS_Init(void); 95 | void USBD_USR_FS_DeviceReset (uint8_t speed); 96 | void USBD_USR_FS_DeviceConfigured (void); 97 | void USBD_USR_FS_DeviceSuspended(void); 98 | void USBD_USR_FS_DeviceResumed(void); 99 | 100 | void USBD_USR_FS_DeviceConnected(void); 101 | void USBD_USR_FS_DeviceDisconnected(void); 102 | 103 | void USBD_USR_HS_Init(void); 104 | void USBD_USR_HS_DeviceReset (uint8_t speed); 105 | void USBD_USR_HS_DeviceConfigured (void); 106 | void USBD_USR_HS_DeviceSuspended(void); 107 | void USBD_USR_HS_DeviceResumed(void); 108 | 109 | void USBD_USR_HS_DeviceConnected(void); 110 | void USBD_USR_HS_DeviceDisconnected(void); 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | /** @defgroup USBD_USR_Exported_FunctionsPrototype 117 | * @{ 118 | */ 119 | /** 120 | * @} 121 | */ 122 | 123 | #endif /*__USBD_USR_H__*/ 124 | 125 | /** 126 | * @} 127 | */ 128 | 129 | /** 130 | * @} 131 | */ 132 | 133 | /** 134 | * @} 135 | */ 136 | 137 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 138 | 139 | 140 | 141 | 142 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_HOST_Library/Class/HID/inc/usbh_hid_keybd.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbh_hid_keybd.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief This file contains all the prototypes for the usbh_hid_keybd.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive -----------------------------------------------*/ 29 | #ifndef __USBH_HID_KEYBD_H 30 | #define __USBH_HID_KEYBD_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | #include "usbh_hid_core.h" 35 | 36 | /** @addtogroup USBH_LIB 37 | * @{ 38 | */ 39 | 40 | /** @addtogroup USBH_CLASS 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup USBH_HID_CLASS 45 | * @{ 46 | */ 47 | 48 | /** @defgroup USBH_HID_KEYBD 49 | * @brief This file is the Header file for USBH_HID_KEYBD.c 50 | * @{ 51 | */ 52 | 53 | 54 | /** @defgroup USBH_HID_KEYBD_Exported_Types 55 | * @{ 56 | */ 57 | 58 | 59 | /** 60 | * @} 61 | */ 62 | 63 | /** @defgroup USBH_HID_KEYBD_Exported_Defines 64 | * @{ 65 | */ 66 | //#define QWERTY_KEYBOARD 67 | #define AZERTY_KEYBOARD 68 | 69 | #define KBD_LEFT_CTRL 0x01 70 | #define KBD_LEFT_SHIFT 0x02 71 | #define KBD_LEFT_ALT 0x04 72 | #define KBD_LEFT_GUI 0x08 73 | #define KBD_RIGHT_CTRL 0x10 74 | #define KBD_RIGHT_SHIFT 0x20 75 | #define KBD_RIGHT_ALT 0x40 76 | #define KBD_RIGHT_GUI 0x80 77 | 78 | #define KBR_MAX_NBR_PRESSED 6 79 | 80 | /** 81 | * @} 82 | */ 83 | 84 | /** @defgroup USBH_HID_KEYBD_Exported_Macros 85 | * @{ 86 | */ 87 | /** 88 | * @} 89 | */ 90 | 91 | /** @defgroup USBH_HID_KEYBD_Exported_Variables 92 | * @{ 93 | */ 94 | 95 | extern HID_cb_TypeDef HID_KEYBRD_cb; 96 | /** 97 | * @} 98 | */ 99 | 100 | /** @defgroup USBH_HID_KEYBD_Exported_FunctionsPrototype 101 | * @{ 102 | */ 103 | void USR_KEYBRD_Init (void); 104 | void USR_KEYBRD_ProcessData (uint8_t pbuf); 105 | /** 106 | * @} 107 | */ 108 | 109 | #endif /* __USBH_HID_KEYBD_H */ 110 | 111 | /** 112 | * @} 113 | */ 114 | 115 | /** 116 | * @} 117 | */ 118 | 119 | /** 120 | * @} 121 | */ 122 | 123 | /** 124 | * @} 125 | */ 126 | 127 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 128 | 129 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_HOST_Library/Class/HID/inc/usbh_hid_mouse.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbh_hid_mouse.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief This file contains all the prototypes for the usbh_hid_mouse.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | 29 | /* Define to prevent recursive ----------------------------------------------*/ 30 | #ifndef __USBH_HID_MOUSE_H 31 | #define __USBH_HID_MOUSE_H 32 | 33 | /* Includes ------------------------------------------------------------------*/ 34 | #include "usbh_hid_core.h" 35 | 36 | /** @addtogroup USBH_LIB 37 | * @{ 38 | */ 39 | 40 | /** @addtogroup USBH_CLASS 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup USBH_HID_CLASS 45 | * @{ 46 | */ 47 | 48 | /** @defgroup USBH_HID_MOUSE 49 | * @brief This file is the Header file for USBH_HID_MOUSE.c 50 | * @{ 51 | */ 52 | 53 | 54 | /** @defgroup USBH_HID_MOUSE_Exported_Types 55 | * @{ 56 | */ 57 | typedef struct _HID_MOUSE_Data 58 | { 59 | uint8_t x; 60 | uint8_t y; 61 | uint8_t z; /* Not Supported */ 62 | uint8_t button; 63 | } 64 | HID_MOUSE_Data_TypeDef; 65 | 66 | /** 67 | * @} 68 | */ 69 | 70 | /** @defgroup USBH_HID_MOUSE_Exported_Defines 71 | * @{ 72 | */ 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup USBH_HID_MOUSE_Exported_Macros 78 | * @{ 79 | */ 80 | /** 81 | * @} 82 | */ 83 | 84 | /** @defgroup USBH_HID_MOUSE_Exported_Variables 85 | * @{ 86 | */ 87 | 88 | extern HID_cb_TypeDef HID_MOUSE_cb; 89 | extern HID_MOUSE_Data_TypeDef HID_MOUSE_Data; 90 | /** 91 | * @} 92 | */ 93 | 94 | /** @defgroup USBH_HID_MOUSE_Exported_FunctionsPrototype 95 | * @{ 96 | */ 97 | void USR_MOUSE_Init (void); 98 | void USR_MOUSE_ProcessData (HID_MOUSE_Data_TypeDef *data); 99 | /** 100 | * @} 101 | */ 102 | 103 | #endif /* __USBH_HID_MOUSE_H */ 104 | 105 | /** 106 | * @} 107 | */ 108 | 109 | /** 110 | * @} 111 | */ 112 | 113 | /** 114 | * @} 115 | */ 116 | 117 | /** 118 | * @} 119 | */ 120 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 121 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_HOST_Library/Class/MSC/inc/usbh_msc_core.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbh_msc_core.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief This file contains all the prototypes for the usbh_msc_core.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive ----------------------------------------------*/ 29 | #ifndef __USBH_MSC_CORE_H 30 | #define __USBH_MSC_CORE_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbh_core.h" 34 | #include "usbh_stdreq.h" 35 | #include "usb_bsp.h" 36 | #include "usbh_ioreq.h" 37 | #include "usbh_hcs.h" 38 | #include "usbh_msc_core.h" 39 | #include "usbh_msc_scsi.h" 40 | #include "usbh_msc_bot.h" 41 | 42 | /** @addtogroup USBH_LIB 43 | * @{ 44 | */ 45 | 46 | /** @addtogroup USBH_CLASS 47 | * @{ 48 | */ 49 | 50 | /** @addtogroup USBH_MSC_CLASS 51 | * @{ 52 | */ 53 | 54 | /** @defgroup USBH_MSC_CORE 55 | * @brief This file is the Header file for usbh_msc_core.c 56 | * @{ 57 | */ 58 | 59 | 60 | /** @defgroup USBH_MSC_CORE_Exported_Types 61 | * @{ 62 | */ 63 | 64 | 65 | /* Structure for MSC process */ 66 | typedef struct _MSC_Process 67 | { 68 | uint8_t hc_num_in; 69 | uint8_t hc_num_out; 70 | uint8_t MSBulkOutEp; 71 | uint8_t MSBulkInEp; 72 | uint16_t MSBulkInEpSize; 73 | uint16_t MSBulkOutEpSize; 74 | uint8_t buff[USBH_MSC_MPS_SIZE]; 75 | uint8_t maxLun; 76 | } 77 | MSC_Machine_TypeDef; 78 | 79 | 80 | /** 81 | * @} 82 | */ 83 | 84 | 85 | 86 | /** @defgroup USBH_MSC_CORE_Exported_Defines 87 | * @{ 88 | */ 89 | 90 | #define USB_REQ_BOT_RESET 0xFF 91 | #define USB_REQ_GET_MAX_LUN 0xFE 92 | 93 | 94 | /** 95 | * @} 96 | */ 97 | 98 | /** @defgroup USBH_MSC_CORE_Exported_Macros 99 | * @{ 100 | */ 101 | /** 102 | * @} 103 | */ 104 | 105 | /** @defgroup USBH_MSC_CORE_Exported_Variables 106 | * @{ 107 | */ 108 | extern USBH_Class_cb_TypeDef USBH_MSC_cb; 109 | extern MSC_Machine_TypeDef MSC_Machine; 110 | extern uint8_t MSCErrorCount; 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | /** @defgroup USBH_MSC_CORE_Exported_FunctionsPrototype 117 | * @{ 118 | */ 119 | 120 | 121 | 122 | /** 123 | * @} 124 | */ 125 | 126 | #endif /* __USBH_MSC_CORE_H */ 127 | 128 | 129 | /** 130 | * @} 131 | */ 132 | 133 | /** 134 | * @} 135 | */ 136 | 137 | /** 138 | * @} 139 | */ 140 | 141 | /** 142 | * @} 143 | */ 144 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 145 | 146 | 147 | 148 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_HOST_Library/Core/inc/usbh_conf_template.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbh_conf_template 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief General USB Host library configuration 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBH_CONF__H__ 30 | #define __USBH_CONF__H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | 34 | /** @addtogroup USBH_OTG_DRIVER 35 | * @{ 36 | */ 37 | 38 | /** @defgroup USBH_CONF 39 | * @brief usb otg low level driver configuration file 40 | * @{ 41 | */ 42 | 43 | /** @defgroup USBH_CONF_Exported_Defines 44 | * @{ 45 | */ 46 | 47 | #define USBH_MAX_NUM_ENDPOINTS 2 48 | #define USBH_MAX_NUM_INTERFACES 2 49 | #ifdef USE_USB_OTG_FS 50 | #define USBH_MSC_MPS_SIZE 0x40 51 | #else 52 | #define USBH_MSC_MPS_SIZE 0x200 53 | #endif 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | 60 | /** @defgroup USBH_CONF_Exported_Types 61 | * @{ 62 | */ 63 | /** 64 | * @} 65 | */ 66 | 67 | 68 | /** @defgroup USBH_CONF_Exported_Macros 69 | * @{ 70 | */ 71 | /** 72 | * @} 73 | */ 74 | 75 | /** @defgroup USBH_CONF_Exported_Variables 76 | * @{ 77 | */ 78 | /** 79 | * @} 80 | */ 81 | 82 | /** @defgroup USBH_CONF_Exported_FunctionsPrototype 83 | * @{ 84 | */ 85 | /** 86 | * @} 87 | */ 88 | 89 | 90 | #endif //__USBH_CONF__H__ 91 | 92 | 93 | /** 94 | * @} 95 | */ 96 | 97 | /** 98 | * @} 99 | */ 100 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 101 | 102 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_HOST_Library/Core/inc/usbh_hcs.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbh_hcs.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief Header file for usbh_hcs.c 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive ----------------------------------------------*/ 29 | #ifndef __USBH_HCS_H 30 | #define __USBH_HCS_H 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usbh_core.h" 34 | 35 | /** @addtogroup USBH_LIB 36 | * @{ 37 | */ 38 | 39 | /** @addtogroup USBH_LIB_CORE 40 | * @{ 41 | */ 42 | 43 | /** @defgroup USBH_HCS 44 | * @brief This file is the header file for usbh_hcs.c 45 | * @{ 46 | */ 47 | 48 | /** @defgroup USBH_HCS_Exported_Defines 49 | * @{ 50 | */ 51 | #define HC_MAX 8 52 | 53 | #define HC_OK 0x0000 54 | #define HC_USED 0x8000 55 | #define HC_ERROR 0xFFFF 56 | #define HC_USED_MASK 0x7FFF 57 | /** 58 | * @} 59 | */ 60 | 61 | /** @defgroup USBH_HCS_Exported_Types 62 | * @{ 63 | */ 64 | /** 65 | * @} 66 | */ 67 | 68 | 69 | /** @defgroup USBH_HCS_Exported_Macros 70 | * @{ 71 | */ 72 | /** 73 | * @} 74 | */ 75 | 76 | /** @defgroup USBH_HCS_Exported_Variables 77 | * @{ 78 | */ 79 | /** 80 | * @} 81 | */ 82 | 83 | /** @defgroup USBH_HCS_Exported_FunctionsPrototype 84 | * @{ 85 | */ 86 | 87 | uint8_t USBH_Alloc_Channel(USB_OTG_CORE_HANDLE *pdev, uint8_t ep_addr); 88 | 89 | uint8_t USBH_Free_Channel (USB_OTG_CORE_HANDLE *pdev, uint8_t idx); 90 | 91 | uint8_t USBH_DeAllocate_AllChannel (USB_OTG_CORE_HANDLE *pdev); 92 | 93 | uint8_t USBH_Open_Channel (USB_OTG_CORE_HANDLE *pdev, 94 | uint8_t ch_num, 95 | uint8_t dev_address, 96 | uint8_t speed, 97 | uint8_t ep_type, 98 | uint16_t mps); 99 | 100 | uint8_t USBH_Modify_Channel (USB_OTG_CORE_HANDLE *pdev, 101 | uint8_t hc_num, 102 | uint8_t dev_address, 103 | uint8_t speed, 104 | uint8_t ep_type, 105 | uint16_t mps); 106 | /** 107 | * @} 108 | */ 109 | 110 | 111 | 112 | #endif /* __USBH_HCS_H */ 113 | 114 | 115 | /** 116 | * @} 117 | */ 118 | 119 | /** 120 | * @} 121 | */ 122 | 123 | /** 124 | * @} 125 | */ 126 | 127 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 128 | 129 | 130 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_OTG_Driver/inc/usb_bsp.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_bsp.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief Specific api's relative to the used hardware platform 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USB_BSP__H__ 30 | #define __USB_BSP__H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_core.h" 34 | #include "usb_conf.h" 35 | #include "usbd_ioreq.h" 36 | 37 | /** @addtogroup USB_OTG_DRIVER 38 | * @{ 39 | */ 40 | 41 | /** @defgroup USB_BSP 42 | * @brief This file is the 43 | * @{ 44 | */ 45 | 46 | 47 | /** @defgroup USB_BSP_Exported_Defines 48 | * @{ 49 | */ 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @defgroup USB_BSP_Exported_Types 56 | * @{ 57 | */ 58 | /** 59 | * @} 60 | */ 61 | 62 | 63 | /** @defgroup USB_BSP_Exported_Macros 64 | * @{ 65 | */ 66 | /** 67 | * @} 68 | */ 69 | 70 | /** @defgroup USB_BSP_Exported_Variables 71 | * @{ 72 | */ 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup USB_BSP_Exported_FunctionsPrototype 78 | * @{ 79 | */ 80 | void BSP_Init(void); 81 | 82 | void USB_OTG_BSP_Init (USB_OTG_CORE_HANDLE *pdev); 83 | void USB_OTG_BSP_uDelay (uint32_t usec); 84 | void USB_OTG_BSP_mDelay (uint32_t msec); 85 | void USB_OTG_BSP_EnableInterrupt (USB_OTG_CORE_HANDLE *pdev); 86 | 87 | 88 | #ifdef USE_HOST_MODE 89 | void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev); 90 | void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state); 91 | #endif 92 | /** 93 | * @} 94 | */ 95 | 96 | #endif //__USB_BSP__H__ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /** 103 | * @} 104 | */ 105 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 106 | 107 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_OTG_Driver/inc/usb_dcd_int.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_dcd_int.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief Peripheral Device Interface Layer 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef USB_DCD_INT_H__ 30 | #define USB_DCD_INT_H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_dcd.h" 34 | 35 | 36 | 37 | /** @addtogroup USB_OTG_DRIVER 38 | * @{ 39 | */ 40 | 41 | /** @defgroup USB_DCD_INT 42 | * @brief This file is the 43 | * @{ 44 | */ 45 | 46 | 47 | /** @defgroup USB_DCD_INT_Exported_Defines 48 | * @{ 49 | */ 50 | 51 | typedef struct _USBD_DCD_INT 52 | { 53 | uint8_t (* DataOutStage) (USB_OTG_CORE_HANDLE *pdev , uint8_t epnum); 54 | uint8_t (* DataInStage) (USB_OTG_CORE_HANDLE *pdev , uint8_t epnum); 55 | uint8_t (* SetupStage) (USB_OTG_CORE_HANDLE *pdev); 56 | uint8_t (* SOF) (USB_OTG_CORE_HANDLE *pdev); 57 | uint8_t (* Reset) (USB_OTG_CORE_HANDLE *pdev); 58 | uint8_t (* Suspend) (USB_OTG_CORE_HANDLE *pdev); 59 | uint8_t (* Resume) (USB_OTG_CORE_HANDLE *pdev); 60 | uint8_t (* IsoINIncomplete) (USB_OTG_CORE_HANDLE *pdev); 61 | uint8_t (* IsoOUTIncomplete) (USB_OTG_CORE_HANDLE *pdev); 62 | 63 | uint8_t (* DevConnected) (USB_OTG_CORE_HANDLE *pdev); 64 | uint8_t (* DevDisconnected) (USB_OTG_CORE_HANDLE *pdev); 65 | 66 | }USBD_DCD_INT_cb_TypeDef; 67 | 68 | extern USBD_DCD_INT_cb_TypeDef *USBD_DCD_INT_fops; 69 | /** 70 | * @} 71 | */ 72 | 73 | 74 | /** @defgroup USB_DCD_INT_Exported_Types 75 | * @{ 76 | */ 77 | /** 78 | * @} 79 | */ 80 | 81 | /** @defgroup USB_DCD_INT_Exported_Macros 82 | * @{ 83 | */ 84 | 85 | #define CLEAR_IN_EP_INTR(epnum,intr) \ 86 | diepint.d32=0; \ 87 | diepint.b.intr = 1; \ 88 | USB_OTG_WRITE_REG32(&pdev->regs.INEP_REGS[epnum]->DIEPINT,diepint.d32); 89 | 90 | #define CLEAR_OUT_EP_INTR(epnum,intr) \ 91 | doepint.d32=0; \ 92 | doepint.b.intr = 1; \ 93 | USB_OTG_WRITE_REG32(&pdev->regs.OUTEP_REGS[epnum]->DOEPINT,doepint.d32); 94 | 95 | /** 96 | * @} 97 | */ 98 | 99 | /** @defgroup USB_DCD_INT_Exported_Variables 100 | * @{ 101 | */ 102 | /** 103 | * @} 104 | */ 105 | 106 | /** @defgroup USB_DCD_INT_Exported_FunctionsPrototype 107 | * @{ 108 | */ 109 | 110 | uint32_t USBD_OTG_ISR_Handler (USB_OTG_CORE_HANDLE *pdev); 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | 117 | #endif // USB_DCD_INT_H__ 118 | 119 | /** 120 | * @} 121 | */ 122 | 123 | /** 124 | * @} 125 | */ 126 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 127 | 128 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_OTG_Driver/inc/usb_hcd.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_hcd.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief Host layer Header file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USB_HCD_H__ 30 | #define __USB_HCD_H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_regs.h" 34 | #include "usb_core.h" 35 | 36 | 37 | /** @addtogroup USB_OTG_DRIVER 38 | * @{ 39 | */ 40 | 41 | /** @defgroup USB_HCD 42 | * @brief This file is the 43 | * @{ 44 | */ 45 | 46 | 47 | /** @defgroup USB_HCD_Exported_Defines 48 | * @{ 49 | */ 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @defgroup USB_HCD_Exported_Types 56 | * @{ 57 | */ 58 | /** 59 | * @} 60 | */ 61 | 62 | 63 | /** @defgroup USB_HCD_Exported_Macros 64 | * @{ 65 | */ 66 | /** 67 | * @} 68 | */ 69 | 70 | /** @defgroup USB_HCD_Exported_Variables 71 | * @{ 72 | */ 73 | /** 74 | * @} 75 | */ 76 | 77 | /** @defgroup USB_HCD_Exported_FunctionsPrototype 78 | * @{ 79 | */ 80 | uint32_t HCD_Init (USB_OTG_CORE_HANDLE *pdev , 81 | USB_OTG_CORE_ID_TypeDef coreID); 82 | uint32_t HCD_HC_Init (USB_OTG_CORE_HANDLE *pdev , 83 | uint8_t hc_num); 84 | uint32_t HCD_SubmitRequest (USB_OTG_CORE_HANDLE *pdev , 85 | uint8_t hc_num) ; 86 | uint32_t HCD_GetCurrentSpeed (USB_OTG_CORE_HANDLE *pdev); 87 | uint32_t HCD_ResetPort (USB_OTG_CORE_HANDLE *pdev); 88 | uint32_t HCD_IsDeviceConnected (USB_OTG_CORE_HANDLE *pdev); 89 | uint32_t HCD_GetCurrentFrame (USB_OTG_CORE_HANDLE *pdev) ; 90 | URB_STATE HCD_GetURB_State (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num); 91 | uint32_t HCD_GetXferCnt (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num); 92 | HC_STATUS HCD_GetHCState (USB_OTG_CORE_HANDLE *pdev, uint8_t ch_num) ; 93 | /** 94 | * @} 95 | */ 96 | 97 | #endif //__USB_HCD_H__ 98 | 99 | 100 | /** 101 | * @} 102 | */ 103 | 104 | /** 105 | * @} 106 | */ 107 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 108 | 109 | -------------------------------------------------------------------------------- /STMLIB/USB/STM32_USB_OTG_Driver/inc/usb_otg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usb_otg.h 4 | * @author MCD Application Team 5 | * @version V2.1.0 6 | * @date 19-March-2012 7 | * @brief OTG Core Header 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USB_OTG__ 30 | #define __USB_OTG__ 31 | 32 | 33 | /** @addtogroup USB_OTG_DRIVER 34 | * @{ 35 | */ 36 | 37 | /** @defgroup USB_OTG 38 | * @brief This file is the 39 | * @{ 40 | */ 41 | 42 | 43 | /** @defgroup USB_OTG_Exported_Defines 44 | * @{ 45 | */ 46 | 47 | 48 | void USB_OTG_InitiateSRP(void); 49 | void USB_OTG_InitiateHNP(uint8_t state , uint8_t mode); 50 | void USB_OTG_Switchback (USB_OTG_CORE_DEVICE *pdev); 51 | uint32_t USB_OTG_GetCurrentState (USB_OTG_CORE_DEVICE *pdev); 52 | 53 | /** 54 | * @} 55 | */ 56 | 57 | 58 | /** @defgroup USB_OTG_Exported_Types 59 | * @{ 60 | */ 61 | /** 62 | * @} 63 | */ 64 | 65 | 66 | /** @defgroup USB_OTG_Exported_Macros 67 | * @{ 68 | */ 69 | /** 70 | * @} 71 | */ 72 | 73 | /** @defgroup USB_OTG_Exported_Variables 74 | * @{ 75 | */ 76 | /** 77 | * @} 78 | */ 79 | 80 | /** @defgroup USB_OTG_Exported_FunctionsPrototype 81 | * @{ 82 | */ 83 | /** 84 | * @} 85 | */ 86 | 87 | 88 | #endif //__USB_OTG__ 89 | 90 | 91 | /** 92 | * @} 93 | */ 94 | 95 | /** 96 | * @} 97 | */ 98 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 99 | 100 | -------------------------------------------------------------------------------- /STMLIB/USB/usb_bsp.c: -------------------------------------------------------------------------------- 1 | #include "usb_bsp.h" 2 | 3 | void USB_OTG_BSP_Init(USB_OTG_CORE_HANDLE *pdev) 4 | { 5 | GPIO_InitTypeDef GPIO_InitStructure; 6 | 7 | RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE); 8 | RCC_AHB2PeriphClockCmd(RCC_AHB2Periph_OTG_FS, ENABLE); 9 | 10 | GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11 | GPIO_Pin_12; 11 | GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF; 12 | GPIO_InitStructure.GPIO_OType = GPIO_OType_PP; 13 | GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz; 14 | GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL; 15 | GPIO_Init(GPIOA, &GPIO_InitStructure); 16 | 17 | GPIO_PinAFConfig(GPIOA,GPIO_PinSource11,GPIO_AF_OTG_FS); 18 | GPIO_PinAFConfig(GPIOA,GPIO_PinSource12,GPIO_AF_OTG_FS); 19 | } 20 | 21 | void USB_OTG_BSP_EnableInterrupt(USB_OTG_CORE_HANDLE *pdev) 22 | { 23 | NVIC_InitTypeDef NVIC_InitStructure; 24 | NVIC_InitStructure.NVIC_IRQChannel = OTG_FS_IRQn; 25 | NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00; 26 | NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x03; 27 | NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; 28 | NVIC_Init(&NVIC_InitStructure); 29 | } 30 | 31 | void USB_OTG_BSP_DisableInterrupt(void) 32 | { 33 | 34 | } 35 | 36 | void USB_OTG_BSP_uDelay(uint32_t usec) 37 | { 38 | uint32_t us_cnt; 39 | for(; usec!= 0; usec--) 40 | { 41 | us_cnt = 35; 42 | while(us_cnt) 43 | { 44 | us_cnt--; 45 | } 46 | } 47 | } 48 | 49 | void USB_OTG_BSP_mDelay(uint32_t msec) 50 | { 51 | uint32_t us_cnt; 52 | for(; msec!= 0; msec--) 53 | { 54 | us_cnt = 42000; 55 | while(us_cnt) 56 | { 57 | us_cnt--; 58 | } 59 | } 60 | } 61 | 62 | -------------------------------------------------------------------------------- /STMLIB/USB/usbd_conf.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_conf.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief USB Device configuration file 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2012 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __USBD_CONF__H__ 30 | #define __USBD_CONF__H__ 31 | 32 | /* Includes ------------------------------------------------------------------*/ 33 | #include "usb_conf.h" 34 | 35 | /** @addtogroup STM32_USB_OTG_DEVICE_LIBRARY 36 | * @{ 37 | */ 38 | 39 | /** @defgroup USBD_CONF 40 | * @brief This file is the device library configuration file 41 | * @{ 42 | */ 43 | 44 | /** @defgroup USBD_CONF_Exported_Defines 45 | * @{ 46 | */ 47 | 48 | 49 | #define USBD_CFG_MAX_NUM 1 50 | #define USBD_ITF_MAX_NUM 1 51 | #define USB_MAX_STR_DESC_SIZ 64 52 | 53 | #define USBD_SELF_POWERED 54 | 55 | /* Class Layer Parameter */ 56 | 57 | #define CDC_IN_EP 0x81 58 | #define CDC_CMD_EP 0x82 59 | #define CDC_OUT_EP 0x03 60 | 61 | #define CDC_IN_PACKET 64 62 | 63 | #define CDC_OUT_PACKET 64 64 | 65 | #define VIRTUAL_COM_PORT_DATA_SIZE 64 66 | #define VIRTUAL_COM_PORT_INT_SIZE 8 67 | #define APP_RX_DATA_SIZE 64 68 | #define CDC_IN_FRAME_INTERVAL 8 //待改 69 | 70 | #define CDC_MEDIA_PACKET 12*1024 //越大速度越快(不过貌似提升不是很多) 71 | 72 | /** 73 | * @} 74 | */ 75 | 76 | 77 | /** @defgroup USB_CONF_Exported_TypesDefinitions 78 | * @{ 79 | */ 80 | /** 81 | * @} 82 | */ 83 | 84 | 85 | /** @defgroup USB_CONF_Exported_Macros 86 | * @{ 87 | */ 88 | /** 89 | * @} 90 | */ 91 | 92 | /** @defgroup USB_CONF_Exported_Variables 93 | * @{ 94 | */ 95 | /** 96 | * @} 97 | */ 98 | 99 | /** @defgroup USB_CONF_Exported_FunctionsPrototype 100 | * @{ 101 | */ 102 | /** 103 | * @} 104 | */ 105 | 106 | #endif //__USBD_CONF__H__ 107 | 108 | /** 109 | * @} 110 | */ 111 | 112 | /** 113 | * @} 114 | */ 115 | 116 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 117 | 118 | -------------------------------------------------------------------------------- /STMLIB/inc/stm32f4xx_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_crc.h 4 | * @author MCD Application Team 5 | * @version V1.7.0 6 | * @date 22-April-2016 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2016 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_CRC_H 31 | #define __STM32F4xx_CRC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup CRC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup CRC_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** 56 | * @} 57 | */ 58 | 59 | /* Exported macro ------------------------------------------------------------*/ 60 | /* Exported functions --------------------------------------------------------*/ 61 | 62 | void CRC_ResetDR(void); 63 | uint32_t CRC_CalcCRC(uint32_t Data); 64 | uint32_t CRC_CalcBlockCRC(uint32_t pBuffer[], uint32_t BufferLength); 65 | uint32_t CRC_GetCRC(void); 66 | void CRC_SetIDRegister(uint8_t IDValue); 67 | uint8_t CRC_GetIDRegister(void); 68 | 69 | #ifdef __cplusplus 70 | } 71 | #endif 72 | 73 | #endif /* __STM32F4xx_CRC_H */ 74 | 75 | /** 76 | * @} 77 | */ 78 | 79 | /** 80 | * @} 81 | */ 82 | 83 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 84 | -------------------------------------------------------------------------------- /STMLIB/inc/stm32f4xx_flash_ramfunc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_flash_ramfunc.h 4 | * @author MCD Application Team 5 | * @version V1.7.0 6 | * @date 22-April-2016 7 | * @brief Header file of FLASH RAMFUNC driver. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2016 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_FLASH_RAMFUNC_H 31 | #define __STM32F4xx_FLASH_RAMFUNC_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup FLASH RAMFUNC 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Private define ------------------------------------------------------------*/ 50 | /** 51 | * @brief __RAM_FUNC definition 52 | */ 53 | #if defined ( __CC_ARM ) 54 | /* ARM Compiler 55 | ------------ 56 | RAM functions are defined using the toolchain options. 57 | Functions that are executed in RAM should reside in a separate source module. 58 | Using the 'Options for File' dialog you can simply change the 'Code / Const' 59 | area of a module to a memory space in physical RAM. 60 | Available memory areas are declared in the 'Target' tab of the 'Options for Target' 61 | dialog. 62 | */ 63 | #define __RAM_FUNC void 64 | 65 | #elif defined ( __ICCARM__ ) 66 | /* ICCARM Compiler 67 | --------------- 68 | RAM functions are defined using a specific toolchain keyword "__ramfunc". 69 | */ 70 | #define __RAM_FUNC __ramfunc void 71 | 72 | #elif defined ( __GNUC__ ) 73 | /* GNU Compiler 74 | ------------ 75 | RAM functions are defined using a specific toolchain attribute 76 | "__attribute__((section(".RamFunc")))". 77 | */ 78 | #define __RAM_FUNC void __attribute__((section(".RamFunc"))) 79 | 80 | #endif 81 | /* Exported constants --------------------------------------------------------*/ 82 | /* Exported macro ------------------------------------------------------------*/ 83 | /* Exported functions --------------------------------------------------------*/ 84 | __RAM_FUNC FLASH_FlashInterfaceCmd(FunctionalState NewState); 85 | __RAM_FUNC FLASH_FlashSleepModeCmd(FunctionalState NewState); 86 | 87 | 88 | #ifdef __cplusplus 89 | } 90 | #endif 91 | 92 | #endif /* __STM32F4xx_FLASH_RAMFUNC_H */ 93 | 94 | /** 95 | * @} 96 | */ 97 | 98 | /** 99 | * @} 100 | */ 101 | 102 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 103 | 104 | -------------------------------------------------------------------------------- /STMLIB/inc/stm32f4xx_wwdg.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_wwdg.h 4 | * @author MCD Application Team 5 | * @version V1.7.0 6 | * @date 22-April-2016 7 | * @brief This file contains all the functions prototypes for the WWDG firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2016 STMicroelectronics

13 | * 14 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 15 | * You may not use this file except in compliance with the License. 16 | * You may obtain a copy of the License at: 17 | * 18 | * http://www.st.com/software_license_agreement_liberty_v2 19 | * 20 | * Unless required by applicable law or agreed to in writing, software 21 | * distributed under the License is distributed on an "AS IS" BASIS, 22 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 23 | * See the License for the specific language governing permissions and 24 | * limitations under the License. 25 | * 26 | ****************************************************************************** 27 | */ 28 | 29 | /* Define to prevent recursive inclusion -------------------------------------*/ 30 | #ifndef __STM32F4xx_WWDG_H 31 | #define __STM32F4xx_WWDG_H 32 | 33 | #ifdef __cplusplus 34 | extern "C" { 35 | #endif 36 | 37 | /* Includes ------------------------------------------------------------------*/ 38 | #include "stm32f4xx.h" 39 | 40 | /** @addtogroup STM32F4xx_StdPeriph_Driver 41 | * @{ 42 | */ 43 | 44 | /** @addtogroup WWDG 45 | * @{ 46 | */ 47 | 48 | /* Exported types ------------------------------------------------------------*/ 49 | /* Exported constants --------------------------------------------------------*/ 50 | 51 | /** @defgroup WWDG_Exported_Constants 52 | * @{ 53 | */ 54 | 55 | /** @defgroup WWDG_Prescaler 56 | * @{ 57 | */ 58 | 59 | #define WWDG_Prescaler_1 ((uint32_t)0x00000000) 60 | #define WWDG_Prescaler_2 ((uint32_t)0x00000080) 61 | #define WWDG_Prescaler_4 ((uint32_t)0x00000100) 62 | #define WWDG_Prescaler_8 ((uint32_t)0x00000180) 63 | #define IS_WWDG_PRESCALER(PRESCALER) (((PRESCALER) == WWDG_Prescaler_1) || \ 64 | ((PRESCALER) == WWDG_Prescaler_2) || \ 65 | ((PRESCALER) == WWDG_Prescaler_4) || \ 66 | ((PRESCALER) == WWDG_Prescaler_8)) 67 | #define IS_WWDG_WINDOW_VALUE(VALUE) ((VALUE) <= 0x7F) 68 | #define IS_WWDG_COUNTER(COUNTER) (((COUNTER) >= 0x40) && ((COUNTER) <= 0x7F)) 69 | 70 | /** 71 | * @} 72 | */ 73 | 74 | /** 75 | * @} 76 | */ 77 | 78 | /* Exported macro ------------------------------------------------------------*/ 79 | /* Exported functions --------------------------------------------------------*/ 80 | 81 | /* Function used to set the WWDG configuration to the default reset state ****/ 82 | void WWDG_DeInit(void); 83 | 84 | /* Prescaler, Refresh window and Counter configuration functions **************/ 85 | void WWDG_SetPrescaler(uint32_t WWDG_Prescaler); 86 | void WWDG_SetWindowValue(uint8_t WindowValue); 87 | void WWDG_EnableIT(void); 88 | void WWDG_SetCounter(uint8_t Counter); 89 | 90 | /* WWDG activation function ***************************************************/ 91 | void WWDG_Enable(uint8_t Counter); 92 | 93 | /* Interrupts and flags management functions **********************************/ 94 | FlagStatus WWDG_GetFlagStatus(void); 95 | void WWDG_ClearFlag(void); 96 | 97 | #ifdef __cplusplus 98 | } 99 | #endif 100 | 101 | #endif /* __STM32F4xx_WWDG_H */ 102 | 103 | /** 104 | * @} 105 | */ 106 | 107 | /** 108 | * @} 109 | */ 110 | 111 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 112 | -------------------------------------------------------------------------------- /STMLIB/src/stm32f4xx_qspi.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/STMLIB/src/stm32f4xx_qspi.c -------------------------------------------------------------------------------- /STMLIB/stm32f4xx.h: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/STMLIB/stm32f4xx.h -------------------------------------------------------------------------------- /STMLIB/stm32f4xx_it.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file Project/STM32F4xx_StdPeriph_Templates/stm32f4xx_it.h 4 | * @author MCD Application Team 5 | * @version V1.4.0 6 | * @date 04-August-2014 7 | * @brief This file contains the headers of the interrupt handlers. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2014 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /* Define to prevent recursive inclusion -------------------------------------*/ 29 | #ifndef __STM32F4xx_IT_H 30 | #define __STM32F4xx_IT_H 31 | 32 | #ifdef __cplusplus 33 | extern "C" { 34 | #endif 35 | 36 | /* Includes ------------------------------------------------------------------*/ 37 | #include "stm32f4xx.h" 38 | 39 | /* Exported types ------------------------------------------------------------*/ 40 | /* Exported constants --------------------------------------------------------*/ 41 | /* Exported macro ------------------------------------------------------------*/ 42 | /* Exported functions ------------------------------------------------------- */ 43 | 44 | void NMI_Handler(void); 45 | void HardFault_Handler(void); 46 | void MemManage_Handler(void); 47 | void BusFault_Handler(void); 48 | void UsageFault_Handler(void); 49 | void SVC_Handler(void); 50 | void DebugMon_Handler(void); 51 | void PendSV_Handler(void); 52 | void SysTick_Handler(void); 53 | 54 | #ifdef __cplusplus 55 | } 56 | #endif 57 | 58 | #endif /* __STM32F4xx_IT_H */ 59 | 60 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 61 | -------------------------------------------------------------------------------- /STMLIB/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @version V1.4.0 6 | * @date 04-August-2014 7 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | *

© COPYRIGHT 2014 STMicroelectronics

12 | * 13 | * Licensed under MCD-ST Liberty SW License Agreement V2, (the "License"); 14 | * You may not use this file except in compliance with the License. 15 | * You may obtain a copy of the License at: 16 | * 17 | * http://www.st.com/software_license_agreement_liberty_v2 18 | * 19 | * Unless required by applicable law or agreed to in writing, software 20 | * distributed under the License is distributed on an "AS IS" BASIS, 21 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 22 | * See the License for the specific language governing permissions and 23 | * limitations under the License. 24 | * 25 | ****************************************************************************** 26 | */ 27 | 28 | /** @addtogroup CMSIS 29 | * @{ 30 | */ 31 | 32 | /** @addtogroup stm32f4xx_system 33 | * @{ 34 | */ 35 | 36 | /** 37 | * @brief Define to prevent recursive inclusion 38 | */ 39 | #ifndef __SYSTEM_STM32F4XX_H 40 | #define __SYSTEM_STM32F4XX_H 41 | 42 | #ifdef __cplusplus 43 | extern "C" { 44 | #endif 45 | 46 | /** @addtogroup STM32F4xx_System_Includes 47 | * @{ 48 | */ 49 | 50 | /** 51 | * @} 52 | */ 53 | 54 | 55 | /** @addtogroup STM32F4xx_System_Exported_types 56 | * @{ 57 | */ 58 | 59 | extern uint32_t SystemCoreClock; /*!< System Clock Frequency (Core Clock) */ 60 | 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 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 106 | -------------------------------------------------------------------------------- /TOOLS/flightplot.jar: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/TOOLS/flightplot.jar -------------------------------------------------------------------------------- /TOOLS/天穹地面站V0.5.1.exe: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/TOOLS/天穹地面站V0.5.1.exe -------------------------------------------------------------------------------- /TOOLS/天穹地面站运行说明.txt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/TOOLS/天穹地面站运行说明.txt -------------------------------------------------------------------------------- /keilkilll.bat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kevincoooool/BlueSkyFlightControl/8626fceee7f12d9ae07561d702f3bad9eceeeb4a/keilkilll.bat -------------------------------------------------------------------------------- /项目开发记录.md: -------------------------------------------------------------------------------- 1 | # 项目开发记录 2 | 3 | # [v0.4.26] 2018.11.20 4 | - 速度卡尔曼滤波中增加GPS z轴速度及TOF速度(未使用)作为观测量 5 | 6 | # [v0.4.25] 2018.11.19 7 | - 将速度估计卡尔曼滤波器改成6阶,新增加速度bias作为状态量 8 | 9 | # [v0.4.17] 2018.10.31 10 | - 天穹地面站测试版发布 11 | 12 | # [v0.4.15] 2018.08.15 13 | - 重新整理和统一飞控中的坐标系。导航系使用ENU(东北天)坐标系,机体系定义为横滚往右为正,俯仰抬头为正,偏航顺时针旋转为正 14 | 15 | # [v0.4.14] 2018.08.10 16 | - 更改加速度计和罗盘校准算法为Levenberg-Marquardt法,提升算法的收敛性和鲁棒性 17 | 18 | # [v0.4.13] 2018.08.09 19 | - 增加飞行日志功能,使用ulog格式,解锁状态下自动记录飞行日志至TF卡,可以使用FlightPlot工具查看日志 20 | 21 | # [v0.4.5] 2018.07.26 22 | - 加入mavlink协议,并自动识别协议类型(bsklink或mavlink),同时增加了对QGC地面站的部分功能支持 23 | 24 | # [v0.4.1] 2018.07.23 25 | - 增加USB虚拟串口驱动 26 | 27 | # [v0.4.0] 2018.07.20 28 | - 对自动降落和自动返航功能进行了测试优化,修复了部分BUG,并简化了自动返航的控制逻辑 29 | 30 | # [v0.3.5] 2018.07.17 31 | - 优化自动模式下的速度控制逻辑,实现打杆匀加速与刹车匀减速的效果 32 | 33 | # [v0.3.3] 2018.07.10 34 | - GPS定点模式下的运动刹车效果优化完成 35 | 36 | # [v0.3.0] 2018.07.5 37 | - 修复GPS定点控制BUG,并进一步优化刹车控制效果,提升了精度与手感 38 | 39 | # [v0.3.0] 2018.07.4 40 | - 在卡尔曼融合中增加观测量相位差补偿,提升导航解算精度 41 | 42 | # [v0.1.3] 2018.06.26 43 | - 增加bsklink飞控通信协议,协议内容还待完善 44 | 45 | # [v0.1.1] 2018.06.21 46 | - 天穹V3主控板已经修改并打样完毕,准备焊接测试 47 | 48 | - 开始设计天穹地面站 49 | 50 | # [v0.1.1] 2018.06.14 51 | - 增加自动降落和自动返航功能(未测试) 52 | 53 | # [v0.1] 2018.06.10 54 | - 飞控电路设计有问题,电机开启后,传感器受到不明干扰使加速度z轴出现极大的零偏误差。最初怀疑是震动导致,经过两个星期的多次对比测试后,确定为电路干扰问题,目前还在查找原因。 55 | 56 | # [v0.1] 2018.06.07 57 | - 增加环境风速估计,以及高度卡尔曼的测量误差方差自适应(基于飞行状态与环境风速) 58 | 59 | - 修复加速度零偏误差计算的BUG 60 | 61 | - 增加加速度传感器的正反比例误差补偿 62 | 63 | 64 | # [v0.1] 2018.06.03 65 | - 完成GPS定点控制逻辑测试,已经能初步实现定点飞行(打杆控速度,回中悬停)。 66 | 67 | - 至此,所有基础飞行功能测试完毕,版本号升至0.1,接下来开始对飞控各部分细节、导航精度及飞行效果进行优化。 68 | 69 | - 目前速度、高度及位置估计算法还只进行了初步的实现,等更换新陀螺仪传感器后,再做各方面的优化。 70 | 71 | # [v0.0] 2018.06.02 72 | - 初步完成定高调试,高度控制精度为±3cm 73 | 74 | - 优化了姿态控制PID参数,将姿态控制精度提升至±0.1°(悬停) 75 | 76 | - 目前使用的MPU6500传感器,其加速度计的某些误差过大,且难以校准,因此准备更换成ICM20689 77 | 78 | - 接下来将会调试GPS定点功能 79 | 80 | # [v0.0] 2018.05.29 81 | - 已完成测试机的组装。 82 | 83 | - 初步完成了姿态控制的参数整定以及该控制环节的小问题修复。 84 | 85 | - 增加了USB HID驱动 86 | 87 | # [v0.0] 2018.05.27 88 | - 完成了飞控板第一版样板的焊接,硬件初步运行正常,正开始在该飞控板上调试程序。 89 | 90 | - 飞机组装遇到一些问题(电机孔位对不上),还需要一些工具才能完成工作。 91 | 92 | # [v0.0] 2018.05.22 93 | - 初步完成了飞控控制部分,包括姿态控制、高度控制和位置控制,并将控制算法与操控逻辑分开实现,降低程序的耦合度,方便后期开发。 94 | 95 | - 接着会增加一些基础的任务控制功能,如自动降落和自动返航等。 96 | 97 | - 目前飞控还需要完成的部分主要有遥控数据处理以及飞行状态检测(包括起飞和落地检测)。 98 | 99 | # [v0.0] 2018.05.21 100 | - 目前飞控软件部分还剩下控制部分未完成,主要在考虑如何处理好各个控制环节的耦合关系,方便后期飞控功能扩展及降低二次开发难度。 101 | 102 | - 飞控电路板(主控和IMU板)已发出打样,估计两三天内回来,接着便要开始焊板子、测试硬件了。 103 | 104 | - 接下来的两天内主要尽快把接收机信号解码、摇杆数据处理及基础控制部分(姿态、高度)写好,方便飞控的初步调试 105 | --------------------------------------------------------------------------------