├── .gitignore ├── .gitmodules ├── .travis.yml ├── CMakeLists.txt ├── Flow.sublime-project ├── Images ├── px4flow-v1.prototype └── px4flow-v2.prototype ├── Makefile ├── README.md ├── Tools ├── check_code_style.sh ├── check_submodules.sh ├── fix_code_style.sh ├── make_can_boot_descriptor.py ├── ocd │ ├── olimex-arm-usb-tiny-h.cfg │ ├── olimex-jtag-tiny.cfg │ └── stm32f4x.cfg ├── px_mkfw.py ├── px_process_params.py └── px_uploader.py ├── baremetal-configs ├── px4flow-v1 │ └── scripts │ │ └── ld.script └── px4flow-v2 │ └── scripts │ └── ld.script ├── cmake ├── Toolchain-arm-none-eabi.cmake └── test.sh ├── inc └── no_warnings.h ├── makefiles ├── README.txt ├── baremetal │ ├── baremetal.mk │ ├── baremetal_px4.mk │ ├── board_px4flow-v1.mk │ ├── board_px4flow-v2.mk │ ├── config_px4flow-v1_default.mk │ ├── config_px4flow-v2_default.mk │ ├── firmware_baremetal.mk │ ├── toolchain_gnu-arm-eabi.mk │ ├── uavcan_board_px4flow-v1.mk │ ├── uavcan_board_px4flow-v2.mk │ └── uavcanbl.mk ├── firmware.mk ├── library.mk ├── module.mk └── setup.mk ├── mavlink ├── include │ └── mavlink │ │ └── v1.0 │ │ ├── checksum.h │ │ ├── common │ │ ├── common.h │ │ ├── mavlink.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_change_operator_control.h │ │ ├── mavlink_msg_change_operator_control_ack.h │ │ ├── mavlink_msg_command_ack.h │ │ ├── mavlink_msg_command_int.h │ │ ├── mavlink_msg_command_long.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_file_transfer_protocol.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_raw_int.h │ │ ├── mavlink_msg_gps_rtk.h │ │ ├── mavlink_msg_gps_status.h │ │ ├── mavlink_msg_heartbeat.h │ │ ├── mavlink_msg_highres_imu.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_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_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_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_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_safety_allowed_area.h │ │ ├── mavlink_msg_safety_set_allowed_area.h │ │ ├── mavlink_msg_scaled_imu.h │ │ ├── mavlink_msg_scaled_imu2.h │ │ ├── mavlink_msg_scaled_pressure.h │ │ ├── mavlink_msg_serial_control.h │ │ ├── mavlink_msg_servo_output_raw.h │ │ ├── mavlink_msg_set_attitude_target.h │ │ ├── mavlink_msg_set_gps_global_origin.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_v2_extension.h │ │ ├── mavlink_msg_vfr_hud.h │ │ ├── mavlink_msg_vicon_position_estimate.h │ │ ├── mavlink_msg_vision_position_estimate.h │ │ ├── mavlink_msg_vision_speed_estimate.h │ │ ├── testsuite.h │ │ └── version.h │ │ ├── mavlink_conversions.h │ │ ├── mavlink_helpers.h │ │ ├── mavlink_protobuf_manager.hpp │ │ ├── mavlink_types.h │ │ └── protocol.h └── lib │ └── pkgconfig │ └── mavlink.pc ├── notebooks └── flow_analysis.ipynb ├── px4flow_bl.elf ├── src ├── drivers │ ├── boards │ │ ├── px4flow-v1 │ │ │ ├── board_config.h │ │ │ ├── module.mk │ │ │ └── px4flow_init.c │ │ └── px4flow-v2 │ │ │ ├── board_config.h │ │ │ ├── module.mk │ │ │ ├── px4flow_init.c │ │ │ └── px4flow_led.c │ └── bootloaders │ │ ├── include │ │ ├── boot_app_shared.h │ │ └── crc.h │ │ └── src │ │ ├── common │ │ └── boot_app_shared.c │ │ └── util │ │ └── crc.c ├── include │ ├── bsp │ │ ├── board.h │ │ └── probes.h │ ├── chip.h │ ├── communication.h │ ├── dcmi.h │ ├── debug.h │ ├── flow.h │ ├── gyro.h │ ├── hrt.h │ ├── i2c.h │ ├── i2c_frame.h │ ├── led.h │ ├── legacy_i2c.h │ ├── main.h │ ├── mavlink_bridge_header.h │ ├── mt9v034.h │ ├── no_warnings.h │ ├── settings.h │ ├── sonar.h │ ├── sonar_mode_filter.h │ ├── stm32f4xx_conf.h │ ├── stm32f4xx_it.h │ ├── uavcan_if.h │ ├── usart.h │ ├── usb_conf.h │ ├── usbd_cdc_vcp.h │ ├── usbd_conf.h │ ├── usbd_desc.h │ ├── utils.h │ └── visibility.h ├── lib │ └── stm32 │ │ └── st │ │ └── v1.0.2 │ │ ├── README │ │ ├── STM32F4xx_StdPeriph_Driver │ │ ├── Release_Notes.html │ │ ├── inc │ │ │ ├── misc.h │ │ │ ├── stm32f4xx_adc.h │ │ │ ├── stm32f4xx_can.h │ │ │ ├── stm32f4xx_crc.h │ │ │ ├── stm32f4xx_cryp.h │ │ │ ├── stm32f4xx_dac.h │ │ │ ├── stm32f4xx_dbgmcu.h │ │ │ ├── stm32f4xx_dcmi.h │ │ │ ├── stm32f4xx_dma.h │ │ │ ├── stm32f4xx_exti.h │ │ │ ├── stm32f4xx_flash.h │ │ │ ├── stm32f4xx_fsmc.h │ │ │ ├── stm32f4xx_gpio.h │ │ │ ├── stm32f4xx_hash.h │ │ │ ├── stm32f4xx_i2c.h │ │ │ ├── stm32f4xx_iwdg.h │ │ │ ├── stm32f4xx_pwr.h │ │ │ ├── stm32f4xx_rcc.h │ │ │ ├── stm32f4xx_rng.h │ │ │ ├── stm32f4xx_rtc.h │ │ │ ├── stm32f4xx_sdio.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_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_dma.c │ │ │ ├── stm32f4xx_exti.c │ │ │ ├── stm32f4xx_flash.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_pwr.c │ │ │ ├── stm32f4xx_rcc.c │ │ │ ├── stm32f4xx_rng.c │ │ │ ├── stm32f4xx_rtc.c │ │ │ ├── stm32f4xx_sdio.c │ │ │ ├── stm32f4xx_spi.c │ │ │ ├── stm32f4xx_syscfg.c │ │ │ ├── stm32f4xx_tim.c │ │ │ ├── stm32f4xx_usart.c │ │ │ └── stm32f4xx_wwdg.c │ │ ├── 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 │ │ ├── core_cm4.h │ │ ├── core_cm4_simd.h │ │ ├── core_cmFunc.h │ │ ├── core_cmInstr.h │ │ ├── startup_stm32f4xx.s │ │ ├── stm32f4xx.h │ │ └── system_stm32f4xx.h ├── modules │ ├── flow │ │ ├── communication.c │ │ ├── dcmi.c │ │ ├── debug.c │ │ ├── flow.c │ │ ├── gyro.c │ │ ├── i2c.c │ │ ├── led.c │ │ ├── main.c │ │ ├── module.mk │ │ ├── mt9v034.c │ │ ├── reset.c │ │ ├── settings.c │ │ ├── sonar.c │ │ ├── sonar_mode_filter.c │ │ ├── stm32f4xx_it.c │ │ ├── system_stm32f4xx.c │ │ ├── usart.c │ │ ├── usb_bsp.c │ │ ├── usbd_cdc_vcp.c │ │ ├── usbd_desc.c │ │ ├── usbd_usr.c │ │ └── utils.c │ ├── libc │ │ ├── .gitignore │ │ ├── Kconfig │ │ ├── README.txt │ │ ├── init.c │ │ ├── lib_internal.h │ │ ├── libc.csv │ │ ├── math.csv │ │ ├── module.mk │ │ ├── stdio │ │ │ ├── Make.defs │ │ │ ├── lib_asprintf.c │ │ │ ├── lib_avsprintf.c │ │ │ ├── lib_clearerr.c │ │ │ ├── lib_dprintf.c │ │ │ ├── lib_dtoa.c │ │ │ ├── lib_fclose.c │ │ │ ├── lib_feof.c │ │ │ ├── lib_ferror.c │ │ │ ├── lib_fflush.c │ │ │ ├── lib_fgetc.c │ │ │ ├── lib_fgetpos.c │ │ │ ├── lib_fgets.c │ │ │ ├── lib_fileno.c │ │ │ ├── lib_fopen.c │ │ │ ├── lib_fprintf.c │ │ │ ├── lib_fputc.c │ │ │ ├── lib_fputs.c │ │ │ ├── lib_fread.c │ │ │ ├── lib_fseek.c │ │ │ ├── lib_fsetpos.c │ │ │ ├── lib_ftell.c │ │ │ ├── lib_fwrite.c │ │ │ ├── lib_gets.c │ │ │ ├── lib_gets_s.c │ │ │ ├── lib_libdtoa.c │ │ │ ├── lib_libfflush.c │ │ │ ├── lib_libfgets.c │ │ │ ├── lib_libflushall.c │ │ │ ├── lib_libfread.c │ │ │ ├── lib_libfwrite.c │ │ │ ├── lib_libnoflush.c │ │ │ ├── lib_libsnoflush.c │ │ │ ├── lib_libsprintf.c │ │ │ ├── lib_libvsprintf.c │ │ │ ├── lib_lowinstream.c │ │ │ ├── lib_lowoutstream.c │ │ │ ├── lib_meminstream.c │ │ │ ├── lib_memoutstream.c │ │ │ ├── lib_memsistream.c │ │ │ ├── lib_memsostream.c │ │ │ ├── lib_nullinstream.c │ │ │ ├── lib_nulloutstream.c │ │ │ ├── lib_perror.c │ │ │ ├── lib_printf.c │ │ │ ├── lib_puts.c │ │ │ ├── lib_rawinstream.c │ │ │ ├── lib_rawoutstream.c │ │ │ ├── lib_rawsistream.c │ │ │ ├── lib_rawsostream.c │ │ │ ├── lib_rdflush.c │ │ │ ├── lib_snprintf.c │ │ │ ├── lib_sprintf.c │ │ │ ├── lib_sscanf.c │ │ │ ├── lib_stdinstream.c │ │ │ ├── lib_stdoutstream.c │ │ │ ├── lib_stdsistream.c │ │ │ ├── lib_stdsostream.c │ │ │ ├── lib_tempnam.c │ │ │ ├── lib_tmpnam.c │ │ │ ├── lib_ungetc.c │ │ │ ├── lib_vdprintf.c │ │ │ ├── lib_vfprintf.c │ │ │ ├── lib_vprintf.c │ │ │ ├── lib_vsnprintf.c │ │ │ ├── lib_vsprintf.c │ │ │ ├── lib_wrflush.c │ │ │ └── lib_zeroinstream.c │ │ ├── stdlib │ │ │ ├── Make.defs │ │ │ ├── lib_abort.c │ │ │ ├── lib_abs.c │ │ │ ├── lib_checkbase.c │ │ │ ├── lib_imaxabs.c │ │ │ ├── lib_itoa.c │ │ │ ├── lib_labs.c │ │ │ ├── lib_llabs.c │ │ │ ├── lib_mkstemp.c │ │ │ ├── lib_mktemp.c │ │ │ ├── lib_qsort.c │ │ │ ├── lib_rand.c │ │ │ ├── lib_strtod.c │ │ │ ├── lib_strtol.c │ │ │ ├── lib_strtoll.c │ │ │ ├── lib_strtoul.c │ │ │ └── lib_strtoull.c │ │ ├── string │ │ │ ├── Make.defs │ │ │ ├── lib_isbasedigit.c │ │ │ ├── lib_memccpy.c │ │ │ ├── lib_memchr.c │ │ │ ├── lib_memcmp.c │ │ │ ├── lib_memcpy.c │ │ │ ├── lib_memmove.c │ │ │ ├── lib_memset.c │ │ │ ├── lib_skipspace.c │ │ │ ├── lib_stpcpy.c │ │ │ ├── lib_strcasecmp.c │ │ │ ├── lib_strcasestr.c │ │ │ ├── lib_strcat.c │ │ │ ├── lib_strchr.c │ │ │ ├── lib_strcmp.c │ │ │ ├── lib_strcpy.c │ │ │ ├── lib_strcspn.c │ │ │ ├── lib_strdup.c │ │ │ ├── lib_strerror.c │ │ │ ├── lib_strlen.c │ │ │ ├── lib_strncasecmp.c │ │ │ ├── lib_strncat.c │ │ │ ├── lib_strncmp.c │ │ │ ├── lib_strncpy.c │ │ │ ├── lib_strndup.c │ │ │ ├── lib_strnlen.c │ │ │ ├── lib_strpbrk.c │ │ │ ├── lib_strrchr.c │ │ │ ├── lib_strspn.c │ │ │ ├── lib_strstr.c │ │ │ ├── lib_strtok.c │ │ │ ├── lib_strtokr.c │ │ │ └── lib_vikmemcpy.c │ │ ├── syslog │ │ │ ├── Make.defs │ │ │ ├── lib_lowsyslog.c │ │ │ ├── lib_setlogmask.c │ │ │ ├── lib_syslog.c │ │ │ ├── lib_syslogstream.c │ │ │ └── syslog.h │ │ └── time │ │ │ ├── Make.defs │ │ │ ├── lib_asctime.c │ │ │ ├── lib_asctimer.c │ │ │ ├── lib_calendar2utc.c │ │ │ ├── lib_ctime.c │ │ │ ├── lib_ctimer.c │ │ │ ├── lib_dayofweek.c │ │ │ ├── lib_daysbeforemonth.c │ │ │ ├── lib_gettimeofday.c │ │ │ ├── lib_gmtime.c │ │ │ ├── lib_gmtimer.c │ │ │ ├── lib_isleapyear.c │ │ │ ├── lib_localtime.c │ │ │ ├── lib_mktime.c │ │ │ ├── lib_settimeofday.c │ │ │ ├── lib_strftime.c │ │ │ └── lib_time.c │ ├── libcxx │ │ ├── libstubs.cpp │ │ └── module.mk │ └── uavcannode │ │ ├── dsdl │ │ └── uavcan │ │ │ └── threedr │ │ │ └── equipment │ │ │ └── flow │ │ │ └── optical_flow │ │ │ ├── 1110.LegacyRawSample.uavcan │ │ │ ├── I2CFrame.uavcan │ │ │ └── I2CFrameIntegral.uavcan │ │ ├── module.mk │ │ ├── uavcannode_main.cpp │ │ └── uavcannode_main.hpp └── platforms │ ├── common │ ├── module.mk │ └── px4_log.c │ ├── compiler.h │ ├── px4_config.h │ ├── px4_defines.h │ ├── px4_log.h │ └── px4_macros.h └── unittests ├── .gitignore ├── Makefile └── tests.c /.gitignore: -------------------------------------------------------------------------------- 1 | .DS_Store 2 | *.d 3 | *.o 4 | *.pyc 5 | *~ 6 | .*.swp 7 | .context 8 | .cproject 9 | .project 10 | .settings 11 | .gdbinit 12 | px4flow_prototype.px4 13 | px4flow*.px4 14 | px4flow*.bin 15 | *.sublime-workspace 16 | build*/ 17 | 18 | CMakeCache.txt 19 | CMakeFiles/ 20 | *.cmake 21 | libperiph.a 22 | px4flow 23 | Build/ 24 | Images/*.bin 25 | Images/*.px4 26 | .swp 27 | .~lock.* 28 | Archives/* 29 | Build/* 30 | build/* 31 | .tags 32 | tags 33 | .tags_sorted_by_file 34 | .pydevproject 35 | .ropeproject 36 | *.orig 37 | src/modules/uORB/topics/* 38 | src/platforms/nuttx/px4_messages/* 39 | src/platforms/ros/px4_messages/* 40 | Firmware.zip 41 | unittests/build 42 | *.generated.h 43 | .vagrant 44 | *.pretty 45 | xcode 46 | src/platforms/posix/px4_messages/ 47 | src/platforms/posix-arm/px4_messages/ 48 | src/platforms/qurt/px4_messages/ 49 | *.ipynb_checkpoints 50 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "src/lib/uavcan"] 2 | path = src/lib/uavcan 3 | url = https://github.com/UAVCAN/libuavcan.git 4 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | 3 | # use travis-ci docker based infrastructure 4 | sudo: false 5 | 6 | cache: 7 | directories: 8 | - $HOME/.ccache 9 | 10 | addons: 11 | apt: 12 | sources: 13 | - ubuntu-toolchain-r-test 14 | packages: 15 | - build-essential 16 | - ccache 17 | - cmake 18 | - genromfs 19 | - libc6-i386 20 | - libncurses5-dev 21 | - python-argparse 22 | - python-empy 23 | - python-serial 24 | - s3cmd 25 | - texinfo 26 | - zlib1g-dev 27 | 28 | before_script: 29 | # General toolchain dependencies 30 | - pushd . 31 | - cd ~ 32 | - wget https://launchpadlibrarian.net/186124160/gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 33 | - tar -jxf gcc-arm-none-eabi-4_8-2014q3-20140805-linux.tar.bz2 34 | - exportline="export PATH=$HOME/gcc-arm-none-eabi-4_8-2014q3/bin:\$PATH" 35 | - if grep -Fxq "$exportline" ~/.profile; then echo nothing to do ; else echo $exportline >> ~/.profile; fi 36 | - . ~/.profile 37 | - popd 38 | # setup ccache 39 | - mkdir -p ~/bin 40 | - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc 41 | - export PATH=~/bin:$PATH 42 | 43 | env: 44 | global: 45 | - secure: "IukUGn1p3sl0X0EqL51GOoxEPA3WQrOolvtUY5jnNyTJEb97RxWkneqJEsQu0Z5HAIrqMGb5AOQiXtn01NyVuU8yZCF/GuUeikIip1X1y9cyt1NIl7rndse3SZ/8CVj0g2MdsC5ViQu7sUVqdlQ9gzxMeJQfyKHdGz77V+pcItg=" 46 | - secure: "GcxEQk+mE/1rdiP6Uac7YVr8kj5ILG55tXGfX322ITt/T/6IgONMJvgeb7seQsxk+s/UMeKQZ3iN8jUMk1FWjLZufVOhXwh8+pvIfSU4Sf+Vvr80kxXCa45CkttN4Qfqh5oP0VBsnS0cUsWO+XjlI//Qj17K3dnwVvALUcn9lHg=" 47 | - PX4_AWS_BUCKET=px4-travis 48 | 49 | script: 50 | - ccache -z 51 | - arm-none-eabi-gcc --version 52 | - make archives 53 | - ccache -s 54 | - make 55 | - ccache -s 56 | - zip Flow.zip Images/*.px4 57 | 58 | 59 | after_script: 60 | - cp Images/px4flow-v1_default.px4 px4flow.px4 61 | - git clone git://github.com/PX4/CI-Tools.git 62 | - ./CI-Tools/s3cmd-configure 63 | # upload newest build for this branch with s3 index 64 | - ./CI-Tools/s3cmd-put px4flow.px4 CI-Tools/directory/index.html Flow/$TRAVIS_BRANCH/ 65 | # archive newest build by date with s3 index 66 | - ./CI-Tools/s3cmd-put Flow.zip archives/Flow/$TRAVIS_BRANCH/`date "+%Y-%m-%d"`-$TRAVIS_BUILD_ID/ 67 | - ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Flow/$TRAVIS_BRANCH/ 68 | 69 | notifications: 70 | webhooks: 71 | urls: 72 | - https://webhooks.gitter.im/e/2b9c4a4cb2211f8befba 73 | on_success: always # options: [always|never|change] default: always 74 | on_failure: always # options: [always|never|change] default: always 75 | -------------------------------------------------------------------------------- /Flow.sublime-project: -------------------------------------------------------------------------------- 1 | { 2 | "build_systems": 3 | [ 4 | { 5 | "cmd": 6 | [ 7 | "make" 8 | ], 9 | "file_regex": "^(..[^:]*):([0-9]+):?([0-9]+)?:? (.*)$", 10 | "name": "PX4", 11 | "working_dir": "${project_path}" 12 | } 13 | ], 14 | "folders": 15 | [ 16 | { 17 | "file_exclude_patterns": 18 | [ 19 | "*.o", 20 | "*.a", 21 | "*.d", 22 | "*.elf", 23 | ".built", 24 | ".context", 25 | ".depend", 26 | ".config", 27 | ".version", 28 | "Make.dep", 29 | ".configured", 30 | "*.sublime-project", 31 | "*.sublime-workspace", 32 | ".project", 33 | ".cproject", 34 | "cscope.out" 35 | ], 36 | "folder_exclude_patterns": 37 | [ 38 | ".settings" 39 | ], 40 | "follow_symlinks": true, 41 | "path": "." 42 | } 43 | ], 44 | "settings": 45 | { 46 | "highlight_line": true, 47 | "tab_size": 8, 48 | "translate_tabs_to_spaces": false 49 | } 50 | } 51 | -------------------------------------------------------------------------------- /Images/px4flow-v1.prototype: -------------------------------------------------------------------------------- 1 | { 2 | "board_id": 6, 3 | "magic": "FLOWv1", 4 | "description": "Firmware for the PX4FLowV1 board", 5 | "image": "", 6 | "build_time": 0, 7 | "summary": "PX4FLOWv1", 8 | "version": "0.1", 9 | "image_size": 0, 10 | "git_identity": "", 11 | "board_revision": 0 12 | } 13 | -------------------------------------------------------------------------------- /Images/px4flow-v2.prototype: -------------------------------------------------------------------------------- 1 | { 2 | "board_id": 6, 3 | "magic": "FLOWv1", 4 | "description": "Firmware for the PX4FLowV1 board", 5 | "image": "", 6 | "build_time": 0, 7 | "summary": "PX4FLOWv1", 8 | "version": "0.1", 9 | "image_size": 0, 10 | "git_identity": "", 11 | "board_revision": 0 12 | } 13 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## PX4Flow Firmware 2 | 3 | [![Build Status](https://travis-ci.org/PX4/Flow.svg?branch=master)](https://travis-ci.org/PX4/Flow) 4 | 5 | [![Gitter](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/PX4/Firmware?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) 6 | 7 | PX4 FLOW is a smart camera processing optical flow directly on the camera module. It is optimized for processing and outputs images only for development purposes. Its main output is a UART or I2C stream of flow measurements at ~400 Hz. 8 | 9 | Dev guide / toolchain installation: 10 | https://docs.px4.io/master/en/sensor/px4flow.html 11 | 12 | For help, run: 13 | 14 | ```bash 15 | make help 16 | ``` 17 | 18 | To build, run: 19 | ```bash 20 | make archives # this needs to be done only once 21 | make 22 | ``` 23 | 24 | To flash via the PX4 bootloader (first run this command, then connect the board): 25 | ```bash 26 | make upload-usb 27 | ``` 28 | 29 | By default the px4flow-v1_default is uploaded; to upload a different version, run: 30 | 31 | ```bash 32 | make upload-usb 33 | ``` 34 | Where `` is one of the px4flow targets listed by `make help`. 35 | -------------------------------------------------------------------------------- /Tools/check_code_style.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | set -eu 3 | failed=0 4 | for fn in $(find . -path './src/lib/uavcan' -prune -o \ 5 | -path './src/lib/mathlib/CMSIS' -prune -o \ 6 | -path './src/lib/eigen' -prune -o \ 7 | -path './src/modules/attitude_estimator_ekf/codegen' -prune -o \ 8 | -path './NuttX' -prune -o \ 9 | -path './Build' -prune -o \ 10 | -path './mavlink' -prune -o \ 11 | -path './unittests/gtest' -prune -o \ 12 | -path './unittests/build' -prune -o \ 13 | -name '*.c' -o -name '*.cpp' -o -name '*.hpp' -o -name '*.h' -type f); do 14 | if [ -f "$fn" ]; 15 | then 16 | ./Tools/fix_code_style.sh --quiet < $fn > $fn.pretty 17 | diffsize=$(diff -y --suppress-common-lines $fn $fn.pretty | wc -l) 18 | rm -f $fn.pretty 19 | if [ $diffsize -ne 0 ]; then 20 | failed=1 21 | echo $diffsize $fn 22 | fi 23 | fi 24 | done 25 | 26 | if [ $failed -eq 0 ]; then 27 | echo "Format checks passed" 28 | exit 0 29 | else 30 | echo "Format checks failed; please run ./Tools/fix_code_style.sh on each file" 31 | exit 1 32 | fi 33 | -------------------------------------------------------------------------------- /Tools/fix_code_style.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | astyle \ 3 | --style=linux \ 4 | --indent=force-tab=8 \ 5 | --indent-cases \ 6 | --indent-preprocessor \ 7 | --break-blocks=all \ 8 | --pad-oper \ 9 | --pad-header \ 10 | --unpad-paren \ 11 | --keep-one-line-blocks \ 12 | --keep-one-line-statements \ 13 | --align-pointer=name \ 14 | --align-reference=name \ 15 | --suffix=none \ 16 | --ignore-exclude-errors-x \ 17 | --lineend=linux \ 18 | --exclude=EASTL \ 19 | $* 20 | -------------------------------------------------------------------------------- /Tools/ocd/olimex-arm-usb-tiny-h.cfg: -------------------------------------------------------------------------------- 1 | # 2 | # Olimex ARM-USB-TINY-H 3 | # 4 | # http://www.olimex.com/dev/arm-usb-tiny-h.html 5 | # 6 | 7 | interface ft2232 8 | ft2232_device_desc "Olimex OpenOCD JTAG ARM-USB-TINY-H" 9 | ft2232_layout olimex-jtag 10 | ft2232_vid_pid 0x15ba 0x002a 11 | 12 | -------------------------------------------------------------------------------- /Tools/ocd/olimex-jtag-tiny.cfg: -------------------------------------------------------------------------------- 1 | # 2 | # Olimex ARM-USB-TINY 3 | # 4 | # http://www.olimex.com/dev/arm-usb-tiny.html 5 | # 6 | 7 | interface ft2232 8 | ft2232_device_desc "Olimex OpenOCD JTAG TINY" 9 | ft2232_layout olimex-jtag 10 | ft2232_vid_pid 0x15ba 0x0004 11 | 12 | -------------------------------------------------------------------------------- /Tools/ocd/stm32f4x.cfg: -------------------------------------------------------------------------------- 1 | # script for stm32f2xxx 2 | 3 | if { [info exists CHIPNAME] } { 4 | set _CHIPNAME $CHIPNAME 5 | } else { 6 | set _CHIPNAME stm32f4xxx 7 | } 8 | 9 | if { [info exists ENDIAN] } { 10 | set _ENDIAN $ENDIAN 11 | } else { 12 | set _ENDIAN little 13 | } 14 | 15 | # Work-area is a space in RAM used for flash programming 16 | # By default use 64kB 17 | if { [info exists WORKAREASIZE] } { 18 | set _WORKAREASIZE $WORKAREASIZE 19 | } else { 20 | set _WORKAREASIZE 0x10000 21 | } 22 | 23 | # JTAG speed should be <= F_CPU/6. F_CPU after reset is 8MHz, so use F_JTAG = 1MHz 24 | # 25 | # Since we may be running of an RC oscilator, we crank down the speed a 26 | # bit more to be on the safe side. Perhaps superstition, but if are 27 | # running off a crystal, we can run closer to the limit. Note 28 | # that there can be a pretty wide band where things are more or less stable. 29 | jtag_khz 1000 30 | 31 | jtag_nsrst_delay 100 32 | jtag_ntrst_delay 100 33 | 34 | #jtag scan chain 35 | if { [info exists CPUTAPID ] } { 36 | set _CPUTAPID $CPUTAPID 37 | } else { 38 | # See STM Document RM0033 39 | # Section 32.6.3 - corresponds to Cortex-M3 r2p0 40 | set _CPUTAPID 0x4ba00477 41 | } 42 | jtag newtap $_CHIPNAME cpu -irlen 4 -ircapture 0x1 -irmask 0xf -expected-id $_CPUTAPID 43 | 44 | if { [info exists BSTAPID ] } { 45 | set _BSTAPID $BSTAPID 46 | } else { 47 | # See STM Document RM0033 48 | # Section 32.6.2 49 | # 50 | set _BSTAPID 0x06413041 51 | } 52 | jtag newtap $_CHIPNAME bs -irlen 5 -expected-id $_BSTAPID 53 | 54 | set _TARGETNAME $_CHIPNAME.cpu 55 | target create $_TARGETNAME cortex_m3 -endian $_ENDIAN -chain-position $_TARGETNAME -rtos auto 56 | 57 | $_TARGETNAME configure -work-area-phys 0x20000000 -work-area-size $_WORKAREASIZE -work-area-backup 0 58 | 59 | set _FLASHNAME $_CHIPNAME.flash 60 | flash bank $_FLASHNAME stm32f2x 0 0 0 0 $_TARGETNAME 61 | 62 | # if srst is not fitted use SYSRESETREQ to 63 | # perform a soft reset 64 | cortex_m3 reset_config sysresetreq 65 | 66 | -------------------------------------------------------------------------------- /cmake/Toolchain-arm-none-eabi.cmake: -------------------------------------------------------------------------------- 1 | include(CMakeForceCompiler) 2 | 3 | # this one is important 4 | set(CMAKE_SYSTEM_NAME Arm) 5 | 6 | #this one not so much 7 | set(CMAKE_SYSTEM_VERSION 1) 8 | 9 | # specify the cross compiler 10 | find_program(C_COMPILER arm-none-eabi-gcc) 11 | cmake_force_c_compiler(${C_COMPILER} GNU) 12 | find_program(CXX_COMPILER arm-none-eabi-g++) 13 | cmake_force_cxx_compiler(${CXX_COMPILER} GNU) 14 | find_program(OBJCOPY arm-none-eabi-objcopy) 15 | 16 | set(LINKER_FLAGS "-Wl,-gc-sections") 17 | set(CMAKE_EXE_LINKER_FLAGS ${LINKER_FLAGS}) 18 | 19 | # where is the target environment 20 | set(CMAKE_FIND_ROOT_PATH get_file_component(${C_COMPILER} PATH)) 21 | 22 | # search for programs in the build host directories 23 | set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER) 24 | # for libraries and headers in the target directories 25 | set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY) 26 | set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY) 27 | -------------------------------------------------------------------------------- /cmake/test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | d=$PWD && \ 3 | mkdir -p $d/build_arm && cd $d/build_arm && cmake .. && make && ctest && cpack -G ZIP && \ 4 | mkdir -p $d/build_host && cd $d/build_host && cmake .. -DHOST_TEST=ON && make && ctest 5 | -------------------------------------------------------------------------------- /inc/no_warnings.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: David Sidrane 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | #ifndef NO_WARNINGS_H_ 35 | #define NO_WARNINGS_H_ 36 | 37 | #include 38 | 39 | inline bool FLOAT_AS_BOOL(float f); 40 | inline int FLOAT_EQ_INT(float f , int i); 41 | inline int FLOAT_EQ_FLOAT(float f1 , float f2); 42 | 43 | inline bool FLOAT_AS_BOOL(float f) 44 | { 45 | #pragma GCC diagnostic push 46 | #pragma GCC diagnostic ignored "-Wfloat-equal" 47 | return (f != 0.0f); 48 | #pragma GCC diagnostic pop 49 | } 50 | 51 | inline int FLOAT_EQ_INT(float f , int i) 52 | { 53 | #pragma GCC diagnostic push 54 | #pragma GCC diagnostic ignored "-Wfloat-equal" 55 | return (f == i); 56 | #pragma GCC diagnostic pop 57 | } 58 | 59 | inline int FLOAT_EQ_FLOAT(float f1 , float f2) 60 | { 61 | #pragma GCC diagnostic push 62 | #pragma GCC diagnostic ignored "-Wfloat-equal" 63 | return (f1 == f2); 64 | #pragma GCC diagnostic pop 65 | } 66 | 67 | #endif /* NO_WARNINGS_H_ */ 68 | -------------------------------------------------------------------------------- /makefiles/README.txt: -------------------------------------------------------------------------------- 1 | PX4 Build System 2 | ================ 3 | 4 | The files in this directory implement the PX4 runtime firmware build system 5 | and configuration for the standard PX4 boards and software, in conjunction 6 | with Makefile in the parent directory. 7 | 8 | ../Makefile 9 | 10 | Top-level makefile for the PX4 build system. 11 | This makefile supports building NuttX archives for the NuttX based 12 | configurations, as well as supervising the building of all 13 | of the defined PX4 firmware configurations. 14 | 15 | Try 'make help' in the parent directory for documentation. 16 | 17 | firmware.mk 18 | 19 | Manages the build for one specific firmware configuration. 20 | See the comments at the top of this file for detailed documentation. 21 | 22 | Builds modules, builtin command lists and the ROMFS (if configured). 23 | 24 | This is the makefile directly used by external build systems; it can 25 | be configured to compile modules both inside and outside the PX4 26 | source tree. When used in this mode, at least BOARD, MODULES and 27 | CONFIG_FILE must be set. 28 | 29 | firmware_baremetal.mk 30 | 31 | Called by firmware.mk to build CMSIS/SPL baremetal based firmware. 32 | 33 | module.mk 34 | 35 | Called by firmware.mk to build individual modules. 36 | See the comments at the top of this file for detailed documentation. 37 | 38 | Not normally used other than by firmware.mk. 39 | 40 | baremetal.mk 41 | 42 | Called by ../Makefile to set BAREMETAL specific parameters if 43 | PX4_TARGET_OS is set to "baremetal". 44 | 45 | upload.mk 46 | 47 | Called by ../Makefile to upload files to a target board. Can be used 48 | by external build systems as well. (NuttX targets only) 49 | 50 | setup.mk 51 | 52 | Provides common path and tool definitions. Implements host 53 | system-specific compatibility hacks. Sets PX4_TARGET_OS. 54 | 55 | board_.mk 56 | 57 | Board-specific configuration for . Typically sets 58 | CONFIG_ARCH and then includes the toolchain definition for the board. 59 | 60 | config__.mk 61 | 62 | Parameters for a specific configuration on a specific board. 63 | The board name is derived from the filename. Sets MODULES to select 64 | source modules to be included in the configuration, may also set 65 | ROMFS_ROOT to build a ROMFS and BUILTIN_COMMANDS to include non-module 66 | commands (e.g. from NuttX) 67 | 68 | toolchain_.mk 69 | 70 | Provides macros used to compile and link source files. 71 | Accepts EXTRADEFINES to add additional pre-processor symbol definitions, 72 | EXTRACFLAGS, EXTRACXXFLAGS, EXTRAAFLAGS and EXTRALDFLAGS to pass 73 | additional flags to the C compiler, C++ compiler, assembler and linker 74 | respectively. 75 | 76 | Defines the COMPILE, COMPILEXX, ASSEMBLE, PRELINK, ARCHIVE and LINK 77 | macros that are used elsewhere in the build system. 78 | -------------------------------------------------------------------------------- /makefiles/baremetal/board_px4flow-v1.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Board-specific definitions for the PX4FLOW 3 | # 4 | 5 | # 6 | # Configure the toolchain 7 | # 8 | 9 | CONFIG_ARCH = CORTEXM4F 10 | CONFIG_BOARD = PX4FLOW_V1 11 | 12 | WUSEPACKED = -Wno-packed 13 | include $(PX4_MK_DIR)baremetal/toolchain_gnu-arm-eabi.mk 14 | 15 | -------------------------------------------------------------------------------- /makefiles/baremetal/board_px4flow-v2.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Board-specific definitions for the PX4FLOW 3 | # 4 | 5 | # 6 | # Configure the toolchain 7 | # 8 | 9 | CONFIG_ARCH = CORTEXM4F 10 | CONFIG_BOARD = PX4FLOW_V2 11 | 12 | WUSEPACKED = -Wno-packed 13 | include $(PX4_MK_DIR)baremetal/toolchain_gnu-arm-eabi.mk 14 | 15 | # 16 | # Bring in common uavcan hardware version definitions 17 | # 18 | include $(PX4_MK_DIR)baremetal/uavcan_board_px4flow-v2.mk 19 | 20 | -------------------------------------------------------------------------------- /makefiles/baremetal/config_px4flow-v1_default.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Makefile for the px4flow_default configuration 3 | # 4 | 5 | # Define the STMicroelectronics version to use 6 | 7 | ST_LIB_VERSION=v1.0.2 8 | 9 | INCLUDE_DIRS += $(PX4_BOOTLOADER_BASE)include \ 10 | $(ST_LIB_DIR)$(ST_LIB_VERSION) \ 11 | $(ST_LIB_DIR)$(ST_LIB_VERSION)/STM32F4xx_StdPeriph_Driver/inc \ 12 | 13 | # 14 | # UAVCAN boot loadable Module ID 15 | # 16 | 17 | export UAVCANBLID_SW_VERSION_MAJOR=0 18 | export UAVCANBLID_SW_VERSION_MINOR=1 19 | 20 | # 21 | # Bring in common uavcan hardware version definitions 22 | # 23 | include $(PX4_MK_DIR)baremetal/uavcan_board_px4flow-v1.mk 24 | 25 | # 26 | # Board support modules 27 | # 28 | MODULES += drivers/boards/px4flow-v1 29 | 30 | # 31 | # General system control 32 | # 33 | ##TODO EXPECT I2C - MODULES += modules/uavcannode 34 | 35 | # C support 36 | MODULES += modules/libc 37 | 38 | # Flow 39 | MODULES += modules/flow 40 | 41 | 42 | 43 | # Generate parameter XML file 44 | #GEN_PARAM_XML = 1 45 | 46 | -------------------------------------------------------------------------------- /makefiles/baremetal/config_px4flow-v2_default.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Makefile for the px4flow_default configuration 3 | # 4 | 5 | # Define the STMicroelectronics version to use 6 | 7 | ST_LIB_VERSION=v1.0.2 8 | 9 | INCLUDE_DIRS += $(PX4_BOOTLOADER_BASE)include \ 10 | $(ST_LIB_DIR)$(ST_LIB_VERSION) \ 11 | $(ST_LIB_DIR)$(ST_LIB_VERSION)/STM32F4xx_StdPeriph_Driver/inc \ 12 | 13 | # 14 | # UAVCAN boot loadable Module ID 15 | # 16 | 17 | export UAVCANBLID_SW_VERSION_MAJOR=0 18 | export UAVCANBLID_SW_VERSION_MINOR=1 19 | 20 | # 21 | # Bring in common uavcan hardware version definitions 22 | # 23 | include $(PX4_MK_DIR)baremetal/uavcan_board_px4flow-v2.mk 24 | 25 | # 26 | # Board support modules 27 | # 28 | MODULES += drivers/boards/px4flow-v2 29 | 30 | # 31 | # General system control 32 | # 33 | MODULES += modules/uavcannode 34 | 35 | # C++ support 36 | MODULES += modules/libcxx 37 | 38 | # C support 39 | MODULES += modules/libc 40 | 41 | # Flow 42 | MODULES += modules/flow 43 | 44 | 45 | 46 | # Generate parameter XML file 47 | #GEN_PARAM_XML = 1 48 | 49 | # 50 | # Make this UAVCAN boot loadable 51 | # 52 | MAKE_UAVCAN_BOOT_LOADABLE_ID=$(call MKUAVCANBLNAME,$(subst $\",,$(UAVCANBLID_NAME))) 53 | 54 | 55 | -------------------------------------------------------------------------------- /makefiles/baremetal/uavcan_board_px4flow-v1.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Uavcan hardware version definitions for the px4flow 3 | # 4 | 5 | export UAVCANBLID_HW_VERSION_MAJOR=1 6 | export UAVCANBLID_HW_VERSION_MINOR=3 7 | export UAVCANBLID_NAME= "\"org.pixhawk.px4flow-v1\"" 8 | -------------------------------------------------------------------------------- /makefiles/baremetal/uavcan_board_px4flow-v2.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Uavcan hardware version definitions for the px4flow 3 | # 4 | 5 | export UAVCANBLID_HW_VERSION_MAJOR=2 6 | export UAVCANBLID_HW_VERSION_MINOR=0 7 | export UAVCANBLID_NAME= "\"org.pixhawk.px4flow-v2\"" 8 | -------------------------------------------------------------------------------- /makefiles/baremetal/uavcanbl.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2015 PX4 Development Team. All rights reserved. 3 | # 4 | # Redistribution and use in source and binary forms, with or without 5 | # modification, are permitted provided that the following conditions 6 | # are met: 7 | # 8 | # 1. Redistributions of source code must retain the above copyright 9 | # notice, this list of conditions and the following disclaimer. 10 | # 2. Redistributions in binary form must reproduce the above copyright 11 | # notice, this list of conditions and the following disclaimer in 12 | # the documentation and/or other materials provided with the 13 | # distribution. 14 | # 3. Neither the name PX4 nor the names of its contributors may be 15 | # used to endorse or promote products derived from this software 16 | # without specific prior written permission. 17 | # 18 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 22 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 25 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 26 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 27 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 28 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | # POSSIBILITY OF SUCH DAMAGE. 30 | # 31 | 32 | # 33 | # The extention uesd on uavcan bootloadable binary images 34 | # 35 | UAVCAN_BL_EXT=uavcan.bin 36 | 37 | # 38 | # The tool to embed the uavcan bootloader application descriptor 39 | # 40 | MKUAVCANBL = $(PX4_TOOLS_DIR)make_can_boot_descriptor.py 41 | 42 | # 43 | # Get a short version string provided by git 44 | # This assumes that git command is available and that 45 | # the directory holding this file also contains .git directory 46 | # 47 | MKUAVCANBL_GIT_DESC := $(shell git rev-list HEAD --max-count=1 --abbrev=8 --abbrev-commit) 48 | ifneq ($(words $(MKUAVCANBL_GIT_DESC)),1) 49 | MKUAVCANBL_GIT_DESC := ffffffff 50 | endif 51 | export MKUAVCANBL_GIT_DESC 52 | 53 | 54 | MKUAVCANBLNAME=$1-$(UAVCANBLID_HW_VERSION_MAJOR).$(UAVCANBLID_HW_VERSION_MINOR)-$(UAVCANBLID_SW_VERSION_MAJOR).$(UAVCANBLID_SW_VERSION_MINOR).$(MKUAVCANBL_GIT_DESC) 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/checksum.h: -------------------------------------------------------------------------------- 1 | #ifdef __cplusplus 2 | extern "C" { 3 | #endif 4 | 5 | #ifndef _CHECKSUM_H_ 6 | #define _CHECKSUM_H_ 7 | 8 | 9 | /** 10 | * 11 | * CALCULATE THE CHECKSUM 12 | * 13 | */ 14 | 15 | #define X25_INIT_CRC 0xffff 16 | #define X25_VALIDATE_CRC 0xf0b8 17 | 18 | #ifndef HAVE_CRC_ACCUMULATE 19 | /** 20 | * @brief Accumulate the X.25 CRC by adding one char at a time. 21 | * 22 | * The checksum function adds the hash of one char at a time to the 23 | * 16 bit checksum (uint16_t). 24 | * 25 | * @param data new char to hash 26 | * @param crcAccum the already accumulated checksum 27 | **/ 28 | static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) 29 | { 30 | /*Accumulate one byte of data into the CRC*/ 31 | uint8_t tmp; 32 | 33 | tmp = data ^ (uint8_t)(*crcAccum &0xff); 34 | tmp ^= (tmp<<4); 35 | *crcAccum = (*crcAccum>>8) ^ (tmp<<8) ^ (tmp <<3) ^ (tmp>>4); 36 | } 37 | #endif 38 | 39 | 40 | /** 41 | * @brief Initiliaze the buffer for the X.25 CRC 42 | * 43 | * @param crcAccum the 16 bit X.25 CRC 44 | */ 45 | static inline void crc_init(uint16_t* crcAccum) 46 | { 47 | *crcAccum = X25_INIT_CRC; 48 | } 49 | 50 | 51 | /** 52 | * @brief Calculates the X.25 checksum on a byte buffer 53 | * 54 | * @param pBuffer buffer containing the byte array to hash 55 | * @param length length of the byte array 56 | * @return the checksum over the buffer bytes 57 | **/ 58 | static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) 59 | { 60 | uint16_t crcTmp; 61 | crc_init(&crcTmp); 62 | while (length--) { 63 | crc_accumulate(*pBuffer++, &crcTmp); 64 | } 65 | return crcTmp; 66 | } 67 | 68 | 69 | /** 70 | * @brief Accumulate the X.25 CRC by adding an array of bytes 71 | * 72 | * The checksum function adds the hash of one char at a time to the 73 | * 16 bit checksum (uint16_t). 74 | * 75 | * @param data new bytes to hash 76 | * @param crcAccum the already accumulated checksum 77 | **/ 78 | static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer, uint16_t length) 79 | { 80 | const uint8_t *p = (const uint8_t *)pBuffer; 81 | while (length--) { 82 | crc_accumulate(*p++, crcAccum); 83 | } 84 | } 85 | 86 | #endif /* _CHECKSUM_H_ */ 87 | 88 | #ifdef __cplusplus 89 | } 90 | #endif 91 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/mavlink.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #ifndef MAVLINK_H 6 | #define MAVLINK_H 7 | 8 | #ifndef MAVLINK_STX 9 | #define MAVLINK_STX 254 10 | #endif 11 | 12 | #ifndef MAVLINK_ENDIAN 13 | #define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN 14 | #endif 15 | 16 | #ifndef MAVLINK_ALIGNED_FIELDS 17 | #define MAVLINK_ALIGNED_FIELDS 1 18 | #endif 19 | 20 | #ifndef MAVLINK_CRC_EXTRA 21 | #define MAVLINK_CRC_EXTRA 1 22 | #endif 23 | 24 | #include "version.h" 25 | #include "common.h" 26 | 27 | #endif // MAVLINK_H 28 | -------------------------------------------------------------------------------- /mavlink/include/mavlink/v1.0/common/version.h: -------------------------------------------------------------------------------- 1 | /** @file 2 | * @brief MAVLink comm protocol built from common.xml 3 | * @see http://mavlink.org 4 | */ 5 | #ifndef MAVLINK_VERSION_H 6 | #define MAVLINK_VERSION_H 7 | 8 | #define MAVLINK_BUILD_DATE "Thu Oct 30 15:09:14 2014" 9 | #define MAVLINK_WIRE_PROTOCOL_VERSION "1.0" 10 | #define MAVLINK_MAX_DIALECT_PAYLOAD_SIZE 255 11 | 12 | #endif // MAVLINK_VERSION_H 13 | -------------------------------------------------------------------------------- /mavlink/lib/pkgconfig/mavlink.pc: -------------------------------------------------------------------------------- 1 | prefix=/ 2 | exec_prefix=/ 3 | 4 | Name: mavlink 5 | Description: 6 | Version: 7 | Cflags: -I//include/mavlink 8 | -------------------------------------------------------------------------------- /notebooks/flow_analysis.ipynb: -------------------------------------------------------------------------------- 1 | { 2 | "cells": [ 3 | { 4 | "cell_type": "code", 5 | "execution_count": 1, 6 | "metadata": { 7 | "collapsed": false 8 | }, 9 | "outputs": [ 10 | { 11 | "name": "stdout", 12 | "output_type": "stream", 13 | "text": [ 14 | "Using matplotlib backend: Qt5Agg\n", 15 | "Populating the interactive namespace from numpy and matplotlib\n" 16 | ] 17 | } 18 | ], 19 | "source": [ 20 | "%pylab" 21 | ] 22 | }, 23 | { 24 | "cell_type": "code", 25 | "execution_count": 20, 26 | "metadata": { 27 | "collapsed": false 28 | }, 29 | "outputs": [ 30 | { 31 | "data": { 32 | "text/plain": [ 33 | "183.23012438744959" 34 | ] 35 | }, 36 | "execution_count": 20, 37 | "metadata": {}, 38 | "output_type": "execute_result" 39 | } 40 | ], 41 | "source": [ 42 | "s = 64 # image size\n", 43 | "win = 4 # search area, pixels\n", 44 | "vel = 1 # velocity\n", 45 | "d = 1 # distance above ground\n", 46 | "rate = 100 # hz\n", 47 | "\n", 48 | "fov = np.deg2rad(5)\n", 49 | "f = s/(2*(tan(fov/2))) # focal length, pixels\n", 50 | "x = win * d/f\n", 51 | "rate = vel/x #rate, hz\n", 52 | "rate" 53 | ] 54 | } 55 | ], 56 | "metadata": { 57 | "anaconda-cloud": {}, 58 | "kernelspec": { 59 | "display_name": "Python [default]", 60 | "language": "python", 61 | "name": "python3" 62 | }, 63 | "language_info": { 64 | "codemirror_mode": { 65 | "name": "ipython", 66 | "version": 3 67 | }, 68 | "file_extension": ".py", 69 | "mimetype": "text/x-python", 70 | "name": "python", 71 | "nbconvert_exporter": "python", 72 | "pygments_lexer": "ipython3", 73 | "version": "3.5.2" 74 | } 75 | }, 76 | "nbformat": 4, 77 | "nbformat_minor": 1 78 | } 79 | -------------------------------------------------------------------------------- /px4flow_bl.elf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/PX4-Flow/6c8cdb3cc04a5854a7b50e76e267e35507a0a0c4/px4flow_bl.elf -------------------------------------------------------------------------------- /src/drivers/boards/px4flow-v1/module.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Board-specific startup code for the PX4FLOW 3 | # 4 | 5 | DEFAULT_VISIBILITY=default 6 | 7 | ABS_BOOTLOADER_SRC := $(PX4_BOOTLOADER_BASE)src/ 8 | 9 | SRCS = \ 10 | px4flow_init.c \ 11 | 12 | BOOTLOADER_SRC = \ 13 | $(ABS_BOOTLOADER_SRC)common/boot_app_shared.c \ 14 | $(ABS_BOOTLOADER_SRC)util/crc.c 15 | 16 | # Use the relitive path to keep the genrated files in the BUILD_DIR 17 | 18 | SRCS += $(subst $(PX4_MODULE_SRC),../../../,$(BOOTLOADER_SRC)) 19 | 20 | 21 | MAXOPTIMIZATION = -Os 22 | -------------------------------------------------------------------------------- /src/drivers/boards/px4flow-v2/module.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Board-specific startup code for the PX4FLOW 3 | # 4 | 5 | DEFAULT_VISIBILITY=default 6 | 7 | ABS_BOOTLOADER_SRC := $(PX4_BOOTLOADER_BASE)src/ 8 | 9 | SRCS = \ 10 | px4flow_init.c \ 11 | px4flow_led.c \ 12 | 13 | BOOTLOADER_SRC = \ 14 | $(ABS_BOOTLOADER_SRC)common/boot_app_shared.c \ 15 | $(ABS_BOOTLOADER_SRC)util/crc.c 16 | 17 | # Use the relitive path to keep the genrated files in the BUILD_DIR 18 | 19 | SRCS += $(subst $(PX4_MODULE_SRC),../../../,$(BOOTLOADER_SRC)) 20 | 21 | 22 | MAXOPTIMIZATION = -Os 23 | -------------------------------------------------------------------------------- /src/include/chip.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include "stm32f4xx_conf.h" 39 | #include "stm32f4xx.h" 40 | 41 | TODO(Need to inject board.h or board_config.h via symlink); 42 | 43 | #define STM32_PCLK1_FREQUENCY (168000000ul/4) 44 | #define STM32_TIMCLK1 (2*STM32_PCLK1_FREQUENCY) 45 | 46 | TODO(STM32_SYSMEM_FSIZE should be in the SPL); 47 | #define STM32_SYSMEM_UID 0x1fff7a10 /* The 96-bit unique device identifier */ 48 | 49 | -------------------------------------------------------------------------------- /src/include/communication.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * Author: Samuel Zihlmann 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #ifndef COMMUNICATION_H_ 36 | #define COMMUNICATION_H_ 37 | 38 | /* Init */ 39 | void communication_init(void); 40 | 41 | /* Send */ 42 | void communication_system_state_send(void); 43 | void communication_parameter_send(void); 44 | 45 | /* Receive */ 46 | void communication_receive(void); 47 | void communication_receive_forward(void); 48 | void communication_receive_usb(void); 49 | void handle_mavlink_message(mavlink_channel_t chan, mavlink_message_t* msg); 50 | 51 | mavlink_status_t* mavlink_get_channel_status(uint8_t channel); 52 | mavlink_message_t* mavlink_get_channel_buffer(uint8_t channel); 53 | 54 | #endif /* COMMUNICATION_H_ */ 55 | -------------------------------------------------------------------------------- /src/include/dcmi.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef DCMI_H_ 35 | #define DCMI_H_ 36 | 37 | #include 38 | #include "mt9v034.h" 39 | 40 | #define DCMI_DR_ADDRESS 0x50050028 41 | 42 | /** 43 | * @brief Copy image to fast RAM address 44 | */ 45 | void dma_copy_image_buffers(uint8_t ** current_image, uint8_t ** previous_image, uint16_t buffer_size, uint8_t image_step); 46 | 47 | /** 48 | * @brief Send calibration image with MAVLINK over USB 49 | */ 50 | void send_calibration_image(uint8_t ** image_buffer_fast_1, uint8_t ** image_buffer_fast_2); 51 | 52 | /** 53 | * @brief Initialize DCMI DMA and enable image capturing 54 | */ 55 | void enable_image_capture(void); 56 | 57 | /* Init Functions */ 58 | void dcmi_clock_init(void); 59 | void dcmi_hw_init(void); 60 | void dcmi_dma_init(uint16_t buffer_size); 61 | void dcmi_it_init(void); 62 | void dma_it_init(void); 63 | 64 | /* Interrupt Handlers */ 65 | void DCMI_IRQHandler(void); 66 | void DMA2_Stream1_IRQHandler(void); 67 | 68 | void dcmi_dma_enable(void); 69 | void dcmi_dma_disable(void); 70 | void dma_reconfigure(void); 71 | void dcmi_restart_calibration_routine(void); 72 | void dma_swap_buffers(void); 73 | 74 | uint32_t get_time_between_images(void); 75 | uint32_t get_frame_counter(void); 76 | void reset_frame_counter(void); 77 | 78 | #endif /* DCMI_H_ */ 79 | -------------------------------------------------------------------------------- /src/include/debug.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef DEBUG_H_ 35 | #define DEBUG_H_ 36 | 37 | #define DEBUG_COUNT 16 38 | #define DEBUG_MAX_LEN MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN 39 | 40 | /** @brief Buffer one debug message */ 41 | uint8_t debug_string_message_buffer(const char* string); 42 | 43 | /** @brief Concatenate string and int and buffer debug message */ 44 | uint8_t debug_int_message_buffer(const char* string, int32_t num); 45 | 46 | /** @brief Concatenate string and float and buffer debug message */ 47 | uint8_t debug_float_message_buffer(const char* string, float num); 48 | 49 | /** @brief Send one of the buffered messages */ 50 | void debug_message_send_one(void); 51 | 52 | #endif /* DEBUG_H_ */ 53 | -------------------------------------------------------------------------------- /src/include/flow.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef FLOW_H_ 35 | #define FLOW_H_ 36 | 37 | #include 38 | 39 | /** 40 | * @brief Computes pixel flow from image1 to image2 41 | */ 42 | uint8_t compute_flow(uint8_t *image1, uint8_t *image2, float x_rate, float y_rate, float z_rate, 43 | float *histflowx, float *histflowy); 44 | 45 | #endif /* FLOW_H_ */ 46 | -------------------------------------------------------------------------------- /src/include/hrt.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | 39 | 40 | __BEGIN_DECLS 41 | __EXPORT uint64_t hrt_absolute_time(void); 42 | __END_DECLS 43 | -------------------------------------------------------------------------------- /src/include/i2c.h: -------------------------------------------------------------------------------- 1 | ///**************************************************************************** 2 | // * 3 | // * Copyright (C) 2013 Fortiss An-Institut TU Munchen All rights reserved. 4 | // * Author: Thomas Boehm 5 | // * 6 | // * Redistribution and use in source and binary forms, with or without 7 | // * modification, are permitted provided that the following conditions 8 | // * are met: 9 | // * 10 | // * 1. Redistributions of source code must retain the above copyright 11 | // * notice, this list of conditions and the following disclaimer. 12 | // * 2. Redistributions in binary form must reproduce the above copyright 13 | // * notice, this list of conditions and the following disclaimer in 14 | // * the documentation and/or other materials provided with the 15 | // * distribution. 16 | // * 17 | // * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 18 | // * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 19 | // * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 20 | // * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 21 | // * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 22 | // * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 23 | // * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 24 | // * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 25 | // * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 26 | // * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 27 | // * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 | // * POSSIBILITY OF SUCH DAMAGE. 29 | // * 30 | // ****************************************************************************/ 31 | 32 | /** 33 | * @file i2c.h 34 | * I2C communication functions. 35 | * @author Thomas Boehm 36 | */ 37 | 38 | 39 | #ifndef I2C_H_ 40 | #define I2C_H_ 41 | #include 42 | #include 43 | 44 | #define I2C1_OWNADDRESS_1_BASE 0x42 //7bit base address 45 | /** 46 | * @brief Configures I2C1 for communication as a slave (default behaviour for STM32F) 47 | */ 48 | 49 | void i2c_init(void); 50 | void update_TX_buffer(float pixel_flow_x, float pixel_flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t qual, 51 | float ground_distance, float x_rate, float y_rate, float z_rate, int16_t gyro_temp, legacy_12c_data_t *pd); 52 | char i2c_get_ownaddress1(void); 53 | #endif /* I2C_H_ */ 54 | 55 | -------------------------------------------------------------------------------- /src/include/led.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef LED_H_ 35 | #define LED_H_ 36 | 37 | #include "stm32f4xx_conf.h" 38 | #include "stm32f4xx.h" 39 | 40 | typedef enum 41 | { 42 | LED_ACT = 0, // Blue 43 | LED_COM = 1, // Amber 44 | LED_ERR = 2, // Red 45 | } Led_TypeDef; 46 | 47 | 48 | #define LEDn 3 49 | 50 | #define LED_ACTIVITY_PIN GPIO_Pin_3 51 | #define LED_ACTIVITY_GPIO_PORT GPIOE 52 | #define LED_ACTIVITY_GPIO_CLK RCC_AHB1Periph_GPIOE 53 | 54 | #define LED_BOOTLOADER_PIN GPIO_Pin_2 55 | #define LED_BOOTLOADER_GPIO_PORT GPIOE 56 | #define LED_BOOTLOADER_GPIO_CLK RCC_AHB1Periph_GPIOE 57 | 58 | #define LED_TEST_PIN GPIO_Pin_7 59 | #define LED_TEST_GPIO_PORT GPIOE 60 | #define LED_TEST_GPIO_CLK RCC_AHB1Periph_GPIOE 61 | 62 | 63 | void LEDInit(Led_TypeDef Led); 64 | void LEDOn(Led_TypeDef Led); 65 | void LEDOff(Led_TypeDef Led); 66 | void LEDToggle(Led_TypeDef Led); 67 | 68 | 69 | #endif /* LED_H_ */ 70 | -------------------------------------------------------------------------------- /src/include/legacy_i2c.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #pragma once 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | typedef struct legacy_12c_data_t { 41 | uint64_t time_stamp_utc; 42 | } legacy_12c_data_t; 43 | -------------------------------------------------------------------------------- /src/include/main.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef __PX4_FLOWBOARD_H 35 | #define __PX4_FLOWBOARD_H 36 | 37 | extern uint32_t get_time_between_images(void); 38 | 39 | void timer_update(void); 40 | void timer_update_ms(void); 41 | uint32_t get_boot_time_ms(void); 42 | uint32_t get_boot_time_us(void); 43 | 44 | #endif /* __PX4_FLOWBOARD_H */ 45 | -------------------------------------------------------------------------------- /src/include/mavlink_bridge_header.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef MAVLINK_BRIDGE_HEADER_H_ 35 | #define MAVLINK_BRIDGE_HEADER_H_ 36 | 37 | #include 38 | #include 39 | 40 | #define MAVLINK_USE_CONVENIENCE_FUNCTIONS 41 | 42 | /* use efficient approach, see mavlink_helpers.h */ 43 | #define MAVLINK_SEND_UART_BYTES mavlink_send_uart_bytes 44 | #define MAVLINK_GET_CHANNEL_STATUS mavlink_get_channel_status 45 | #define MAVLINK_GET_CHANNEL_BUFFER mavlink_get_channel_buffer 46 | 47 | #include 48 | 49 | extern mavlink_system_t mavlink_system; 50 | 51 | /* defined in communication.c */ 52 | extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t * ch, uint16_t length); 53 | 54 | mavlink_status_t* mavlink_get_channel_status(uint8_t chan); 55 | mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan); 56 | 57 | #endif /* MAVLINK_BRIDGE_HEADER_H_ */ 58 | -------------------------------------------------------------------------------- /src/include/no_warnings.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2015 PX4 Development Team. All rights reserved. 4 | * Author: David Sidrane 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | #ifndef NO_WARNINGS_H_ 35 | #define NO_WARNINGS_H_ 36 | 37 | #include 38 | 39 | inline bool FLOAT_AS_BOOL(float f); 40 | inline int FLOAT_EQ_INT(float f , int i); 41 | inline int FLOAT_EQ_FLOAT(float f1 , float f2); 42 | 43 | inline bool FLOAT_AS_BOOL(float f) 44 | { 45 | #pragma GCC diagnostic push 46 | #pragma GCC diagnostic ignored "-Wfloat-equal" 47 | return (f != 0.0f); 48 | #pragma GCC diagnostic pop 49 | } 50 | 51 | inline int FLOAT_EQ_INT(float f , int i) 52 | { 53 | #pragma GCC diagnostic push 54 | #pragma GCC diagnostic ignored "-Wfloat-equal" 55 | return (f == i); 56 | #pragma GCC diagnostic pop 57 | } 58 | 59 | inline int FLOAT_EQ_FLOAT(float f1 , float f2) 60 | { 61 | #pragma GCC diagnostic push 62 | #pragma GCC diagnostic ignored "-Wfloat-equal" 63 | return (f1 == f2); 64 | #pragma GCC diagnostic pop 65 | } 66 | 67 | #endif /* NO_WARNINGS_H_ */ 68 | -------------------------------------------------------------------------------- /src/include/sonar.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef SONAR_H_ 35 | #define SONAR_H_ 36 | 37 | #include 38 | #include 39 | #include "settings.h" 40 | 41 | /** 42 | * @brief Configures the sonar sensor Peripheral. 43 | */ 44 | void sonar_config(void); 45 | 46 | /** 47 | * @brief Sonar interrupt handler 48 | */ 49 | void UART4_IRQHandler(void); 50 | 51 | /** 52 | * @brief Triggers the sonar to measure the next value 53 | */ 54 | void sonar_trigger(void); 55 | 56 | /** 57 | * @brief Read out newest sonar data 58 | * 59 | * @return true if valid measurement values were obtained, false else 60 | */ 61 | bool sonar_read(float* sonar_value_filtered, float* sonar_value_raw); 62 | 63 | /** 64 | * @brief Get the timestamp of the new sonar value when available to the main code 65 | */ 66 | uint32_t get_sonar_measure_time(void); 67 | 68 | /** 69 | * @brief Get the timestamp of the new sonar value when the interrupt is triggered 70 | */ 71 | uint32_t get_sonar_measure_time_interrupt(void); 72 | 73 | #endif /* SONAR_H_ */ 74 | -------------------------------------------------------------------------------- /src/include/sonar_mode_filter.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (c) 2014 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef SONAR_MODE_FILTER_H_ 35 | #define SONAR_MODE_FILTER_H_ 36 | 37 | #ifdef __cplusplus 38 | extern "C" { 39 | #endif 40 | 41 | float insert_sonar_value_and_get_mode_value(float insert); 42 | 43 | #ifdef __cplusplus 44 | } 45 | #endif 46 | 47 | #endif /* SONAR_MODE_FILTER_H_ */ 48 | -------------------------------------------------------------------------------- /src/include/stm32f4xx_it.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_it.h 4 | * @author MCD Application Team 5 | * @version V1.0.0 6 | * @date 19-September-2011 7 | * @brief This file contains the headers of the interrupt handlers. 8 | ****************************************************************************** 9 | * @attention 10 | * 11 | * THE PRESENT FIRMWARE WHICH IS FOR GUIDANCE ONLY AIMS AT PROVIDING CUSTOMERS 12 | * WITH CODING INFORMATION REGARDING THEIR PRODUCTS IN ORDER FOR THEM TO SAVE 13 | * TIME. AS A RESULT, STMICROELECTRONICS SHALL NOT BE HELD LIABLE FOR ANY 14 | * DIRECT, INDIRECT OR CONSEQUENTIAL DAMAGES WITH RESPECT TO ANY CLAIMS ARISING 15 | * FROM THE CONTENT OF SUCH FIRMWARE AND/OR THE USE MADE BY CUSTOMERS OF THE 16 | * CODING INFORMATION CONTAINED HEREIN IN CONNECTION WITH THEIR PRODUCTS. 17 | * 18 | *

© COPYRIGHT 2011 STMicroelectronics

19 | ****************************************************************************** 20 | */ 21 | 22 | /* Define to prevent recursive inclusion -------------------------------------*/ 23 | #ifndef __STM32F4xx_IT_H 24 | #define __STM32F4xx_IT_H 25 | 26 | #include "stm32f4xx.h" 27 | 28 | void NMI_Handler(void); 29 | void HardFault_Handler(void); 30 | void MemManage_Handler(void); 31 | void BusFault_Handler(void); 32 | void UsageFault_Handler(void); 33 | void SVC_Handler(void); 34 | void DebugMon_Handler(void); 35 | void PendSV_Handler(void); 36 | void SysTick_Handler(void); 37 | 38 | #endif /* __STM32F4xx_IT_H */ 39 | 40 | /******************* (C) COPYRIGHT 2011 STMicroelectronics *****END OF FILE****/ 41 | -------------------------------------------------------------------------------- /src/include/usart.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | #ifndef USART_H_ 35 | #define USART_H_ 36 | 37 | #include 38 | 39 | /** 40 | * @brief Configures USART2 and USART3 for communication 41 | */ 42 | void usart_init(void); 43 | 44 | /** 45 | * @brief Pop one byte from ringbuffer of USART2 46 | */ 47 | uint8_t usart2_rx_ringbuffer_pop(void); 48 | 49 | /** 50 | * @brief Push one byte to ringbuffer of USART2 51 | */ 52 | uint8_t usart2_tx_ringbuffer_push(const uint8_t* ch, uint8_t len); 53 | 54 | /** 55 | * @brief Pop one byte from ringbuffer of USART3 56 | */ 57 | uint8_t usart3_rx_ringbuffer_pop(void); 58 | 59 | /** 60 | * @brief Push one byte to ringbuffer of USART3 61 | */ 62 | uint8_t usart3_tx_ringbuffer_push(const uint8_t* ch, uint8_t len); 63 | 64 | /** 65 | * @brief Check character availability USART2 66 | */ 67 | int usart2_char_available(void); 68 | 69 | /** 70 | * @brief Check character availability USART3 71 | */ 72 | int usart3_char_available(void); 73 | 74 | void USART2_IRQHandler(void); 75 | void USART3_IRQHandler(void); 76 | 77 | #endif /* USART_H_ */ 78 | -------------------------------------------------------------------------------- /src/include/usbd_cdc_vcp.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_cdc_vcp.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief Header for usbd_cdc_vcp.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 | #ifndef __USBD_CDC_VCP_H 29 | #define __USBD_CDC_VCP_H 30 | 31 | #include 32 | 33 | /* The following structures groups all needed parameters to be configured for the 34 | ComPort. These parameters can modified on the fly by the host through CDC class 35 | command class requests. */ 36 | typedef struct 37 | { 38 | uint32_t bitrate; 39 | uint8_t format; 40 | uint8_t paritytype; 41 | uint8_t datatype; 42 | }LINE_CODING; 43 | 44 | /* Functions */ 45 | void VCP_put_char(uint8_t buf); 46 | 47 | void VCP_send_str(uint8_t* buf); 48 | 49 | uint8_t VCP_get_char(uint8_t *buf); 50 | 51 | uint8_t VCP_get_string(uint8_t *buf); 52 | 53 | #endif /* __USBD_CDC_VCP_H */ 54 | 55 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 56 | -------------------------------------------------------------------------------- /src/include/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 | #ifndef __USBD_CONF__H__ 29 | #define __USBD_CONF__H__ 30 | 31 | #include "usb_conf.h" 32 | 33 | #define USBD_CFG_MAX_NUM 1 34 | #define USBD_ITF_MAX_NUM 1 35 | #define USB_MAX_STR_DESC_SIZ 255 36 | 37 | #define CDC_IN_EP 0x81 /* EP1 for data IN */ 38 | #define CDC_OUT_EP 0x01 /* EP1 for data OUT */ 39 | #define CDC_CMD_EP 0x82 /* EP2 for CDC commands */ 40 | 41 | /* CDC Endpoints parameters: you can fine tune these values depending on the needed baudrates and performance. */ 42 | 43 | #define CDC_DATA_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ 44 | #define CDC_CMD_PACKET_SZE 8 /* Control Endpoint Packet size */ 45 | 46 | /* Number of frames between IN transfers */ 47 | #define CDC_IN_FRAME_INTERVAL 40 //5 48 | 49 | /* Total size of IN buffer: APP_RX_DATA_SIZE*8/MAX_BAUDARATE*1000 should be > CDC_IN_FRAME_INTERVAL */ 50 | #define APP_RX_DATA_SIZE 32768 //2048 51 | 52 | #define APP_FOPS VCP_fops 53 | 54 | #endif //__USBD_CONF__H__ 55 | 56 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 57 | 58 | -------------------------------------------------------------------------------- /src/include/usbd_desc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file usbd_desc.h 4 | * @author MCD Application Team 5 | * @version V1.1.0 6 | * @date 19-March-2012 7 | * @brief header file for the usbd_desc.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 | #ifndef __USB_DESC_H 29 | #define __USB_DESC_H 30 | 31 | #include "usbd_def.h" 32 | 33 | #define USB_DEVICE_DESCRIPTOR_TYPE 0x01 34 | #define USB_CONFIGURATION_DESCRIPTOR_TYPE 0x02 35 | #define USB_STRING_DESCRIPTOR_TYPE 0x03 36 | #define USB_INTERFACE_DESCRIPTOR_TYPE 0x04 37 | #define USB_ENDPOINT_DESCRIPTOR_TYPE 0x05 38 | #define USB_SIZ_DEVICE_DESC 18 39 | #define USB_SIZ_STRING_LANGID 4 40 | 41 | extern uint8_t USBD_DeviceDesc [USB_SIZ_DEVICE_DESC]; 42 | extern uint8_t USBD_StrDesc[USB_MAX_STR_DESC_SIZ]; 43 | extern uint8_t USBD_OtherSpeedCfgDesc[USB_LEN_CFG_DESC]; 44 | extern uint8_t USBD_DeviceQualifierDesc[USB_LEN_DEV_QUALIFIER_DESC]; 45 | extern uint8_t USBD_LangIDDesc[USB_SIZ_STRING_LANGID]; 46 | extern USBD_DEVICE USR_desc; 47 | 48 | uint8_t * USBD_USR_DeviceDescriptor( uint8_t speed , uint16_t *length); 49 | uint8_t * USBD_USR_LangIDStrDescriptor( uint8_t speed , uint16_t *length); 50 | uint8_t * USBD_USR_ManufacturerStrDescriptor ( uint8_t speed , uint16_t *length); 51 | uint8_t * USBD_USR_ProductStrDescriptor ( uint8_t speed , uint16_t *length); 52 | uint8_t * USBD_USR_SerialStrDescriptor( uint8_t speed , uint16_t *length); 53 | uint8_t * USBD_USR_ConfigStrDescriptor( uint8_t speed , uint16_t *length); 54 | uint8_t * USBD_USR_InterfaceStrDescriptor( uint8_t speed , uint16_t *length); 55 | 56 | #endif /* __USBD_DESC_H */ 57 | 58 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 59 | -------------------------------------------------------------------------------- /src/include/utils.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2013 PX4 Development Team. All rights reserved. 4 | * -> Code from CodeForge.com 5 | * 6 | * Redistribution and use in source and binary forms, with or without 7 | * modification, are permitted provided that the following conditions 8 | * are met: 9 | * 10 | * 1. Redistributions of source code must retain the above copyright 11 | * notice, this list of conditions and the following disclaimer. 12 | * 2. Redistributions in binary form must reproduce the above copyright 13 | * notice, this list of conditions and the following disclaimer in 14 | * the documentation and/or other materials provided with the 15 | * distribution. 16 | * 3. Neither the name PX4 nor the names of its contributors may be 17 | * used to endorse or promote products derived from this software 18 | * without specific prior written permission. 19 | * 20 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | * POSSIBILITY OF SUCH DAMAGE. 32 | * 33 | ****************************************************************************/ 34 | 35 | #ifndef UTILS_H_ 36 | #define UTILS_H_ 37 | 38 | #include 39 | 40 | /** 41 | * @brief Float to Ascii string 42 | */ 43 | char* flow_ftoa(float f); 44 | char* flow_ltoa(char *buf, unsigned long i, int base); 45 | char* flow_itoa(char *buf, unsigned int i, int base); 46 | 47 | #endif /* UTILS_H_ */ 48 | -------------------------------------------------------------------------------- /src/include/visibility.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * 3 | * Copyright (C) 2012 PX4 Development Team. All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in 13 | * the documentation and/or other materials provided with the 14 | * distribution. 15 | * 3. Neither the name PX4 nor the names of its contributors may be 16 | * used to endorse or promote products derived from this software 17 | * without specific prior written permission. 18 | * 19 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 20 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 21 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 22 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 23 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 24 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 25 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 26 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 27 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 28 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 29 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 30 | * POSSIBILITY OF SUCH DAMAGE. 31 | * 32 | ****************************************************************************/ 33 | 34 | /** 35 | * @file visibility.h 36 | * 37 | * Definitions controlling symbol naming and visibility. 38 | * 39 | * This file is normally included automatically by the build system. 40 | */ 41 | 42 | #ifndef __SYSTEMLIB_VISIBILITY_H 43 | #define __SYSTEMLIB_VISIBILITY_H 44 | 45 | #ifdef __EXPORT 46 | # undef __EXPORT 47 | #endif 48 | #define __EXPORT __attribute__ ((visibility ("default"))) 49 | 50 | #ifdef __PRIVATE 51 | # undef __PRIVATE 52 | #endif 53 | #define __PRIVATE __attribute__ ((visibility ("hidden"))) 54 | 55 | #ifdef __cplusplus 56 | # define __BEGIN_DECLS extern "C" { 57 | # define __END_DECLS } 58 | #else 59 | # define __BEGIN_DECLS 60 | # define __END_DECLS 61 | #endif 62 | #endif -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/README: -------------------------------------------------------------------------------- 1 | STM32 Library 2 | 3 | Version: STM32_USB-Host-Device_Lib_V2.1.0 4 | 5 | 6 | Changes / Bugfixes: 7 | 8 | * stm32f4xx.h 9 | - 6998: add assert_param to avoid make errors 10 | 11 | * STM32_USB_OTG_Driver/src/usb_dcd_int.c 12 | - 680: add a workaround for "empty buffer interrupt"-bug 13 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/Release_Notes.html: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/PX4-Flow/6c8cdb3cc04a5854a7b50e76e267e35507a0a0c4/src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/Release_Notes.html -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/inc/stm32f4xx_crc.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file stm32f4xx_crc.h 4 | * @author MCD Application Team 5 | * @version V1.0.2 6 | * @date 05-March-2012 7 | * @brief This file contains all the functions prototypes for the CRC firmware 8 | * library. 9 | ****************************************************************************** 10 | * @attention 11 | * 12 | *

© COPYRIGHT 2012 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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/PX4-Flow/6c8cdb3cc04a5854a7b50e76e267e35507a0a0c4/src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_flash.c -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PX4/PX4-Flow/6c8cdb3cc04a5854a7b50e76e267e35507a0a0c4/src/lib/stm32/st/v1.0.2/STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | 36 | /** @addtogroup USB_OTG_DRIVER 37 | * @{ 38 | */ 39 | 40 | /** @defgroup USB_BSP 41 | * @brief This file is the 42 | * @{ 43 | */ 44 | 45 | 46 | /** @defgroup USB_BSP_Exported_Defines 47 | * @{ 48 | */ 49 | /** 50 | * @} 51 | */ 52 | 53 | 54 | /** @defgroup USB_BSP_Exported_Types 55 | * @{ 56 | */ 57 | /** 58 | * @} 59 | */ 60 | 61 | 62 | /** @defgroup USB_BSP_Exported_Macros 63 | * @{ 64 | */ 65 | /** 66 | * @} 67 | */ 68 | 69 | /** @defgroup USB_BSP_Exported_Variables 70 | * @{ 71 | */ 72 | /** 73 | * @} 74 | */ 75 | 76 | /** @defgroup USB_BSP_Exported_FunctionsPrototype 77 | * @{ 78 | */ 79 | void BSP_Init(void); 80 | 81 | void USB_OTG_BSP_Init (USB_OTG_CORE_HANDLE *pdev); 82 | void USB_OTG_BSP_uDelay (const uint32_t usec); 83 | void USB_OTG_BSP_mDelay (const uint32_t msec); 84 | void USB_OTG_BSP_EnableInterrupt (USB_OTG_CORE_HANDLE *pdev); 85 | #ifdef USE_HOST_MODE 86 | void USB_OTG_BSP_ConfigVBUS(USB_OTG_CORE_HANDLE *pdev); 87 | void USB_OTG_BSP_DriveVBUS(USB_OTG_CORE_HANDLE *pdev,uint8_t state); 88 | #endif 89 | /** 90 | * @} 91 | */ 92 | 93 | #endif //__USB_BSP__H__ 94 | 95 | /** 96 | * @} 97 | */ 98 | 99 | /** 100 | * @} 101 | */ 102 | /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ 103 | 104 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/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 | -------------------------------------------------------------------------------- /src/lib/stm32/st/v1.0.2/system_stm32f4xx.h: -------------------------------------------------------------------------------- 1 | /** 2 | ****************************************************************************** 3 | * @file system_stm32f4xx.h 4 | * @author MCD Application Team 5 | * @version V1.0.2 6 | * @date 05-March-2012 7 | * @brief CMSIS Cortex-M4 Device System Source File for STM32F4xx devices. 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 | /** @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 | -------------------------------------------------------------------------------- /src/modules/flow/module.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Flow Code as it 3 | # 4 | DEFAULT_VISIBILITY=default 5 | 6 | ST_LIB_VERSION=v1.0.2 7 | 8 | ST_LIB = $(ST_LIB_DIR)$(ST_LIB_VERSION)/ 9 | 10 | INCLUDE_DIRS += $(MAVLINK_SRC) \ 11 | $(MAVLINK_SRC)common \ 12 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/inc \ 13 | $(ST_LIB)STM32_USB_Device_Library/Class/cdc/inc \ 14 | $(ST_LIB)STM32_USB_Device_Library/Core/inc \ 15 | $(ST_LIB)STM32_USB_HOST_Library/Core/inc \ 16 | $(ST_LIB)STM32_USB_OTG_Driver/inc \ 17 | 18 | 19 | SRCS = $(ST_LIB)startup_stm32f4xx.s \ 20 | system_stm32f4xx.c \ 21 | stm32f4xx_it.c 22 | 23 | 24 | SRCS += main.c \ 25 | utils.c \ 26 | led.c \ 27 | settings.c \ 28 | communication.c \ 29 | flow.c \ 30 | dcmi.c \ 31 | mt9v034.c \ 32 | gyro.c \ 33 | usart.c \ 34 | sonar.c \ 35 | debug.c \ 36 | usb_bsp.c \ 37 | usbd_cdc_vcp.c \ 38 | usbd_desc.c \ 39 | usbd_usr.c \ 40 | i2c.c \ 41 | reset.c \ 42 | sonar_mode_filter.c 43 | 44 | SRCS += $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/misc.c \ 45 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_rcc.c \ 46 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dma.c \ 47 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_dcmi.c \ 48 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_i2c.c \ 49 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_tim.c \ 50 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_spi.c \ 51 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_usart.c \ 52 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_adc.c \ 53 | $(ST_LIB)STM32F4xx_StdPeriph_Driver/src/stm32f4xx_gpio.c \ 54 | $(ST_LIB)STM32_USB_OTG_Driver/src/usb_core.c \ 55 | $(ST_LIB)STM32_USB_OTG_Driver/src/usb_dcd_int.c \ 56 | $(ST_LIB)STM32_USB_OTG_Driver/src/usb_dcd.c \ 57 | $(ST_LIB)STM32_USB_Device_Library/Core/src/usbd_core.c \ 58 | $(ST_LIB)STM32_USB_Device_Library/Core/src/usbd_req.c \ 59 | $(ST_LIB)STM32_USB_Device_Library/Core/src/usbd_ioreq.c \ 60 | $(ST_LIB)STM32_USB_Device_Library/Class/cdc/src/usbd_cdc_core.c 61 | 62 | $(info SRCS=$(SRCS)) 63 | -------------------------------------------------------------------------------- /src/modules/libc/.gitignore: -------------------------------------------------------------------------------- 1 | /Make_bin.dep 2 | /Make_ubin.dep 3 | /Make_kbin.dep 4 | /.depend 5 | /*.lib 6 | 7 | -------------------------------------------------------------------------------- /src/modules/libc/init.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2004 CodeSourcery, LLC 3 | * 4 | * Permission to use, copy, modify, and distribute this file 5 | * for any purpose is hereby granted without fee, provided that 6 | * the above copyright notice and this notice appears in all 7 | * copies. 8 | * 9 | * This file is distributed WITHOUT ANY WARRANTY; without even the implied 10 | * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 11 | */ 12 | 13 | /* Handle ELF .{pre_init,init,fini}_array sections. */ 14 | #include 15 | 16 | #ifdef HAVE_INITFINI_ARRAY 17 | 18 | /* These magic symbols are provided by the linker. */ 19 | extern void (*__preinit_array_start []) (void) __attribute__((weak)); 20 | extern void (*__preinit_array_end []) (void) __attribute__((weak)); 21 | extern void (*__init_array_start []) (void) __attribute__((weak)); 22 | extern void (*__init_array_end []) (void) __attribute__((weak)); 23 | extern void (*__fini_array_start []) (void) __attribute__((weak)); 24 | extern void (*__fini_array_end []) (void) __attribute__((weak)); 25 | 26 | extern void _init (void); 27 | extern void _fini (void); 28 | void __libc_init_array (void); 29 | void __libc_fini_array (void); 30 | 31 | 32 | void _init(void) 33 | { 34 | 35 | } 36 | /* Iterate over all the init routines. */ 37 | void 38 | __libc_init_array (void) 39 | { 40 | size_t count; 41 | size_t i; 42 | 43 | count = __preinit_array_end - __preinit_array_start; 44 | for (i = 0; i < count; i++) 45 | __preinit_array_start[i] (); 46 | 47 | _init (); 48 | 49 | count = __init_array_end - __init_array_start; 50 | for (i = 0; i < count; i++) 51 | __init_array_start[i] (); 52 | } 53 | 54 | /* Run all the cleanup routines. */ 55 | void 56 | __libc_fini_array (void) 57 | { 58 | size_t count; 59 | size_t i; 60 | 61 | count = __fini_array_end - __fini_array_start; 62 | for (i = count; i > 0; i--) 63 | __fini_array_start[i-1] (); 64 | 65 | _fini (); 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /src/modules/libc/module.mk: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. 4 | # Author: Pavel Kirienko 5 | # David Sidrane 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name PX4 nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | CLIB_INC = $(PX4_MODULE_SRC)/modules/libc/ 37 | INCLUDE_DIRS += $(CLIB_INC) $(CLIB_INC)syslog 38 | 39 | $(info INCLUDE_DIRS=$(INCLUDE_DIRS)) 40 | DEFAULT_VISIBILITY=default 41 | 42 | # Main 43 | 44 | STRINGSRCS = string/lib_strncmp.c \ 45 | string/lib_strcmp.c \ 46 | string/lib_memcpy.c \ 47 | string/lib_memset.c \ 48 | string/lib_strcpy.c \ 49 | string/lib_strncpy.c \ 50 | string/lib_strlen.c \ 51 | string/lib_strncat.c \ 52 | string/lib_skipspace.c \ 53 | string/lib_isbasedigit.c \ 54 | 55 | STDIOSRCS = stdio/lib_printf.c \ 56 | 57 | STDLIBSRCS= stdlib/lib_strtol.c \ 58 | stdlib/lib_strtoul.c \ 59 | stdlib/lib_checkbase.c \ 60 | 61 | 62 | SRCS = init.c \ 63 | $(STRINGSRCS) \ 64 | $(STDIOSRCS) \ 65 | $(STDLIBSRCS) -------------------------------------------------------------------------------- /src/modules/libc/stdio/lib_clearerr.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/stdio/lib_clearerr.c 3 | * 4 | * Copyright (C) 2012 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #if CONFIG_NFILE_STREAMS > 0 48 | 49 | /**************************************************************************** 50 | * Public Functions 51 | ****************************************************************************/ 52 | 53 | /**************************************************************************** 54 | * Name: Functions 55 | * 56 | * Description: 57 | * Clear any end-of-file or error conditions. 58 | * 59 | * Returned Value: 60 | * None 61 | * 62 | ****************************************************************************/ 63 | 64 | void clearerr(FILE *stream) 65 | { 66 | stream->fs_flags = 0; 67 | } 68 | #endif /* CONFIG_NFILE_STREAMS */ 69 | 70 | -------------------------------------------------------------------------------- /src/modules/libc/stdio/lib_dprintf.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/stdio/lib_dprintf.c 3 | * 4 | * Copyright (C) 2012 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | /**************************************************************************** 43 | * Public Functions 44 | **************************************************************************/ 45 | 46 | /**************************************************************************** 47 | * Name: dprintf 48 | **************************************************************************/ 49 | 50 | int dprintf(int fd, FAR const char *fmt, ...) 51 | { 52 | va_list ap; 53 | int ret; 54 | 55 | va_start(ap, fmt); 56 | ret = vdprintf(fd, fmt, ap); 57 | va_end(ap); 58 | 59 | return ret; 60 | } 61 | -------------------------------------------------------------------------------- /src/modules/libc/stdio/lib_fileno.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/stdio/lib_fileno.c 3 | * 4 | * Copyright (C) 2009, 2011, 2013 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #if CONFIG_NFILE_STREAMS > 0 48 | 49 | /**************************************************************************** 50 | * Global Functions 51 | ****************************************************************************/ 52 | 53 | int fileno(FAR FILE *stream) 54 | { 55 | int ret = -1; 56 | if (stream) 57 | { 58 | ret = stream->fs_fd; 59 | } 60 | 61 | if (ret < 0) 62 | { 63 | set_errno(EBADF); 64 | return ERROR; 65 | } 66 | 67 | return ret; 68 | } 69 | #endif /* CONFIG_NFILE_STREAMS */ 70 | -------------------------------------------------------------------------------- /src/modules/libc/stdio/lib_vdprintf.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/stdio/lib_vdprintf.c 3 | * 4 | * Copyright (C) 2012 Andrew Tridgell. All rights reserved. 5 | * Authors: Author: Andrew Tridgell 6 | * Gregory Nutt 7 | * 8 | * Redistribution and use in source and binary forms, with or without 9 | * modification, are permitted provided that the following conditions 10 | * are met: 11 | * 12 | * 1. Redistributions of source code must retain the above copyright 13 | * notice, this list of conditions and the following disclaimer. 14 | * 2. Redistributions in binary form must reproduce the above copyright 15 | * notice, this list of conditions and the following disclaimer in 16 | * the documentation and/or other materials provided with the 17 | * distribution. 18 | * 3. Neither the name NuttX nor the names of its contributors may be 19 | * used to endorse or promote products derived from this software 20 | * without specific prior written permission. 21 | * 22 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 23 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 24 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 25 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 26 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 27 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 28 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 29 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 30 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 31 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 32 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 33 | * POSSIBILITY OF SUCH DAMAGE. 34 | * 35 | ****************************************************************************/ 36 | 37 | /**************************************************************************** 38 | * Included Files 39 | ****************************************************************************/ 40 | 41 | #include 42 | 43 | #include 44 | 45 | #include "lib_internal.h" 46 | 47 | /**************************************************************************** 48 | * Public Functions 49 | ****************************************************************************/ 50 | 51 | int vdprintf(int fd, FAR const char *fmt, va_list ap) 52 | { 53 | struct lib_rawoutstream_s rawoutstream; 54 | 55 | /* Wrap the fd in a stream object and let lib_vsprintf do the work. */ 56 | 57 | lib_rawoutstream(&rawoutstream, fd); 58 | return lib_vsprintf(&rawoutstream.public, fmt, ap); 59 | } 60 | -------------------------------------------------------------------------------- /src/modules/libc/stdlib/Make.defs: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # libc/stdlib/Make.defs 3 | # 4 | # Copyright (C) 2012 Gregory Nutt. All rights reserved. 5 | # Author: Gregory Nutt 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name NuttX nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | # Add the stdlib C files to the build 37 | 38 | CSRCS += lib_abs.c lib_abort.c lib_imaxabs.c lib_itoa.c lib_labs.c 39 | CSRCS += lib_llabs.c lib_rand.c lib_qsort.c 40 | CSRCS += lib_strtol.c lib_strtoll.c lib_strtoul.c lib_strtoull.c 41 | CSRCS += lib_strtod.c lib_checkbase.c 42 | 43 | ifeq ($(CONFIG_FS_WRITABLE),y) 44 | CSRCS += lib_mktemp.c lib_mkstemp.c 45 | endif 46 | 47 | # Add the stdlib directory to the build 48 | 49 | DEPPATH += --dep-path stdlib 50 | VPATH += :stdlib 51 | -------------------------------------------------------------------------------- /src/modules/libc/stdlib/lib_abs.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/stdlib/lib_abs.c 3 | * 4 | * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | /************************************************************************ 44 | * Global Functions 45 | ************************************************************************/ 46 | 47 | int abs(int j) 48 | { 49 | if (j < 0) 50 | { 51 | j = -j; 52 | } 53 | return j; 54 | } 55 | -------------------------------------------------------------------------------- /src/modules/libc/stdlib/lib_imaxabs.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/stdlib//lib_abs.c 3 | * 4 | * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | /************************************************************************ 44 | * Global Functions 45 | ************************************************************************/ 46 | 47 | intmax_t imaxabs(intmax_t j) 48 | { 49 | if (j < 0) 50 | { 51 | j = -j; 52 | } 53 | return j; 54 | } 55 | -------------------------------------------------------------------------------- /src/modules/libc/stdlib/lib_labs.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/stdlib/lib_labs.c 3 | * 4 | * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | #include 42 | 43 | /************************************************************************ 44 | * Global Functions 45 | ************************************************************************/ 46 | 47 | long int labs(long int j) 48 | { 49 | if (j < 0) 50 | { 51 | j = -j; 52 | } 53 | return j; 54 | } 55 | -------------------------------------------------------------------------------- /src/modules/libc/stdlib/lib_llabs.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/stdlib/lib_llabs.c 3 | * 4 | * Copyright (C) 2010-2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /************************************************************************ 45 | * Global Functions 46 | ************************************************************************/ 47 | 48 | #ifdef CONFIG_HAVE_LONG_LONG 49 | long long int llabs(long long int j) 50 | { 51 | if (j < 0) 52 | { 53 | j = -j; 54 | } 55 | return j; 56 | } 57 | #endif 58 | -------------------------------------------------------------------------------- /src/modules/libc/string/Make.defs: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # libc/string/Make.defs 3 | # 4 | # Copyright (C) 2011-2012, 2014 Gregory Nutt. All rights reserved. 5 | # Author: Gregory Nutt 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name NuttX nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | # Add the string C files to the build 37 | 38 | CSRCS += lib_isbasedigit.c lib_memset.c lib_memchr.c lib_memccpy.c 39 | CSRCS += lib_memcmp.c lib_memmove.c lib_skipspace.c lib_stpcpy.c 40 | CSRCS += lib_strcasecmp.c lib_strcat.c lib_strchr.c lib_strcpy.c 41 | CSRCS += lib_strcmp.c lib_strcspn.c lib_strdup.c lib_strerror.c lib_strlen.c 42 | CSRCS += lib_strnlen.c lib_strncasecmp.c lib_strncat.c lib_strncmp.c 43 | CSRCS += lib_strncpy.c lib_strndup.c lib_strcasestr.c lib_strpbrk.c 44 | CSRCS += lib_strrchr.c lib_strspn.c lib_strstr.c lib_strtok.c lib_strtokr.c 45 | 46 | ifneq ($(CONFIG_ARCH_MEMCPY),y) 47 | ifeq ($(CONFIG_MEMCPY_VIK),y) 48 | CSRCS += lib_vikmemcpy.c 49 | else 50 | CSRCS += lib_memcpy.c 51 | endif 52 | endif 53 | 54 | # Add the string directory to the build 55 | 56 | DEPPATH += --dep-path string 57 | VPATH += :string 58 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_memcmp.c: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * libc/string/lib_memcmp.c 3 | * 4 | * Copyright (C) 2007, 2011-2012 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************/ 35 | 36 | /************************************************************ 37 | * Compilation Switches 38 | ************************************************************/ 39 | 40 | /************************************************************ 41 | * Included Files 42 | ************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /************************************************************ 49 | * Global Functions 50 | ************************************************************/ 51 | 52 | #ifndef CONFIG_ARCH_MEMCMP 53 | int memcmp(FAR const void *s1, FAR const void *s2, size_t n) 54 | { 55 | unsigned char *p1 = (unsigned char *)s1; 56 | unsigned char *p2 = (unsigned char *)s2; 57 | 58 | while (n-- > 0) 59 | { 60 | if (*p1 < *p2) 61 | { 62 | return -1; 63 | } 64 | else if (*p1 > *p2) 65 | { 66 | return 1; 67 | } 68 | 69 | p1++; 70 | p2++; 71 | } 72 | return 0; 73 | } 74 | #endif 75 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_memcpy.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_memcpy.c 3 | * 4 | * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Compilation Switches 38 | ****************************************************************************/ 39 | 40 | /**************************************************************************** 41 | * Included Files 42 | ****************************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /**************************************************************************** 49 | * Global Functions 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Name: memcpy 54 | ****************************************************************************/ 55 | 56 | #ifndef CONFIG_ARCH_MEMCPY 57 | FAR void *memcpy(FAR void *dest, FAR const void *src, size_t n) 58 | { 59 | FAR unsigned char *pout = (FAR unsigned char*)dest; 60 | FAR unsigned char *pin = (FAR unsigned char*)src; 61 | while (n-- > 0) *pout++ = *pin++; 62 | return dest; 63 | } 64 | #endif 65 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_memmove.c: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * libc/string/lib_memmove.c 3 | * 4 | * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************/ 35 | 36 | /************************************************************ 37 | * Compilation Switches 38 | ************************************************************/ 39 | 40 | /************************************************************ 41 | * Included Files 42 | ************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /************************************************************ 49 | * Global Functions 50 | ************************************************************/ 51 | 52 | #ifndef CONFIG_ARCH_MEMMOVE 53 | FAR void *memmove(FAR void *dest, FAR const void *src, size_t count) 54 | { 55 | char *tmp, *s; 56 | if (dest <= src) 57 | { 58 | tmp = (char*) dest; 59 | s = (char*) src; 60 | while (count--) 61 | { 62 | *tmp++ = *s++; 63 | } 64 | } 65 | else 66 | { 67 | tmp = (char*) dest + count; 68 | s = (char*) src + count; 69 | while (count--) 70 | { 71 | *--tmp = *--s; 72 | } 73 | } 74 | 75 | return dest; 76 | } 77 | #endif 78 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_skipspace.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_skipspace.c 3 | * 4 | * Copyright (C) 2007, 2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | #include "lib_internal.h" 44 | 45 | #define isspace(c) \ 46 | ((c) == ' ' || (c) == '\t' || (c) == '\n' || \ 47 | (c) == '\r' || (c) == '\f' || c== '\v') 48 | 49 | 50 | /**************************************************************************** 51 | * Private Functions 52 | ****************************************************************************/ 53 | 54 | /**************************************************************************** 55 | * Public Functions 56 | ****************************************************************************/ 57 | 58 | /**************************************************************************** 59 | * Name: lib_skipspace 60 | * 61 | * Description: 62 | * Skip over leading whitespace 63 | * 64 | ****************************************************************************/ 65 | #pragma GCC diagnostic ignored "-Wchar-subscripts" 66 | void lib_skipspace(const char **pptr) 67 | { 68 | const char *ptr = *pptr; 69 | while (isspace(*ptr)) ptr++; 70 | *pptr = ptr; 71 | } 72 | 73 | 74 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_stpcpy.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/string/lib_strppy.c 3 | * 4 | * Copyright (C) 2014 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | /************************************************************************ 45 | * Public Functions 46 | ************************************************************************/ 47 | 48 | /************************************************************************ 49 | * Name: stpcpy 50 | * 51 | * Description: 52 | * Copies the string pointed to by 'src' (including the terminating NUL 53 | * character) into the array pointed to by 'dest'. 54 | * 55 | * Returned value: 56 | * The stpcpy() function returns a pointer to the terminating NUL 57 | * character copied into the 'dest' buffer 58 | * 59 | ************************************************************************/ 60 | 61 | #ifndef CONFIG_ARCH_STPCPY 62 | FAR char *stpcpy(FAR char *dest, FAR const char *src) 63 | { 64 | while ((*dest++ = *src++) != '\0'); 65 | return --dest; 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strcasecmp.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strcasecmp.c 3 | * 4 | * Copyright (C) 2008-2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | *****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | /**************************************************************************** 46 | * Public Functions 47 | *****************************************************************************/ 48 | 49 | #ifndef CONFIG_ARCH_STRCMP 50 | int strcasecmp(const char *cs, const char *ct) 51 | { 52 | int result; 53 | for (;;) 54 | { 55 | if ((result = (int)toupper(*cs) - (int)toupper(*ct)) != 0 || !*cs) 56 | { 57 | break; 58 | } 59 | 60 | cs++; 61 | ct++; 62 | } 63 | return result; 64 | } 65 | #endif 66 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strcat.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strcat.c 3 | * 4 | * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | /**************************************************************************** 45 | * Global Functions 46 | ****************************************************************************/ 47 | 48 | #ifndef CONFIG_ARCH_STRCAT 49 | char *strcat(char *dest, const char *src) 50 | { 51 | char *ret = dest; 52 | 53 | dest += strlen(dest); 54 | while (*src != '\0') 55 | { 56 | *dest++ = *src++; 57 | } 58 | *dest = '\0'; 59 | 60 | return ret; 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strcmp.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strcmp.c 3 | * 4 | * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | *****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | /**************************************************************************** 45 | * Public Functions 46 | *****************************************************************************/ 47 | 48 | #ifndef CONFIG_ARCH_STRCMP 49 | int strcmp(const char *cs, const char *ct) 50 | { 51 | register signed char result; 52 | for (;;) 53 | { 54 | if ((result = *cs - *ct++) != 0 || !*cs++) 55 | break; 56 | } 57 | return result; 58 | } 59 | #endif 60 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strcpy.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/string/lib_strcpy.c 3 | * 4 | * Copyright (C) 2007, 2009, 2011, 2014 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | /************************************************************************ 45 | * Public Functions 46 | ************************************************************************/ 47 | 48 | /************************************************************************ 49 | * Name: strcpy 50 | * 51 | * Description: 52 | * Copies the string pointed to by 'src' (including the terminating NUL 53 | * character) into the array pointed to by 'des'. 54 | * 55 | * Returned value: 56 | * The strcpy() function returns the 'dest' pointer 57 | * 58 | ************************************************************************/ 59 | 60 | #ifndef CONFIG_ARCH_STRCPY 61 | FAR char *strcpy(FAR char *dest, FAR const char *src) 62 | { 63 | char *tmp = dest; 64 | while ((*dest++ = *src++) != '\0'); 65 | return tmp; 66 | } 67 | #endif 68 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strcspn.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strcspn.c 3 | * 4 | * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Compilation Switches 38 | ****************************************************************************/ 39 | 40 | /**************************************************************************** 41 | * Included Files 42 | ****************************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /**************************************************************************** 49 | * Global Functions 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Name: strcspn 54 | * 55 | * Description: 56 | * strspn() calculates the length of the initial segment of s which 57 | * consists entirely of characters not in reject 58 | * 59 | ****************************************************************************/ 60 | 61 | size_t strcspn(const char *s, const char *reject) 62 | { 63 | size_t i; 64 | for (i = 0; s[i] && strchr(reject, s[i]) == NULL; i++); 65 | return i; 66 | } 67 | 68 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strdup.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/string//lib_strdup.c 3 | * 4 | * Copyright (C) 2007, 2009, 2011, 2013 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | #include "lib_internal.h" 45 | 46 | /************************************************************************ 47 | * Global Functions 48 | ************************************************************************/ 49 | 50 | FAR char *strdup(const char *s) 51 | { 52 | FAR char *news = NULL; 53 | if (s) 54 | { 55 | news = (FAR char*)lib_malloc(strlen(s) + 1); 56 | if (news) 57 | { 58 | strcpy(news, s); 59 | } 60 | } 61 | 62 | return news; 63 | } 64 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strlen.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strlen.c 3 | * 4 | * Copyright (C) 2007, 2008, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /**************************************************************************** 45 | * Global Functions 46 | ****************************************************************************/ 47 | 48 | #ifndef CONFIG_ARCH_STRLEN 49 | size_t strlen(const char *s) 50 | { 51 | const char *sc; 52 | for (sc = s; *sc != '\0'; ++sc); 53 | return sc - s; 54 | } 55 | #endif 56 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strncasecmp.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strncasecmp.c 3 | * 4 | * Copyright (C) 2007-2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Compilation Switches 38 | *****************************************************************************/ 39 | 40 | /**************************************************************************** 41 | * Included Files 42 | *****************************************************************************/ 43 | 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | 50 | /**************************************************************************** 51 | * Global Functions 52 | *****************************************************************************/ 53 | 54 | #ifndef CONFIG_ARCH_STRNCASECMP 55 | int strncasecmp(const char *cs, const char *ct, size_t nb) 56 | { 57 | int result = 0; 58 | for (; nb > 0; nb--) 59 | { 60 | if ((result = (int)toupper(*cs) - (int)toupper(*ct)) != 0 || !*cs) 61 | { 62 | break; 63 | } 64 | 65 | cs++; 66 | ct++; 67 | } 68 | return result; 69 | } 70 | #endif 71 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strncat.c: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * libc/string/lib_strncat.c 3 | * 4 | * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************/ 35 | 36 | /************************************************************ 37 | * Included Files 38 | ************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /************************************************************ 45 | * Global Functions 46 | ************************************************************/ 47 | 48 | #ifndef CONFIG_ARCH_STRNCAT 49 | char *strncat(char *dest, const char *src, size_t n) 50 | { 51 | char *ret = dest; 52 | 53 | dest += strlen(dest); 54 | for (; n > 0 && *src != '\0' ; n--) 55 | { 56 | *dest++ = *src++; 57 | } 58 | *dest = '\0'; 59 | 60 | return ret; 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strncmp.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/lib_strncmp.c 3 | * 4 | * Copyright (C) 2007-2009 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | *****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Compilation Switches 38 | *****************************************************************************/ 39 | 40 | /**************************************************************************** 41 | * Included Files 42 | *****************************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /**************************************************************************** 49 | * Global Functions 50 | *****************************************************************************/ 51 | 52 | #ifndef CONFIG_ARCH_STRNCMP 53 | int strncmp(const char *cs, const char *ct, size_t nb) 54 | { 55 | int result = 0; 56 | for (; nb > 0; nb--) 57 | { 58 | if ((result = (int)*cs - (int)*ct++) != 0 || !*cs++) 59 | { 60 | break; 61 | } 62 | } 63 | 64 | return result; 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strncpy.c: -------------------------------------------------------------------------------- 1 | /************************************************************ 2 | * libc/string/lib_strncpy.c 3 | * 4 | * Copyright (C) 2007, 2011, 2014 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************/ 35 | 36 | /************************************************************ 37 | * Included Files 38 | ************************************************************/ 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | /************************************************************ 45 | * Public Functions 46 | ************************************************************/ 47 | 48 | /************************************************************ 49 | * Name: strncpy 50 | ************************************************************/ 51 | 52 | #ifndef CONFIG_ARCH_STRNCPY 53 | char *strncpy(FAR char *dest, FAR const char *src, size_t n) 54 | { 55 | char *ret = dest; /* Value to be returned */ 56 | char *end = dest + n; /* End of dest buffer + 1 byte */ 57 | 58 | while ((dest != end) && (*dest++ = *src++) != '\0'); 59 | while (dest != end) 60 | { 61 | *dest++ = '\0'; 62 | } 63 | 64 | return ret; 65 | } 66 | #endif 67 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strnlen.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strnlen.c 3 | * 4 | * This file is part of NuttX, contributed by Michael Hrabanek 5 | * 6 | * Copyright (C) 2010 Gregory Nutt. All rights reserved. 7 | * Author: Michael Hrabanek 8 | * 9 | * Derives from the file libc/lib_strlen.c: 10 | * 11 | * Copyright (C) 2007, 2008, 2010 Gregory Nutt. All rights reserved. 12 | * Author: Gregory Nutt 13 | * 14 | * Redistribution and use in source and binary forms, with or without 15 | * modification, are permitted provided that the following conditions 16 | * are met: 17 | * 18 | * 1. Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * 2. Redistributions in binary form must reproduce the above copyright 21 | * notice, this list of conditions and the following disclaimer in 22 | * the documentation and/or other materials provided with the 23 | * distribution. 24 | * 3. Neither the name NuttX nor the names of its contributors may be 25 | * used to endorse or promote products derived from this software 26 | * without specific prior written permission. 27 | * 28 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 29 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 30 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 31 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 32 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 33 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 34 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 35 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 36 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 37 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 38 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 39 | * POSSIBILITY OF SUCH DAMAGE. 40 | * 41 | ****************************************************************************/ 42 | 43 | /**************************************************************************** 44 | * Included Files 45 | ****************************************************************************/ 46 | 47 | #include 48 | #include 49 | #include 50 | 51 | /**************************************************************************** 52 | * Global Functions 53 | ****************************************************************************/ 54 | 55 | #ifndef CONFIG_ARCH_STRNLEN 56 | size_t strnlen(const char *s, size_t maxlen) 57 | { 58 | const char *sc; 59 | for (sc = s; maxlen != 0 && *sc != '\0'; maxlen--, ++sc); 60 | return sc - s; 61 | } 62 | #endif 63 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strrchr.c: -------------------------------------------------------------------------------- 1 | /************************************************************************ 2 | * libc/string/lib_strrchr.c 3 | * 4 | * Copyright (C) 2007, 2009, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ************************************************************************/ 35 | 36 | /************************************************************************ 37 | * Included Files 38 | ************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | /************************************************************************ 45 | * Global Functions 46 | ************************************************************************/ 47 | 48 | /* The strrchr() function returns a pointer to the last 49 | * occurrence of the character c in the string s. 50 | */ 51 | 52 | char *strrchr(const char *s, int c) 53 | { 54 | if (s) 55 | { 56 | const char *p = &s[strlen(s) - 1]; 57 | for (; p >= s; p--) 58 | { 59 | if (*p == c) 60 | { 61 | return (char*)p; 62 | } 63 | } 64 | } 65 | 66 | return NULL; 67 | } 68 | 69 | -------------------------------------------------------------------------------- /src/modules/libc/string/lib_strspn.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/string/lib_strspn.c 3 | * 4 | * Copyright (C) 2007, 2011 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Compilation Switches 38 | ****************************************************************************/ 39 | 40 | /**************************************************************************** 41 | * Included Files 42 | ****************************************************************************/ 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | /**************************************************************************** 49 | * Global Functions 50 | ****************************************************************************/ 51 | 52 | /**************************************************************************** 53 | * Name: strspn 54 | * 55 | * Description: 56 | * strspn() calculates the length of the initial segment of s which 57 | * consists entirely of characters in accept. 58 | * 59 | ****************************************************************************/ 60 | 61 | size_t strspn(const char *s, const char *accept) 62 | { 63 | size_t i; 64 | for (i = 0; s[i] && strchr(accept, s[i]) != NULL; i++); 65 | return i; 66 | } 67 | -------------------------------------------------------------------------------- /src/modules/libc/syslog/Make.defs: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # libc/syslog/Make.defs 3 | # 4 | # Copyright (C) 2014 Gregory Nutt. All rights reserved. 5 | # Author: Gregory Nutt 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name NuttX nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | # Add the internal C files to the build 37 | 38 | CSRCS += lib_syslog.c lib_lowsyslog.c lib_setlogmask.c 39 | 40 | ifeq ($(CONFIG_SYSLOG),y) 41 | CSRCS += lib_syslogstream.c 42 | endif 43 | 44 | # Add the syslog directory to the build 45 | 46 | DEPPATH += --dep-path syslog 47 | VPATH += :syslog 48 | -------------------------------------------------------------------------------- /src/modules/libc/time/Make.defs: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # libc/time/Make.defs 3 | # 4 | # Copyright (C) 2011-2012, 2014-2015 Gregory Nutt. All rights reserved. 5 | # Author: Gregory Nutt 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name NuttX nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | # Add the time C files to the build 37 | 38 | CSRCS += lib_strftime.c lib_calendar2utc.c lib_daysbeforemonth.c 39 | CSRCS += lib_gettimeofday.c lib_isleapyear.c lib_settimeofday.c lib_time.c 40 | 41 | ifdef CONFIG_LIBC_LOCALTIME 42 | CSRCS += lib_localtime.c lib_asctime.c lib_asctimer.c lib_ctime.c 43 | CSRCS += lib_ctimer.c 44 | else 45 | CSRCS += lib_mktime.c lib_gmtime.c lib_gmtimer.c 46 | ifdef CONFIG_TIME_EXTENDED 47 | CSRCS += lib_dayofweek.c lib_asctime.c lib_asctimer.c lib_ctime.c 48 | CSRCS += lib_ctimer.c 49 | endif 50 | endif 51 | 52 | # Add the time directory to the build 53 | 54 | DEPPATH += --dep-path time 55 | VPATH += :time 56 | -------------------------------------------------------------------------------- /src/modules/libc/time/lib_asctime.c: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * libc/time/lib_asctime.c 3 | * 4 | * Copyright (C) 2015 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | /**************************************************************************** 37 | * Included Files 38 | ****************************************************************************/ 39 | 40 | #include 41 | 42 | #include 43 | 44 | #ifdef CONFIG_TIME_EXTENDED 45 | 46 | /**************************************************************************** 47 | * Public Functions 48 | ****************************************************************************/ 49 | 50 | /**************************************************************************** 51 | * Function: asctime 52 | * 53 | * Description: 54 | * asctime and asctime_r convert the time provided in a struct tm to a 55 | * string representation. asctime is not re-entrant; asctime_r is re- 56 | * entrant 57 | * 58 | * Parameters: 59 | * tp - Pointer to the time to be converted. 60 | * 61 | * Return Value: 62 | * One success a pointer to the string is returned; on failure, NULL is 63 | * returned. 64 | * 65 | ****************************************************************************/ 66 | 67 | FAR char *asctime(FAR const struct tm *tp) 68 | { 69 | static char buf[26]; 70 | return asctime_r(tp, buf); 71 | } 72 | 73 | #endif /* CONFIG_TIME_EXTENDED */ 74 | -------------------------------------------------------------------------------- /src/modules/libcxx/libstubs.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Pavel Kirienko, 2014 3 | * Standard library stubs 4 | */ 5 | 6 | #include 7 | #include 8 | #include 9 | 10 | #if __GNUC__ 11 | # pragma GCC diagnostic ignored "-Wmissing-declarations" 12 | #endif 13 | 14 | __EXPORT void* __dso_handle; 15 | 16 | __EXPORT void* operator new(size_t) 17 | { 18 | std::abort(); 19 | return reinterpret_cast(0xFFFFFFFF); 20 | } 21 | 22 | __EXPORT void* operator new[](size_t) 23 | { 24 | std::abort(); 25 | return reinterpret_cast(0xFFFFFFFF); 26 | } 27 | 28 | __EXPORT void operator delete(void*) 29 | { 30 | std::abort(); 31 | } 32 | 33 | __EXPORT void operator delete[](void*) 34 | { 35 | std::abort(); 36 | } 37 | 38 | namespace __gnu_cxx 39 | { 40 | 41 | __EXPORT void __verbose_terminate_handler() 42 | { 43 | std::abort(); 44 | } 45 | 46 | } 47 | 48 | /* 49 | * libstdc++ stubs 50 | */ 51 | extern "C" 52 | { 53 | 54 | int __aeabi_atexit(void*, void(*)(void*), void*) 55 | { 56 | return 0; 57 | } 58 | 59 | __extension__ typedef int __guard __attribute__((mode (__DI__))); 60 | 61 | void __cxa_atexit(void(*)(void *), void*, void*) 62 | { 63 | } 64 | 65 | int __cxa_guard_acquire(__guard* g) 66 | { 67 | return !*g; 68 | } 69 | 70 | void __cxa_guard_release (__guard* g) 71 | { 72 | *g = 1; 73 | } 74 | 75 | void __cxa_guard_abort (__guard*) 76 | { 77 | } 78 | 79 | void __cxa_pure_virtual() 80 | { 81 | std::abort(); 82 | } 83 | 84 | } 85 | 86 | /* 87 | * stdio 88 | */ 89 | extern "C" 90 | { 91 | 92 | __attribute__((used)) 93 | void abort() 94 | { 95 | while (true) { } 96 | } 97 | 98 | int _read_r(struct _reent*, int, char*, int) 99 | { 100 | return -1; 101 | } 102 | 103 | int _lseek_r(struct _reent*, int, int, int) 104 | { 105 | return -1; 106 | } 107 | 108 | int _write_r(struct _reent*, int, char*, int) 109 | { 110 | return -1; 111 | } 112 | 113 | int _close_r(struct _reent*, int) 114 | { 115 | return -1; 116 | } 117 | 118 | __attribute__((used)) 119 | caddr_t _sbrk_r(struct _reent*, int) 120 | { 121 | return 0; 122 | } 123 | 124 | int _fstat_r(struct _reent*, int, struct stat*) 125 | { 126 | return -1; 127 | } 128 | 129 | int _isatty_r(struct _reent*, int) 130 | { 131 | return -1; 132 | } 133 | 134 | void _exit(int) 135 | { 136 | abort(); 137 | } 138 | 139 | pid_t _getpid(void) 140 | { 141 | return 1; 142 | } 143 | 144 | void _kill(pid_t) 145 | { 146 | } 147 | 148 | } 149 | -------------------------------------------------------------------------------- /src/modules/libcxx/module.mk: -------------------------------------------------------------------------------- 1 | ############################################################################ 2 | # 3 | # Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. 4 | # Author: Pavel Kirienko 5 | # David Sidrane 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # 1. Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # 2. Redistributions in binary form must reproduce the above copyright 14 | # notice, this list of conditions and the following disclaimer in 15 | # the documentation and/or other materials provided with the 16 | # distribution. 17 | # 3. Neither the name PX4 nor the names of its contributors may be 18 | # used to endorse or promote products derived from this software 19 | # without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | # OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | # AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | ############################################################################ 35 | 36 | DEFAULT_VISIBILITY=default 37 | # Main 38 | SRCS += libstubs.cpp \ 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/1110.LegacyRawSample.uavcan: -------------------------------------------------------------------------------- 1 | # Current time. 2 | # Note that the data type "uavcan.Timestamp" is defined by the UAVCAN specification. 3 | uavcan.Timestamp time 4 | 5 | I2CFrame frame 6 | I2CFrameIntegral integral 7 | -------------------------------------------------------------------------------- /src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrame.uavcan: -------------------------------------------------------------------------------- 1 | # 2 | # Nested Type 3 | # Legacy I2C 4 | # 5 | 6 | uint16 frame_count 7 | int16 pixel_flow_x_sum 8 | int16 pixel_flow_y_sum 9 | int16 flow_comp_m_x 10 | int16 flow_comp_m_y 11 | int16 qual 12 | int16 gyro_x_rate 13 | int16 gyro_y_rate 14 | int16 gyro_z_rate 15 | uint8 gyro_range 16 | uint8 sonarimestamp 17 | int16 ground_distance 18 | -------------------------------------------------------------------------------- /src/modules/uavcannode/dsdl/uavcan/threedr/equipment/flow/optical_flow/I2CFrameIntegral.uavcan: -------------------------------------------------------------------------------- 1 | # 2 | # Nested Type 3 | # Legacy I2C Integral 4 | # 5 | 6 | uint16 frame_count_since_last_readout 7 | int16 pixel_flow_x_integral 8 | int16 pixel_flow_y_integral 9 | int16 gyro_x_rate_integral 10 | int16 gyro_y_rate_integral 11 | int16 gyro_z_rate_integral 12 | uint32 integration_timespan 13 | uint32 sonar_timestamp 14 | uint16 ground_distance 15 | int16 gyro_temperature 16 | uint8 qual 17 | -------------------------------------------------------------------------------- /src/platforms/common/module.mk: -------------------------------------------------------------------------------- 1 | # 2 | # Common OS porting APIs 3 | # 4 | 5 | SRCS = px4_log.c 6 | 7 | -------------------------------------------------------------------------------- /src/platforms/common/px4_log.c: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | __EXPORT int __px4_log_level_current = PX4_LOG_LEVEL_AT_RUN_TIME; 4 | 5 | __EXPORT const char *__px4_log_level_str[_PX4_LOG_LEVEL_DEBUG+1] = { "INFO", "PANIC", "ERROR", "WARN", "DEBUG" }; 6 | -------------------------------------------------------------------------------- /src/platforms/px4_config.h: -------------------------------------------------------------------------------- 1 | /**************************************************************************** 2 | * include/nuttx/compiler.h 3 | * 4 | * Copyright (C) 2007-2009, 2012-2013, 2015 Gregory Nutt. All rights reserved. 5 | * Author: Gregory Nutt 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * 1. Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * 2. Redistributions in binary form must reproduce the above copyright 14 | * notice, this list of conditions and the following disclaimer in 15 | * the documentation and/or other materials provided with the 16 | * distribution. 17 | * 3. Neither the name NuttX nor the names of its contributors may be 18 | * used to endorse or promote products derived from this software 19 | * without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 28 | * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 29 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | * 34 | ****************************************************************************/ 35 | 36 | #pragma once 37 | 38 | /**************************************************************************** 39 | * Included Files 40 | ****************************************************************************/ 41 | /**************************************************************************** 42 | * Pre-processor Definitions 43 | ****************************************************************************/ 44 | #define __LINUX_ERRNO_EXTENSIONS__ 45 | #define PX4_OK 0 46 | #define FAR 47 | #define CONFIG_MEMSET_OPTSPEED 1 48 | -------------------------------------------------------------------------------- /unittests/.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | tests 3 | -------------------------------------------------------------------------------- /unittests/Makefile: -------------------------------------------------------------------------------- 1 | 2 | CC=g++ 3 | CFLAGS=-I. -I../src -lm 4 | 5 | all: tests 6 | 7 | TEST_FILES=../src/sonar_mode_filter.c \ 8 | tests.cpp 9 | 10 | tests: $(TEST_FILES) 11 | $(CC) -o tests $(TEST_FILES) $(CFLAGS) 12 | 13 | .PHONY: clean 14 | 15 | clean: 16 | rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ tests 17 | -------------------------------------------------------------------------------- /unittests/tests.c: -------------------------------------------------------------------------------- 1 | /** 2 | * UNIT TESTS for flow code 3 | */ 4 | 5 | 6 | #include 7 | #include "../inc/sonar_mode_filter.h" 8 | 9 | int main(int argc, char *argv[]) { 10 | 11 | for (unsigned i = 0; i < 25; i++) { 12 | 13 | unsigned in = i; 14 | 15 | if (in % 5 == 0) { 16 | in*= 10; 17 | } 18 | 19 | float inf = in / 10.0f; 20 | 21 | float out = insert_sonar_value_and_get_mode_value(inf); 22 | 23 | printf("in: %f\tout: %f\n", (double)inf, (double)out); 24 | } 25 | 26 | } 27 | --------------------------------------------------------------------------------