forked from Archive/PX4-Autopilot
uavcannode updates and px4_fmu-v4_cannode example
- drivers/uavcannode add baro, mag, gps publications - delete old px4_cannode-v1 board - add stripped down simple rcS for CAN nodes
This commit is contained in:
parent
342e0da796
commit
d7c3e1066a
|
@ -32,20 +32,36 @@ pipeline {
|
|||
"airmind_mindpx-v2_default",
|
||||
"av_x-v1_default",
|
||||
"bitcraze_crazyflie_default",
|
||||
"holybro_kakutef7", "holybro_durandal-v1_default", "holybro_durandal-v1_stackcheck",
|
||||
"holybro_durandal-v1_default",
|
||||
"holybro_durandal-v1_stackcheck",
|
||||
"holybro_kakutef7",
|
||||
"intel_aerofc-v1_default",
|
||||
"modalai_fc-v1_default",
|
||||
"mro_x21_default", "mro_ctrl-zero-f7_default", "mro_x21-777_default",
|
||||
"mro_ctrl-zero-f7_default",
|
||||
"mro_x21-777_default",
|
||||
"mro_x21_default",
|
||||
"nxp_fmuk66-v3_default",
|
||||
"nxp_fmurt1062-v1_default",
|
||||
"nxp_rddrone-uavcan146_default",
|
||||
"omnibus_f4sd_default",
|
||||
"px4_fmu-v2_default", "px4_fmu-v2_fixedwing", "px4_fmu-v2_lpe", "px4_fmu-v2_multicopter", "px4_fmu-v2_rover", "px4_fmu-v2_test",
|
||||
"px4_fmu-v2_default",
|
||||
"px4_fmu-v2_fixedwing",
|
||||
"px4_fmu-v2_lpe",
|
||||
"px4_fmu-v2_multicopter",
|
||||
"px4_fmu-v2_rover",
|
||||
"px4_fmu-v2_test",
|
||||
"px4_fmu-v3_default",
|
||||
"px4_fmu-v4_cannode",
|
||||
"px4_fmu-v4_default",
|
||||
"px4_fmu-v4pro_default",
|
||||
"px4_fmu-v5_default", "px4_fmu-v5_fixedwing", "px4_fmu-v5_multicopter", "px4_fmu-v5_rover", "px4_fmu-v5_rtps", "px4_fmu-v5_stackcheck",
|
||||
"px4_fmu-v5x_default", "px4_fmu-v5x_p2_base_phy_LAN8742Ai",
|
||||
"px4_fmu-v5_default",
|
||||
"px4_fmu-v5_fixedwing",
|
||||
"px4_fmu-v5_multicopter",
|
||||
"px4_fmu-v5_rover",
|
||||
"px4_fmu-v5_rtps",
|
||||
"px4_fmu-v5_stackcheck",
|
||||
"px4_fmu-v5x_default",
|
||||
"px4_fmu-v5x_p2_base_phy_LAN8742Ai",
|
||||
"px4_io-v2_default",
|
||||
"uvify_core_default"
|
||||
],
|
||||
|
|
|
@ -18,18 +18,36 @@ jobs:
|
|||
airmind_mindpx-v2_default,
|
||||
av_x-v1_default,
|
||||
bitcraze_crazyflie_default,
|
||||
holybro_kakutef7, holybro_durandal-v1_default, holybro_durandal-v1_stackcheck,
|
||||
holybro_durandal-v1_default,
|
||||
holybro_durandal-v1_stackcheck,
|
||||
holybro_kakutef7,
|
||||
intel_aerofc-v1_default,
|
||||
modalai_fc-v1_default,
|
||||
mro_x21_default, mro_ctrl-zero-f7_default, mro_x21-777_default,
|
||||
mro_ctrl-zero-f7_default,
|
||||
mro_x21-777_default,
|
||||
mro_x21_default,
|
||||
nxp_fmuk66-v3_default,
|
||||
nxp_fmurt1062-v1_default,
|
||||
nxp_rddrone-uavcan146_default,
|
||||
omnibus_f4sd_default,
|
||||
px4_fmu-v2_default, px4_fmu-v2_fixedwing, px4_fmu-v2_lpe, px4_fmu-v2_multicopter, px4_fmu-v2_rover, px4_fmu-v2_test,
|
||||
px4_fmu-v2_default,
|
||||
px4_fmu-v2_fixedwing,
|
||||
px4_fmu-v2_lpe,
|
||||
px4_fmu-v2_multicopter,
|
||||
px4_fmu-v2_rover,
|
||||
px4_fmu-v2_test,
|
||||
px4_fmu-v3_default,
|
||||
px4_fmu-v4_cannode,
|
||||
px4_fmu-v4_default,
|
||||
px4_fmu-v4pro_default,
|
||||
px4_fmu-v5_default, px4_fmu-v5_fixedwing, px4_fmu-v5_multicopter, px4_fmu-v5_rover, px4_fmu-v5_rtps, px4_fmu-v5_stackcheck,
|
||||
px4_fmu-v5_default,
|
||||
px4_fmu-v5_fixedwing,
|
||||
px4_fmu-v5_multicopter,
|
||||
px4_fmu-v5_rover,
|
||||
px4_fmu-v5_rtps,
|
||||
px4_fmu-v5_stackcheck,
|
||||
px4_fmu-v5x_default,
|
||||
px4_fmu-v5x_p2_base_phy_LAN8742Ai,
|
||||
px4_io-v2_default,
|
||||
uvify_core_default
|
||||
]
|
||||
|
|
|
@ -42,10 +42,6 @@
|
|||
path = platforms/nuttx/NuttX/apps
|
||||
url = https://github.com/PX4/NuttX-apps.git
|
||||
branch = px4_firmware_nuttx-8.2
|
||||
[submodule "cmake/configs/uavcan_board_ident"]
|
||||
path = cmake/configs/uavcan_board_ident
|
||||
url = https://github.com/PX4/uavcan_board_ident.git
|
||||
branch = master
|
||||
[submodule "platforms/qurt/dspal"]
|
||||
path = platforms/qurt/dspal
|
||||
url = https://github.com/ATLFlight/dspal.git
|
||||
|
|
9
Makefile
9
Makefile
|
@ -230,7 +230,7 @@ excelsior_rtps: atlflight_excelsior_rtps atlflight_excelsior_qurt-rtps
|
|||
# Other targets
|
||||
# --------------------------------------------------------------------
|
||||
|
||||
.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware alt_firmware check_rtps
|
||||
.PHONY: qgc_firmware px4fmu_firmware misc_qgc_extra_firmware check_rtps
|
||||
|
||||
# QGroundControl flashable NuttX firmware
|
||||
qgc_firmware: px4fmu_firmware misc_qgc_extra_firmware
|
||||
|
@ -256,11 +256,6 @@ misc_qgc_extra_firmware: \
|
|||
check_px4_fmu-v2_lpe \
|
||||
sizes
|
||||
|
||||
# Other NuttX firmware
|
||||
alt_firmware: \
|
||||
check_px4_cannode-v1_default \
|
||||
sizes
|
||||
|
||||
# builds with RTPS
|
||||
check_rtps: \
|
||||
check_px4_fmu-v3_rtps \
|
||||
|
@ -275,7 +270,7 @@ sizes:
|
|||
@-find build -name *.elf -type f | xargs size 2> /dev/null || :
|
||||
|
||||
# All default targets that don't require a special build environment
|
||||
check: check_px4_sitl_default px4fmu_firmware misc_qgc_extra_firmware alt_firmware tests check_format
|
||||
check: check_px4_sitl_default px4fmu_firmware misc_qgc_extra_firmware tests check_format
|
||||
|
||||
# quick_check builds a single nuttx and SITL target, runs testing, and checks the style
|
||||
quick_check: check_px4_sitl_test check_px4_fmu-v5_default tests check_format
|
||||
|
|
|
@ -0,0 +1,34 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
# are met:
|
||||
#
|
||||
# 1. Redistributions of source code must retain the above copyright
|
||||
# notice, this list of conditions and the following disclaimer.
|
||||
# 2. Redistributions in binary form must reproduce the above copyright
|
||||
# notice, this list of conditions and the following disclaimer in
|
||||
# the documentation and/or other materials provided with the
|
||||
# distribution.
|
||||
# 3. Neither the name PX4 nor the names of its contributors may be
|
||||
# used to endorse or promote products derived from this software
|
||||
# without specific prior written permission.
|
||||
#
|
||||
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
# POSSIBILITY OF SUCH DAMAGE.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
add_subdirectory(init.d)
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
@ -31,18 +31,6 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
add_library(drivers_board
|
||||
buttons.c
|
||||
can.c
|
||||
init.c
|
||||
led.c
|
||||
spi.c
|
||||
px4_add_romfs_files(
|
||||
rcS
|
||||
)
|
||||
|
||||
target_link_libraries(drivers_board
|
||||
PRIVATE
|
||||
nuttx_arch # sdio
|
||||
nuttx_drivers # sdio
|
||||
drivers__led # drv_led_start
|
||||
px4_layer
|
||||
)
|
|
@ -0,0 +1,106 @@
|
|||
#!/bin/sh
|
||||
# Un comment and use set +e to ignore and set -e to enable 'exit on error control'
|
||||
set +e
|
||||
# Un comment the line below to help debug scripts by printing a trace of the script commands
|
||||
#set -x
|
||||
# PX4FMU startup script.
|
||||
#
|
||||
# NOTE: environment variable references:
|
||||
# If the dollar sign ('$') is followed by a left bracket ('{') then the
|
||||
# variable name is terminated with the right bracket character ('}').
|
||||
# Otherwise, the variable name goes to the end of the argument.
|
||||
#
|
||||
#
|
||||
# NOTE: COMMENT LINES ARE REMOVED BEFORE STORED IN ROMFS.
|
||||
#
|
||||
#------------------------------------------------------------------------------
|
||||
|
||||
#
|
||||
# Mount the procfs.
|
||||
#
|
||||
mount -t procfs /proc
|
||||
|
||||
#
|
||||
# Start CDC/ACM serial driver.
|
||||
#
|
||||
sercon
|
||||
|
||||
#
|
||||
# Print full system version.
|
||||
#
|
||||
ver all
|
||||
|
||||
#
|
||||
# Start the ORB (first app to start)
|
||||
# tone_alarm and tune_control
|
||||
# is dependent.
|
||||
#
|
||||
uorb start
|
||||
|
||||
#
|
||||
# Set the parameter file if mtd starts successfully.
|
||||
#
|
||||
if mtd start
|
||||
then
|
||||
set PARAM_FILE /fs/mtd_params
|
||||
fi
|
||||
|
||||
#
|
||||
# Load parameters.
|
||||
#
|
||||
param select $PARAM_FILE
|
||||
if ! param load
|
||||
then
|
||||
param reset
|
||||
fi
|
||||
|
||||
#
|
||||
# Optional board defaults: rc.board_defaults
|
||||
#
|
||||
set BOARD_RC_DEFAULTS /etc/init.d/rc.board_defaults
|
||||
if [ -f $BOARD_RC_DEFAULTS ]
|
||||
then
|
||||
echo "Board defaults: ${BOARD_RC_DEFAULTS}"
|
||||
sh $BOARD_RC_DEFAULTS
|
||||
fi
|
||||
unset BOARD_RC_DEFAULTS
|
||||
|
||||
#
|
||||
# Start system state indicator.
|
||||
#
|
||||
rgbled start
|
||||
rgbled_ncp5623c start
|
||||
rgbled_pwm start
|
||||
|
||||
if param greater LIGHT_EN_BLINKM 0
|
||||
then
|
||||
if blinkm start
|
||||
then
|
||||
blinkm systemstate
|
||||
fi
|
||||
fi
|
||||
|
||||
#
|
||||
# board sensors: rc.sensors
|
||||
#
|
||||
set BOARD_RC_SENSORS /etc/init.d/rc.board_sensors
|
||||
if [ -f $BOARD_RC_SENSORS ]
|
||||
then
|
||||
echo "Board sensors: ${BOARD_RC_SENSORS}"
|
||||
sh $BOARD_RC_SENSORS
|
||||
fi
|
||||
unset BOARD_RC_SENSORS
|
||||
|
||||
#
|
||||
# Start UART/Serial device drivers.
|
||||
# Note: rc.serial is auto-generated from Tools/serial/generate_config.py
|
||||
#
|
||||
sh /etc/init.d/rc.serial
|
||||
|
||||
# Check for flow sensor, launched as a background task to scan
|
||||
if param compare SENS_EN_PX4FLOW 1
|
||||
then
|
||||
px4flow start &
|
||||
fi
|
||||
|
||||
uavcannode start
|
|
@ -1,3 +1,23 @@
|
|||
|
||||
|
||||
# UAVCAN boot loadable Module ID
|
||||
set(uavcanblid_sw_version_major 0)
|
||||
set(uavcanblid_sw_version_minor 1)
|
||||
add_definitions(
|
||||
-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
|
||||
-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
|
||||
)
|
||||
|
||||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"org.nxp.rddrone-uavcan146\"")
|
||||
|
||||
add_definitions(
|
||||
-DHW_UAVCAN_NAME=${uavcanblid_name}
|
||||
-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
|
||||
-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
|
||||
)
|
||||
|
||||
px4_add_board(
|
||||
PLATFORM nuttx
|
||||
VENDOR nxp
|
||||
|
@ -5,15 +25,51 @@ px4_add_board(
|
|||
LABEL default
|
||||
TOOLCHAIN arm-none-eabi
|
||||
ARCHITECTURE cortex-m4
|
||||
|
||||
ROMFSROOT cannode
|
||||
UAVCAN_INTERFACES 2
|
||||
|
||||
DRIVERS
|
||||
|
||||
#adc
|
||||
#barometer # all available barometer drivers
|
||||
#bootloaders
|
||||
#differential_pressure # all available differential pressure drivers
|
||||
#distance_sensor # all available distance sensor drivers
|
||||
#dshot
|
||||
#gps
|
||||
#imu # all available imu drivers
|
||||
#lights
|
||||
#magnetometer # all available magnetometer drivers
|
||||
#optical_flow # all available optical flow drivers
|
||||
#px4fmu
|
||||
#safety_button
|
||||
#tone_alarm
|
||||
#uavcannode # TODO: CAN driver needed
|
||||
MODULES
|
||||
|
||||
#ekf2
|
||||
#load_mon
|
||||
#sensors
|
||||
#temperature_compensation
|
||||
SYSTEMCMDS
|
||||
#bl_update
|
||||
#dmesg
|
||||
#dumpfile
|
||||
#esc_calib
|
||||
#hardfault_log
|
||||
i2cdetect
|
||||
|
||||
EXAMPLES
|
||||
)
|
||||
#led_control
|
||||
#mixer
|
||||
#motor_ramp
|
||||
#motor_test
|
||||
#nshterm
|
||||
#param
|
||||
#perf
|
||||
#pwm
|
||||
reboot
|
||||
#reflect
|
||||
#sd_bench
|
||||
shutdown
|
||||
#top
|
||||
#topic_listener
|
||||
#tune_control
|
||||
ver
|
||||
#work_queue
|
||||
)
|
||||
|
|
|
@ -1,57 +0,0 @@
|
|||
|
||||
add_definitions(
|
||||
-DPARAM_NO_ORB
|
||||
-DPARAM_NO_AUTOSAVE
|
||||
)
|
||||
|
||||
# UAVCAN boot loadable Module ID
|
||||
set(uavcanblid_sw_version_major 0)
|
||||
set(uavcanblid_sw_version_minor 1)
|
||||
add_definitions(
|
||||
-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
|
||||
-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
|
||||
)
|
||||
|
||||
# Bring in common uavcan hardware identity definitions
|
||||
include(px4_git)
|
||||
px4_add_git_submodule(TARGET git_uavcan_board_ident PATH "cmake/configs/uavcan_board_ident")
|
||||
include(configs/uavcan_board_ident/px4cannode-v1)
|
||||
|
||||
add_definitions(
|
||||
-DHW_UAVCAN_NAME=${uavcanblid_name}
|
||||
-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
|
||||
-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
|
||||
)
|
||||
|
||||
include(px4_make_uavcan_bootloader)
|
||||
px4_make_uavcan_bootloadable(
|
||||
BOARD px4_cannode-v1
|
||||
BIN ${PX4_BINARY_DIR}/px4_cannode-v1.bin
|
||||
HWNAME ${uavcanblid_name}
|
||||
HW_MAJOR ${uavcanblid_hw_version_major}
|
||||
HW_MINOR ${uavcanblid_hw_version_minor}
|
||||
SW_MAJOR ${uavcanblid_sw_version_major}
|
||||
SW_MINOR ${uavcanblid_sw_version_minor}
|
||||
)
|
||||
|
||||
px4_add_board(
|
||||
PLATFORM nuttx
|
||||
VENDOR px4
|
||||
MODEL cannode-v1
|
||||
TOOLCHAIN arm-none-eabi
|
||||
ARCHITECTURE cortex-m3
|
||||
|
||||
DRIVERS
|
||||
bootloaders
|
||||
uavcannode
|
||||
|
||||
MODULES
|
||||
|
||||
SYSTEMCMDS
|
||||
config
|
||||
reboot
|
||||
top
|
||||
ver
|
||||
work_queue
|
||||
|
||||
)
|
|
@ -1,13 +0,0 @@
|
|||
{
|
||||
"board_id": 22,
|
||||
"magic": "CANNODEFWv1",
|
||||
"description": "Firmware for the PX4CANNODE board",
|
||||
"image": "",
|
||||
"build_time": 0,
|
||||
"summary": "PX4CANNODEv1",
|
||||
"version": "0.1",
|
||||
"image_size": 0,
|
||||
"image_maxsize": 122880,
|
||||
"git_identity": "",
|
||||
"board_revision": 0
|
||||
}
|
|
@ -1,22 +0,0 @@
|
|||
#
|
||||
# For a description of the syntax of this configuration file,
|
||||
# see misc/tools/kconfig-language.txt.
|
||||
#
|
||||
|
||||
if CONFIG_ARCH_BOARD_PX4_CANNODE_V1
|
||||
|
||||
config BOARD_HAS_PROBES
|
||||
bool "Board provides GPIO or other Hardware for signaling to timing analyze."
|
||||
default y
|
||||
---help---
|
||||
This board provides GPIO D0-D2,A0-A3 as PROBE_1-6 to provide timing signals from selected drivers.
|
||||
|
||||
config BOARD_USE_PROBES
|
||||
bool "Enable the use provides GPIO D0-D2,A0-A3 as PROBE_1-6 to provide timing signals from selected drivers"
|
||||
default n
|
||||
depends on BOARD_HAS_PROBES
|
||||
|
||||
---help---
|
||||
Select to use GPIO provides GPIO D0-D2,A0-A3 to provide timing signals from selected drivers.
|
||||
|
||||
endif
|
|
@ -1,6 +0,0 @@
|
|||
This directory contains header files unique to the
|
||||
PX4 Can Node V1 board
|
||||
Currently this is a demo on the OLIMEXINO-STM32
|
||||
|
||||
https://www.olimex.com/Products/Duino/STM32/OLIMEXINO-STM32/resources/OLIMEXINO-STM32.pdf
|
||||
http://www.mouser.com/ProductDetail/Olimex-Ltd/OLIMEXINO-STM32/?qs=sGAEpiMZZMsUcx5t7XFI3Z5HrEKlvGsZ
|
|
@ -1,259 +0,0 @@
|
|||
/************************************************************************************
|
||||
* nuttx-configs/px4_cannode-v1/include/board.h
|
||||
*
|
||||
* Copyright (C) 2015 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name NuttX nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef __CONFIGS_PX4CANNODE_V1_INCLUDE_BOARD_H
|
||||
#define __CONFIGS_PX4CANNODE_V1_INCLUDE_BOARD_H
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
# include <stdint.h>
|
||||
#endif
|
||||
#include "stm32_rcc.h"
|
||||
#include "stm32_sdio.h"
|
||||
#include "stm32.h"
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
/* Clocking *************************************************************************/
|
||||
|
||||
/* HSI - 8 MHz RC factory-trimmed
|
||||
* LSI - 40 KHz RC (30-60KHz, uncalibrated)
|
||||
* HSE - On-board crystal frequency is 8MHz
|
||||
* LSE - 32.768 kHz
|
||||
*/
|
||||
|
||||
#define STM32_BOARD_XTAL 8000000ul
|
||||
|
||||
#define STM32_HSI_FREQUENCY 8000000ul
|
||||
#define STM32_LSI_FREQUENCY 40000
|
||||
#define STM32_HSE_FREQUENCY STM32_BOARD_XTAL
|
||||
#define STM32_LSE_FREQUENCY 32768
|
||||
|
||||
/* PLL source is HSE/1, PLL multipler is 9: PLL frequency is 8MHz (XTAL) x 9 = 72MHz */
|
||||
|
||||
#define STM32_CFGR_PLLSRC RCC_CFGR_PLLSRC
|
||||
#define STM32_CFGR_PLLXTPRE 0
|
||||
#define STM32_CFGR_PLLMUL RCC_CFGR_PLLMUL_CLKx9
|
||||
#define STM32_PLL_FREQUENCY (9*STM32_BOARD_XTAL)
|
||||
|
||||
/* Use the PLL and set the SYSCLK source to be the PLL */
|
||||
|
||||
#define STM32_SYSCLK_SW RCC_CFGR_SW_PLL
|
||||
#define STM32_SYSCLK_SWS RCC_CFGR_SWS_PLL
|
||||
#define STM32_SYSCLK_FREQUENCY STM32_PLL_FREQUENCY
|
||||
|
||||
/* AHB clock (HCLK) is SYSCLK (72MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_HPRE RCC_CFGR_HPRE_SYSCLK
|
||||
#define STM32_HCLK_FREQUENCY STM32_PLL_FREQUENCY
|
||||
#define STM32_BOARD_HCLK STM32_HCLK_FREQUENCY /* same as above, to satisfy compiler */
|
||||
|
||||
/* APB2 clock (PCLK2) is HCLK (72MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_PPRE2 RCC_CFGR_PPRE2_HCLK
|
||||
#define STM32_PCLK2_FREQUENCY STM32_HCLK_FREQUENCY
|
||||
|
||||
/* APB2 timers 1 and 8 will receive PCLK2. */
|
||||
|
||||
#define STM32_APB2_TIM1_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
#define STM32_APB2_TIM8_CLKIN (STM32_PCLK2_FREQUENCY)
|
||||
|
||||
/* APB1 clock (PCLK1) is HCLK/2 (36MHz) */
|
||||
|
||||
#define STM32_RCC_CFGR_PPRE1 RCC_CFGR_PPRE1_HCLKd2
|
||||
#define STM32_PCLK1_FREQUENCY (STM32_HCLK_FREQUENCY/2)
|
||||
|
||||
/* APB1 timers 2-4 will be twice PCLK1 */
|
||||
|
||||
#define STM32_APB1_TIM2_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM3_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM4_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM5_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM6_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
#define STM32_APB1_TIM7_CLKIN (2*STM32_PCLK1_FREQUENCY)
|
||||
|
||||
/* Timer Frequencies, if APBx is set to 1, frequency is same to APBx
|
||||
* otherwise frequency is 2xAPBx.
|
||||
* Note: TIM1,8 are on APB2, others on APB1
|
||||
*/
|
||||
|
||||
#define BOARD_TIM1_FREQUENCY STM32_APB2_TIM1_CLKIN
|
||||
#define BOARD_TIM2_FREQUENCY STM32_APB1_TIM2_CLKIN
|
||||
#define BOARD_TIM3_FREQUENCY STM32_APB1_TIM3_CLKIN
|
||||
#define BOARD_TIM4_FREQUENCY STM32_APB1_TIM4_CLKIN
|
||||
#define BOARD_TIM5_FREQUENCY STM32_APB1_TIM5_CLKIN
|
||||
#define BOARD_TIM6_FREQUENCY STM32_APB1_TIM6_CLKIN
|
||||
#define BOARD_TIM7_FREQUENCY STM32_APB1_TIM7_CLKIN
|
||||
#define BOARD_TIM8_FREQUENCY STM32_APB2_TIM8_CLKIN
|
||||
|
||||
/* USB divider -- Divide PLL clock by 1.5 */
|
||||
|
||||
#define STM32_CFGR_USBPRE 0
|
||||
|
||||
|
||||
/* Buttons *************************************************************************/
|
||||
|
||||
#define BUTTON_BOOT0_BIT (0)
|
||||
#define BUTTON_BOOT0_MASK (1<<BUTTON_BOOT0_BIT)
|
||||
|
||||
/* Leds *************************************************************************/
|
||||
|
||||
/* LED index values for use with board_setled() */
|
||||
|
||||
#define BOARD_LED1 0
|
||||
#define BOARD_LED_GREEN BOARD_LED1
|
||||
#define BOARD_LED2 1
|
||||
#define BOARD_LED_YELLOW BOARD_LED2
|
||||
#define BOARD_NLEDS 2
|
||||
|
||||
/* LED bits for use with board_setleds() */
|
||||
|
||||
#define BOARD_LED_GREEN_BIT (1 << BOARD_LED_GREEN)
|
||||
#define BOARD_LED_YELLOW_BIT (1 << BOARD_LED_YELLOW)
|
||||
|
||||
/* These LEDs are not used by the board port unless CONFIG_ARCH_LEDS is
|
||||
* defined. In that case, the usage by the board port is as follows:
|
||||
*
|
||||
* SYMBOL Meaning LED state
|
||||
* Green Yellow
|
||||
* ------------------------ -------------------------- ------ ------ */
|
||||
|
||||
#define LED_STARTED 0 /* NuttX has been started OFF OFF */
|
||||
#define LED_HEAPALLOCATE 1 /* Heap has been allocated OFF OFF */
|
||||
#define LED_IRQSENABLED 2 /* Interrupts enabled OFF OFF */
|
||||
#define LED_STACKCREATED 3 /* Idle stack created ON OFF */
|
||||
#define LED_INIRQ 4 /* In an interrupt N/C ON */
|
||||
#define LED_SIGNAL 5 /* In a signal handler N/C ON */
|
||||
#define LED_ASSERTION 6 /* An assertion failed N/C ON */
|
||||
#define LED_PANIC 7 /* The system has crashed N/C Blinking */
|
||||
#define LED_IDLE 8 /* MCU is is sleep mode OFF N/C */
|
||||
|
||||
/* Thus if the Green is statically on, NuttX has successfully booted and is,
|
||||
* apparently, running normally. If the YellowLED is flashing at
|
||||
* approximately 2Hz, then a fatal error has been detected and the system
|
||||
* has halted.
|
||||
*
|
||||
* NOTE: That the Yellow is not used after completion of booting and may
|
||||
* be used by other board-specific logic.
|
||||
*/
|
||||
|
||||
|
||||
#if defined(CONFIG_BOARD_USE_PROBES)
|
||||
# define PROBE_N(n) (1<<((n)-1))
|
||||
# define PROBE_1 (GPIO_OUTPUT|GPIO_CNF_OUTPP | GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN1)
|
||||
# define PROBE_2 (GPIO_OUTPUT|GPIO_CNF_OUTPP | GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN2)
|
||||
# define PROBE_3 (GPIO_OUTPUT|GPIO_CNF_OUTPP | GPIO_MODE_50MHz|GPIO_OUTPUT_CLEAR|GPIO_PORTC|GPIO_PIN3)
|
||||
|
||||
# define PROBE_INIT(mask) \
|
||||
do { \
|
||||
if ((mask)& PROBE_N(1)) { stm32_configgpio(PROBE_1); } \
|
||||
if ((mask)& PROBE_N(2)) { stm32_configgpio(PROBE_2); } \
|
||||
if ((mask)& PROBE_N(3)) { stm32_configgpio(PROBE_3); } \
|
||||
} while(0)
|
||||
|
||||
# define PROBE(n,s) do {stm32_gpiowrite(PROBE_##n,(s));}while(0)
|
||||
# define PROBE_MARK(n) PROBE(n,false);PROBE(n,true)
|
||||
#else
|
||||
# define PROBE_INIT(mask)
|
||||
# define PROBE(n,s)
|
||||
# define PROBE_MARK(n)
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Public Data
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
#undef EXTERN
|
||||
#if defined(__cplusplus)
|
||||
#define EXTERN extern "C"
|
||||
extern "C"
|
||||
{
|
||||
#else
|
||||
#define EXTERN extern
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Public Function Prototypes
|
||||
************************************************************************************/
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the initialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void stm32_boardinitialize(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_ledinit, stm32_setled, and stm32_setleds
|
||||
*
|
||||
* Description:
|
||||
* If CONFIG_ARCH_LEDS is defined, then NuttX will control the on-board LEDs. If
|
||||
* CONFIG_ARCH_LEDS is not defined, then the following interfaces are available to
|
||||
* control the LEDs from user applications.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef CONFIG_ARCH_LEDS
|
||||
void stm32_led_initialize(void);
|
||||
void stm32_setled(int led, bool ledon);
|
||||
void stm32_setleds(uint8_t ledset);
|
||||
#endif
|
||||
|
||||
#if !defined(CONFIG_NSH_LIBRARY)
|
||||
int app_archinitialize(void);
|
||||
#else
|
||||
#define app_archinitialize() (-ENOSYS)
|
||||
#endif
|
||||
|
||||
#undef EXTERN
|
||||
#if defined(__cplusplus)
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
#endif /* __CONFIGS_PX4CANNODE_V1_INCLUDE_BOARD_H */
|
|
@ -1,105 +0,0 @@
|
|||
#
|
||||
# This file is autogenerated: PLEASE DO NOT EDIT IT.
|
||||
#
|
||||
# You can use "make menuconfig" to make any modifications to the installed .config file.
|
||||
# You can then do "make savedefconfig" to generate a new defconfig file that includes your
|
||||
# modifications.
|
||||
#
|
||||
# CONFIG_DEV_NULL is not set
|
||||
# CONFIG_DISABLE_OS_API is not set
|
||||
# CONFIG_DISABLE_PSEUDOFS_OPERATIONS is not set
|
||||
CONFIG_ARCH="arm"
|
||||
CONFIG_ARCH_BOARD_CUSTOM=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR="../nuttx-config"
|
||||
CONFIG_ARCH_BOARD_CUSTOM_DIR_RELPATH=y
|
||||
CONFIG_ARCH_BOARD_CUSTOM_NAME="px4"
|
||||
CONFIG_ARCH_CHIP="stm32"
|
||||
CONFIG_ARCH_CHIP_STM32=y
|
||||
CONFIG_ARCH_CHIP_STM32F103RB=y
|
||||
CONFIG_ARCH_INTERRUPTSTACK=360
|
||||
CONFIG_ARCH_STACKDUMP=y
|
||||
CONFIG_ARMV7M_USEBASEPRI=y
|
||||
CONFIG_BOARDCTL_RESET=y
|
||||
CONFIG_BOARD_CUSTOM_BUTTONS=y
|
||||
CONFIG_BOARD_CUSTOM_IRQBUTTONS=y
|
||||
CONFIG_BOARD_CUSTOM_LEDS=y
|
||||
CONFIG_BOARD_LOOPSPERMSEC=5483
|
||||
CONFIG_BUILTIN=y
|
||||
CONFIG_BUILTIN_PROXY_STACKSIZE=392
|
||||
CONFIG_C99_BOOL8=y
|
||||
CONFIG_CLOCK_MONOTONIC=y
|
||||
CONFIG_DEBUG_FULLOPT=y
|
||||
CONFIG_DEBUG_HARDFAULT_ALERT=y
|
||||
CONFIG_DEBUG_SYMBOLS=y
|
||||
CONFIG_DEFAULT_SMALL=y
|
||||
CONFIG_EXAMPLES_NULL=y
|
||||
CONFIG_FDCLONE_STDIO=y
|
||||
CONFIG_HAVE_CXX=y
|
||||
CONFIG_HAVE_CXXINITIALIZE=y
|
||||
CONFIG_IDLETHREAD_STACKSIZE=272
|
||||
CONFIG_LIBC_FLOATINGPOINT=y
|
||||
CONFIG_LIBC_LONG_LONG=y
|
||||
CONFIG_LIB_BOARDCTL=y
|
||||
CONFIG_LIB_SENDFILE_BUFSIZE=0
|
||||
CONFIG_MAX_TASKS=4
|
||||
CONFIG_MAX_WDOGPARMS=2
|
||||
CONFIG_MEMSET_64BIT=y
|
||||
CONFIG_MEMSET_OPTSPEED=y
|
||||
CONFIG_MM_SMALL=y
|
||||
CONFIG_MQ_MAXMSGSIZE=8
|
||||
CONFIG_NAME_MAX=8
|
||||
CONFIG_NFILE_DESCRIPTORS=5
|
||||
CONFIG_NFILE_STREAMS=1
|
||||
CONFIG_NPTHREAD_KEYS=0
|
||||
CONFIG_NXFONTS_DISABLE_16BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_1BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_24BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_2BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_32BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_4BPP=y
|
||||
CONFIG_NXFONTS_DISABLE_8BPP=y
|
||||
CONFIG_POSIX_SPAWN_PROXY_STACKSIZE=394
|
||||
CONFIG_PREALLOC_MQ_MSGS=4
|
||||
CONFIG_PREALLOC_TIMERS=2
|
||||
CONFIG_PREALLOC_WDOGS=4
|
||||
CONFIG_PRIORITY_INHERITANCE=y
|
||||
CONFIG_PTHREAD_MUTEX_ROBUST=y
|
||||
CONFIG_PTHREAD_STACK_DEFAULT=516
|
||||
CONFIG_PTHREAD_STACK_MIN=516
|
||||
CONFIG_RAM_SIZE=20480
|
||||
CONFIG_RAM_START=0x20000000
|
||||
CONFIG_RAW_BINARY=y
|
||||
CONFIG_SCHED_ATEXIT=y
|
||||
CONFIG_SCHED_INSTRUMENTATION=y
|
||||
CONFIG_SCHED_WAITPID=y
|
||||
CONFIG_SDCLONE_DISABLE=y
|
||||
CONFIG_SEM_NNESTPRIO=4
|
||||
CONFIG_SEM_PREALLOCHOLDERS=4
|
||||
CONFIG_SIG_DEFAULT=y
|
||||
CONFIG_SIG_SIGALRM_ACTION=y
|
||||
CONFIG_SIG_SIGUSR1_ACTION=y
|
||||
CONFIG_SIG_SIGUSR2_ACTION=y
|
||||
CONFIG_STACK_COLORATION=y
|
||||
CONFIG_START_DAY=20
|
||||
CONFIG_START_MONTH=11
|
||||
CONFIG_STM32_DISABLE_IDLE_SLEEP_DURING_DEBUG=y
|
||||
CONFIG_STM32_DMA1=y
|
||||
CONFIG_STM32_DMA2=y
|
||||
CONFIG_STM32_FORCEPOWER=y
|
||||
CONFIG_STM32_JTAG_FULL_ENABLE=y
|
||||
CONFIG_STM32_PWR=y
|
||||
CONFIG_STM32_SERIAL_DISABLE_REORDERING=y
|
||||
CONFIG_STM32_TIM3=y
|
||||
CONFIG_STM32_TIM3_FULL_REMAP=y
|
||||
CONFIG_STM32_USART1=y
|
||||
CONFIG_SYMTAB_ORDEREDBYNAME=y
|
||||
CONFIG_SYSTEM_READLINE=y
|
||||
CONFIG_TASK_NAME_SIZE=12
|
||||
CONFIG_TASK_SPAWN_DEFAULT_STACKSIZE=394
|
||||
CONFIG_USART1_RXBUFSIZE=32
|
||||
CONFIG_USART1_RXDMA=y
|
||||
CONFIG_USART1_SERIAL_CONSOLE=y
|
||||
CONFIG_USART1_TXBUFSIZE=32
|
||||
CONFIG_USERMAIN_STACKSIZE=1048
|
||||
CONFIG_USER_ENTRYPOINT="uavcannode_start"
|
||||
CONFIG_WDOG_INTRESERVE=2
|
|
@ -1,153 +0,0 @@
|
|||
/****************************************************************************
|
||||
* nuttx-configs/px4_cannode-v1/scripts/ld.script
|
||||
*
|
||||
* Copyright (C) 2015 Gregory Nutt. All rights reserved.
|
||||
* Author: Gregory Nutt <gnutt@nuttx.org>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name NuttX nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/* The STM32F103RB has 128KiB of FLASH beginning at address 0x0800:0000 and
|
||||
* 20KiB of SRAM beginning at address 0x2000:0000. When booting from FLASH,
|
||||
* FLASH memory is aliased to address 0x0000:0000 where the code expects to
|
||||
* begin execution by jumping to the entry point in the 0x0800:0000 address
|
||||
* range.
|
||||
*
|
||||
* The first 8KiB of flash is reserved for the bootloader.
|
||||
*/
|
||||
|
||||
MEMORY
|
||||
{
|
||||
flash (rx) : ORIGIN = 0x08002000, LENGTH = 120K
|
||||
sram (rwx) : ORIGIN = 0x20000000, LENGTH = 20K
|
||||
}
|
||||
|
||||
OUTPUT_ARCH(arm)
|
||||
|
||||
ENTRY(__start) /* treat __start as the anchor for dead code stripping */
|
||||
EXTERN(_vectors) /* force the vectors to be included in the output */
|
||||
/*
|
||||
* Ensure that abort() is present in the final object. The exception handling
|
||||
* code pulled in by libgcc.a requires it (and that code cannot be easily avoided).
|
||||
*/
|
||||
EXTERN(abort)
|
||||
|
||||
SECTIONS
|
||||
{
|
||||
.text : {
|
||||
_stext = ABSOLUTE(.);
|
||||
*(.vectors)
|
||||
. = ALIGN(8);
|
||||
/*
|
||||
* This section positions the app_descriptor_t used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc
|
||||
*/
|
||||
KEEP(*(.app_descriptor))
|
||||
*(.text .text.*)
|
||||
*(.fixup)
|
||||
*(.gnu.warning)
|
||||
*(.rodata .rodata.*)
|
||||
*(.gnu.linkonce.t.*)
|
||||
*(.got)
|
||||
*(.gcc_except_table)
|
||||
*(.gnu.linkonce.r.*)
|
||||
_etext = ABSOLUTE(.);
|
||||
/*
|
||||
* This is a hack to make the newlib libm __errno() call
|
||||
* use the NuttX get_errno_ptr() function.
|
||||
*/
|
||||
__errno = get_errno_ptr;
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Init functions (static constructors and the like)
|
||||
*/
|
||||
.init_section : {
|
||||
_sinit = ABSOLUTE(.);
|
||||
KEEP(*(.init_array .init_array.*))
|
||||
_einit = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
/*
|
||||
* Construction data for parameters.
|
||||
*/
|
||||
__param ALIGN(4): {
|
||||
__param_start = ABSOLUTE(.);
|
||||
KEEP(*(__param))
|
||||
__param_end = ABSOLUTE(.);
|
||||
} > flash
|
||||
|
||||
.ARM.extab : {
|
||||
*(.ARM.extab*)
|
||||
} > flash
|
||||
|
||||
__exidx_start = ABSOLUTE(.);
|
||||
.ARM.exidx : {
|
||||
*(.ARM.exidx*)
|
||||
} > flash
|
||||
__exidx_end = ABSOLUTE(.);
|
||||
|
||||
_eronly = ABSOLUTE(.);
|
||||
|
||||
/* The STM32F103CB has 20Kb of SRAM beginning at the following address */
|
||||
|
||||
.data : {
|
||||
_sdata = ABSOLUTE(.);
|
||||
*(.data .data.*)
|
||||
*(.gnu.linkonce.d.*)
|
||||
CONSTRUCTORS
|
||||
_edata = ABSOLUTE(.);
|
||||
} > sram AT > flash
|
||||
|
||||
.bss : {
|
||||
_sbss = ABSOLUTE(.);
|
||||
*(.bss .bss.*)
|
||||
*(.gnu.linkonce.b.*)
|
||||
*(COMMON)
|
||||
. = ALIGN(4);
|
||||
_ebss = ABSOLUTE(.);
|
||||
} > sram
|
||||
|
||||
/* Stabs debugging sections. */
|
||||
.stab 0 : { *(.stab) }
|
||||
.stabstr 0 : { *(.stabstr) }
|
||||
.stab.excl 0 : { *(.stab.excl) }
|
||||
.stab.exclstr 0 : { *(.stab.exclstr) }
|
||||
.stab.index 0 : { *(.stab.index) }
|
||||
.stab.indexstr 0 : { *(.stab.indexstr) }
|
||||
.comment 0 : { *(.comment) }
|
||||
.debug_abbrev 0 : { *(.debug_abbrev) }
|
||||
.debug_info 0 : { *(.debug_info) }
|
||||
.debug_line 0 : { *(.debug_line) }
|
||||
.debug_pubnames 0 : { *(.debug_pubnames) }
|
||||
.debug_aranges 0 : { *(.debug_aranges) }
|
||||
}
|
|
@ -1,281 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file board_config.h
|
||||
*
|
||||
* PX4CANNODEv1 internal definitions
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
/****************************************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdint.h>
|
||||
|
||||
/************************************************************************************
|
||||
* Definitions
|
||||
************************************************************************************/
|
||||
|
||||
#if STM32_NSPI < 1
|
||||
# undef CONFIG_STM32_SPI1
|
||||
# undef CONFIG_STM32_SPI2
|
||||
#elif STM32_NSPI < 2
|
||||
# undef CONFIG_STM32_SPI2
|
||||
#endif
|
||||
|
||||
/* High-resolution timer
|
||||
*/
|
||||
#define HRT_TIMER 1 /* use timer1 for the HRT */
|
||||
#define HRT_TIMER_CHANNEL 1 /* use capture/compare channel */
|
||||
#define HRT_PPM_CHANNEL 3 /* use capture/compare channel 3 */
|
||||
#define GPIO_PPM_IN (GPIO_ALT|GPIO_CNF_INPULLUP|GPIO_PORTB|GPIO_PIN12)
|
||||
|
||||
/* LEDs *****************************************************************************
|
||||
*
|
||||
* GPIO Function MPU Board
|
||||
* Pin # Name
|
||||
* -- ----- -------------------------------- ----------------------------
|
||||
*
|
||||
* PA[05] PA5/SPI1_SCK/ADC5 21 D13(SCK1/LED1)
|
||||
* PA[01] PA1/USART2_RTS/ADC1/TIM2_CH2 15 D3(LED2)
|
||||
*/
|
||||
|
||||
#define GPIO_LED1 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTA | GPIO_PIN5 | GPIO_OUTPUT_CLEAR)
|
||||
#define GPIO_LED_GREEN GPIO_LED1
|
||||
#define GPIO_LED2 (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTA | GPIO_PIN1 | GPIO_OUTPUT_CLEAR)
|
||||
#define GPIO_LED_YELLOW GPIO_LED2
|
||||
|
||||
/* BUTTON ***************************************************************************
|
||||
*
|
||||
* GPIO Function MPU Board
|
||||
* Pin # Name
|
||||
* -- ----- -------------------------------- ----------------------------
|
||||
*
|
||||
* PC[09] PC9/TIM3_CH4 40 BOOT0
|
||||
*
|
||||
*/
|
||||
|
||||
#define BUTTON_BOOT0n (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN9 | GPIO_EXTI)
|
||||
#define IRQBUTTON BUTTON_BOOT0_BIT
|
||||
|
||||
/* USBs *****************************************************************************
|
||||
*
|
||||
* GPIO Function MPU Board
|
||||
* Pin # Name
|
||||
* -- ----- -------------------------------- ----------------------------
|
||||
*
|
||||
* PC[11] PC11/USART3_RX 52 USB_P
|
||||
* PC[12] PC12/USART3_CK 53 DISC
|
||||
*
|
||||
*/
|
||||
|
||||
#define GPIO_USB_VBUS (GPIO_INPUT | GPIO_CNF_INFLOAT | GPIO_PORTC | GPIO_PIN11)
|
||||
#define GPIO_USB_PULLUPn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTC | GPIO_PIN12 | GPIO_OUTPUT_SET)
|
||||
|
||||
/* SPI ***************************************************************************
|
||||
*
|
||||
* GPIO Function MPU Board
|
||||
* Pin # Name
|
||||
* -- ----- -------------------------------- ----------------------------
|
||||
*
|
||||
* PC[09] PA4/SPI1_NSS/USART2_CK/ADC4 20 D10(#SS1)
|
||||
* PD[02] PD2/TIM3_ETR 54 D25(MMC_CS)
|
||||
*/
|
||||
|
||||
#define GPIO_SPI1_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTC | GPIO_PIN9 | GPIO_OUTPUT_SET)
|
||||
#define USER_CSn GPIO_SPI1_SSn
|
||||
|
||||
#define GPIO_SPI2_SSn (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTD | GPIO_PIN2 | GPIO_OUTPUT_SET)
|
||||
#define MMCSD_CSn GPIO_SPI2_SSn
|
||||
|
||||
/* CAN ***************************************************************************
|
||||
*
|
||||
* GPIO Function MPU Board
|
||||
* Pin # Name
|
||||
* -- ----- -------------------------------- ----------------------------
|
||||
*
|
||||
* PB[08] PB8/TIM4_CH3/I2C1_SCL/CANRX 61 D14(CANRX)
|
||||
* PB[09] PB9/TIM4_CH4/I2C1_SDA/CANTX 62 D24(CANTX)
|
||||
* PC[13] PC13/ANTI_TAMP 2 D21(CAN_CTRL)
|
||||
*/
|
||||
|
||||
#define GPIO_CAN_CTRL (GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_PORTC | GPIO_PIN13 | GPIO_OUTPUT_CLEAR)
|
||||
|
||||
__BEGIN_DECLS
|
||||
|
||||
/************************************************************************************
|
||||
* Public Types
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public data
|
||||
************************************************************************************/
|
||||
|
||||
#ifndef __ASSEMBLY__
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \
|
||||
defined(CONFIG_STM32_SPI3)
|
||||
void weak_function board_spiinitialize(void);
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usbinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to setup USB-related GPIO pins.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void stm32_usbinitialize(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_usb_set_pwr_callback()
|
||||
*
|
||||
* Description:
|
||||
* Called to setup set a call back for USB power state changes.
|
||||
*
|
||||
* Inputs:
|
||||
* pwr_changed_handler: An interrupt handler that will be called on VBUS power
|
||||
* state changes.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void stm32_usb_set_pwr_callback(xcpt_t pwr_changed_handler);
|
||||
|
||||
/****************************************************************************
|
||||
* Name: stm32_led_initialize
|
||||
*
|
||||
* Description:
|
||||
* This functions is called very early in initialization to perform board-
|
||||
* specific initialization of LED-related resources. This includes such
|
||||
* things as, for example, configure GPIO pins to drive the LEDs and also
|
||||
* putting the LEDs in their correct initial state.
|
||||
*
|
||||
* NOTE: In most architectures, LED initialization() is called from
|
||||
* board-specific initialization and should, therefore, have the name
|
||||
* <arch>_led_intialize(). But there are a few architectures where the
|
||||
* LED initialization function is still called from common chip
|
||||
* architecture logic. This interface is not, however, a common board
|
||||
* interface in any event and the name board_autoled_initialization is
|
||||
* deprecated.
|
||||
*
|
||||
* Input Parameters:
|
||||
* None
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_ARCH_LEDS
|
||||
void board_autoled_initialize(void);
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_can_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called at application startup time to initialize the CAN functionality.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#if defined(CONFIG_CAN) && (defined(CONFIG_STM32_CAN1) || defined(CONFIG_STM32_CAN2))
|
||||
int board_can_initialize(void);
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Name: board_button_initialize
|
||||
*
|
||||
* Description:
|
||||
* Called at application startup time to initialize the Buttons functionality.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
#if defined(CONFIG_ARCH_BUTTONS)
|
||||
void board_button_initialize(void);
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Name: usbmsc_archinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called from the application system/usbmc or the boards_nsh if the
|
||||
* application is not included.
|
||||
* Perform architecture specific initialization. This function must
|
||||
* configure the block device to export via USB. This function must be
|
||||
* provided by architecture-specific logic in order to use this add-on.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_USBMSC)
|
||||
int usbmsc_archinitialize(void);
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Name: composite_archinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called from the application system/composite or the boards_nsh if the
|
||||
* application is not included.
|
||||
* Perform architecture specific initialization. This function must
|
||||
* configure the block device to export via USB. This function must be
|
||||
* provided by architecture-specific logic in order to use this add-on.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#if !defined(CONFIG_NSH_BUILTIN_APPS) && !defined(CONFIG_SYSTEM_COMPOSITE)
|
||||
extern int composite_archinitialize(void);
|
||||
#endif
|
||||
|
||||
#include <px4_platform_common/board_common.h>
|
||||
|
||||
#endif /* __ASSEMBLY__ */
|
||||
|
||||
__END_DECLS
|
|
@ -1,152 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_cannode_buttons.c
|
||||
*
|
||||
* PX4CANNODE- Buttons
|
||||
*/
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
#include <nuttx/board.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
#ifdef CONFIG_ARCH_BUTTONS
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
void board_button_initialize(void);
|
||||
/****************************************************************************
|
||||
* Button support.
|
||||
*
|
||||
* Description:
|
||||
* board_button_initialize() must be called to initialize button resources.
|
||||
*
|
||||
* After board_button_initialize() has been called, board_buttons() may be
|
||||
* called to collect the state of all buttons. board_buttons() returns an
|
||||
* 8-bit bit set with each bit associated with a button.
|
||||
* See the BUTTON_*_BIT definitions in board.h for the meaning of each bit.
|
||||
*
|
||||
* board_button_irq() may be called to register an interrupt handler that
|
||||
* will be called when a button is depressed or released. The ID value is
|
||||
* a button enumeration value that uniquely identifies a button resource.
|
||||
* See the BUTTON_* definitions in board.h for the meaning of enumeration
|
||||
* value. The previous interrupt handler address is returned
|
||||
* (so that it may restored, if so desired).
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_button_initialize
|
||||
*
|
||||
* Description:
|
||||
* board_button_initialize() must be called to initialize button resources.
|
||||
* After that, board_buttons() may be called to collect the current state of
|
||||
* all buttons or board_button_irq() may be called to register button
|
||||
* interrupt handlers.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
void board_button_initialize(void)
|
||||
{
|
||||
stm32_configgpio(BUTTON_BOOT0n);
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_buttons
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* After board_button_initialize() has been called, board_buttons() may be
|
||||
* called to collect the state of all buttons. board_buttons() returns an
|
||||
* 8-bit bit set with each bit associated with a button.
|
||||
* See the BUTTON_*_BIT definitions in board.h for the meaning of each bit.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
uint32_t board_buttons(void)
|
||||
{
|
||||
return stm32_gpioread(BUTTON_BOOT0n) ? 0 : BUTTON_BOOT0_MASK;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_button_irq
|
||||
*
|
||||
* Description:
|
||||
*
|
||||
* board_button_irq() may be called to register an interrupt handler that
|
||||
* will be called when a button is depressed or released. The ID value is
|
||||
* a button enumeration value that uniquely identifies a button resource.
|
||||
* See the BUTTON_* definitions in board.h for the meaning of enumeration
|
||||
* value. The previous interrupt handler address is returned
|
||||
* (so that it may restored, if so desired).
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_ARCH_IRQBUTTONS
|
||||
int board_button_irq(int id, xcpt_t irqhandler, FAR void *arg)
|
||||
{
|
||||
int ret = -EINVAL;
|
||||
|
||||
/* The following should be atomic */
|
||||
|
||||
if (id == IRQBUTTON) {
|
||||
ret = stm32_gpiosetevent(BUTTON_BOOT0n, true, true, true, irqhandler, arg);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
#endif
|
||||
#endif /* CONFIG_ARCH_BUTTONS */
|
|
@ -1,131 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file pxesc_can.c
|
||||
*
|
||||
* Board-specific CAN functions.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/can/can.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include "chip.h"
|
||||
#include "up_arch.h"
|
||||
|
||||
#include "stm32.h"
|
||||
#include "stm32_can.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#if defined(CONFIG_CAN)
|
||||
|
||||
/************************************************************************************
|
||||
* Pre-processor Definitions
|
||||
************************************************************************************/
|
||||
/* Configuration ********************************************************************/
|
||||
/* The STM32F107VC supports CAN1 and CAN2 */
|
||||
|
||||
#if defined(CONFIG_STM32_CAN1) && defined(CONFIG_STM32_CAN2)
|
||||
# warning "Both CAN1 and CAN2 are enabled. Assuming only CAN1."
|
||||
# undef CONFIG_STM32_CAN2
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_CAN1
|
||||
# define CAN_PORT 1
|
||||
#else
|
||||
# define CAN_PORT 2
|
||||
#endif
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
int can_devinit(void);
|
||||
|
||||
/************************************************************************************
|
||||
* Name: can_devinit
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following interface to work with
|
||||
* examples/can.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT int can_devinit(void)
|
||||
{
|
||||
static bool initialized = false;
|
||||
struct can_dev_s *can;
|
||||
int ret;
|
||||
|
||||
/* Check if we have already initialized */
|
||||
|
||||
if (!initialized) {
|
||||
/* Call stm32_caninitialize() to get an instance of the CAN interface */
|
||||
|
||||
can = stm32_caninitialize(CAN_PORT);
|
||||
|
||||
if (can == NULL) {
|
||||
canerr("ERROR: Failed to get CAN interface\n");
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
/* Register the CAN driver at "/dev/can0" */
|
||||
|
||||
ret = can_register("/dev/can0", can);
|
||||
|
||||
if (ret < 0) {
|
||||
canerr("ERROR: can_register failed: %d\n", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
/* Now we are initialized */
|
||||
|
||||
initialized = true;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
#endif /* CONFIG_CAN && CONFIG_STM32_CAN1 */
|
|
@ -1,182 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_cannode_init.c
|
||||
*
|
||||
* PX4CANNODE-specific early startup code. This file implements the
|
||||
* board_app_initialize() function that is called early by nsh during startup.
|
||||
*
|
||||
* Code here is run before the rcS script is invoked; it should start required
|
||||
* subsystems and perform board-specific initialization.
|
||||
*/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdio.h>
|
||||
#include <debug.h>
|
||||
#include <errno.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <nuttx/i2c/i2c_master.h>
|
||||
#include <nuttx/mmcsd.h>
|
||||
#include <nuttx/analog/adc.h>
|
||||
|
||||
#include <stm32.h>
|
||||
#include "board_config.h"
|
||||
#include "stm32_uart.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_board_led.h>
|
||||
|
||||
#include <px4_platform_common/init.h>
|
||||
|
||||
#if defined(CONFIG_HAVE_CXX) && defined(CONFIG_HAVE_CXXINITIALIZE)
|
||||
#endif
|
||||
|
||||
#include "board_config.h"
|
||||
|
||||
/* todo: This is constant but not proper */
|
||||
__BEGIN_DECLS
|
||||
extern void led_off(int led);
|
||||
__END_DECLS
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Pre-Processor Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/* Configuration ************************************************************/
|
||||
|
||||
/* Debug ********************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Protected Functions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_boardinitialize
|
||||
*
|
||||
* Description:
|
||||
* All STM32 architectures must provide the following entry point. This entry point
|
||||
* is called early in the initialization -- after all memory has been configured
|
||||
* and mapped but before any devices have been initialized.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
__EXPORT void stm32_boardinitialize(void)
|
||||
{
|
||||
/* configure LEDs */
|
||||
board_autoled_initialize();
|
||||
board_button_initialize();
|
||||
stm32_configgpio(GPIO_CAN_CTRL);
|
||||
|
||||
/* configure CAN interface */
|
||||
|
||||
stm32_configgpio(GPIO_CAN1_RX);
|
||||
stm32_configgpio(GPIO_CAN1_TX);
|
||||
|
||||
#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || \
|
||||
defined(CONFIG_STM32_SPI3)
|
||||
board_spiinitialize();
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
__EXPORT void board_initialize(void)
|
||||
{
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_app_initialize
|
||||
*
|
||||
* Description:
|
||||
* Perform application specific initialization. This function is never
|
||||
* called directly from application code, but only indirectly via the
|
||||
* (non-standard) boardctl() interface using the command BOARDIOC_INIT.
|
||||
*
|
||||
* Input Parameters:
|
||||
* arg - The boardctl() argument is passed to the board_app_initialize()
|
||||
* implementation without modification. The argument has no
|
||||
* meaning to NuttX; the meaning of the argument is a contract
|
||||
* between the board-specific initalization logic and the the
|
||||
* matching application logic. The value cold be such things as a
|
||||
* mode enumeration value, a set of DIP switch switch settings, a
|
||||
* pointer to configuration data read from a file or serial FLASH,
|
||||
* or whatever you would like to do with it. Every implementation
|
||||
* should accept zero/NULL as a default configuration.
|
||||
*
|
||||
* Returned Value:
|
||||
* Zero (OK) is returned on success; a negated errno value is returned on
|
||||
* any failure to indicate the nature of the failure.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
__EXPORT int board_app_initialize(uintptr_t arg)
|
||||
{
|
||||
int result = OK;
|
||||
|
||||
px4_platform_init();
|
||||
|
||||
/* set up the serial DMA polling */
|
||||
static struct hrt_call serial_dma_call;
|
||||
struct timespec ts;
|
||||
|
||||
/*
|
||||
* Poll at 1ms intervals for received bytes that have not triggered
|
||||
* a DMA event.
|
||||
*/
|
||||
ts.tv_sec = 0;
|
||||
ts.tv_nsec = 1000000;
|
||||
|
||||
hrt_call_every(&serial_dma_call,
|
||||
ts_to_abstime(&ts),
|
||||
ts_to_abstime(&ts),
|
||||
(hrt_callout)stm32_serial_dma_poll,
|
||||
NULL);
|
||||
|
||||
return result;
|
||||
}
|
|
@ -1,174 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_cannode_led.c
|
||||
*
|
||||
* PX4ESC LED backend.
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <nuttx/board.h>
|
||||
|
||||
#include "stm32.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <systemlib/px4_macros.h>
|
||||
|
||||
/*
|
||||
* Ideally we'd be able to get these from up_internal.h,
|
||||
* but since we want to be able to disable the NuttX use
|
||||
* of leds for system indication at will and there is no
|
||||
* separate switch, we need to build independent of the
|
||||
* CONFIG_ARCH_LEDS configuration switch.
|
||||
*/
|
||||
__BEGIN_DECLS
|
||||
extern void led_init(void);
|
||||
extern void led_on(int led);
|
||||
extern void led_off(int led);
|
||||
extern void led_toggle(int led);
|
||||
__END_DECLS
|
||||
|
||||
static uint16_t g_ledmap[] = {
|
||||
GPIO_LED_GREEN, // Indexed by BOARD_LED_GREEN
|
||||
GPIO_LED_YELLOW, // Indexed by BOARD_LED_YELLOW
|
||||
};
|
||||
|
||||
__EXPORT void led_init(void)
|
||||
{
|
||||
/* Configure LED1-2 GPIOs for output */
|
||||
for (size_t l = 0; l < arraySize(g_ledmap); l++) {
|
||||
stm32_configgpio(g_ledmap[l]);
|
||||
}
|
||||
}
|
||||
|
||||
__EXPORT void board_autoled_initialize(void)
|
||||
{
|
||||
led_init();
|
||||
}
|
||||
|
||||
static void phy_set_led(int led, bool state)
|
||||
{
|
||||
/* Pull Up to switch on */
|
||||
stm32_gpiowrite(g_ledmap[led], state);
|
||||
}
|
||||
|
||||
static bool phy_get_led(int led)
|
||||
{
|
||||
|
||||
return !stm32_gpioread(g_ledmap[led]);
|
||||
}
|
||||
|
||||
__EXPORT void led_on(int led)
|
||||
{
|
||||
phy_set_led(led, true);
|
||||
}
|
||||
|
||||
__EXPORT void led_off(int led)
|
||||
{
|
||||
phy_set_led(led, false);
|
||||
}
|
||||
|
||||
__EXPORT void led_toggle(int led)
|
||||
{
|
||||
|
||||
phy_set_led(led, !phy_get_led(led));
|
||||
}
|
||||
|
||||
static bool g_initialized;
|
||||
|
||||
// Nuttx Usages
|
||||
|
||||
__EXPORT void board_autoled_on(int led)
|
||||
{
|
||||
switch (led) {
|
||||
default:
|
||||
case LED_STARTED:
|
||||
case LED_HEAPALLOCATE:
|
||||
case LED_IRQSENABLED:
|
||||
phy_set_led(BOARD_LED_GREEN, false);
|
||||
phy_set_led(BOARD_LED_YELLOW, false);
|
||||
break;
|
||||
|
||||
case LED_STACKCREATED:
|
||||
phy_set_led(BOARD_LED_GREEN, true);
|
||||
phy_set_led(BOARD_LED_YELLOW, false);
|
||||
g_initialized = true;
|
||||
break;
|
||||
|
||||
case LED_INIRQ:
|
||||
case LED_SIGNAL:
|
||||
case LED_ASSERTION:
|
||||
case LED_PANIC:
|
||||
phy_set_led(BOARD_LED_YELLOW, true);
|
||||
break;
|
||||
|
||||
case LED_IDLE : /* IDLE */
|
||||
phy_set_led(BOARD_LED_GREEN, false);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: board_autoled_off
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
__EXPORT void board_autoled_off(int led)
|
||||
{
|
||||
switch (led) {
|
||||
default:
|
||||
case LED_STARTED:
|
||||
case LED_HEAPALLOCATE:
|
||||
case LED_IRQSENABLED:
|
||||
case LED_STACKCREATED:
|
||||
phy_set_led(BOARD_LED_GREEN, false);
|
||||
|
||||
/* FALLTHROUGH */
|
||||
|
||||
case LED_INIRQ:
|
||||
case LED_SIGNAL:
|
||||
case LED_ASSERTION:
|
||||
case LED_PANIC:
|
||||
phy_set_led(BOARD_LED_YELLOW, false);
|
||||
break;
|
||||
|
||||
case LED_IDLE: /* IDLE */
|
||||
phy_set_led(BOARD_LED_GREEN, g_initialized);
|
||||
break;
|
||||
}
|
||||
}
|
|
@ -1,216 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file px4_cannode_led.c
|
||||
*
|
||||
* PX4FMU SPI backend.
|
||||
*/
|
||||
|
||||
/************************************************************************************
|
||||
* Included Files
|
||||
************************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
#include <errno.h>
|
||||
#include <debug.h>
|
||||
|
||||
#include <nuttx/spi/spi.h>
|
||||
#include <arch/board/board.h>
|
||||
|
||||
#include <nuttx/board.h>
|
||||
#include "chip.h"
|
||||
#include "stm32.h"
|
||||
#include "board_config.h"
|
||||
|
||||
#if defined(CONFIG_STM32_SPI1) || defined(CONFIG_STM32_SPI2) || defined(CONFIG_STM32_SPI3)
|
||||
|
||||
/************************************************************************************
|
||||
* Definitions
|
||||
************************************************************************************/
|
||||
|
||||
|
||||
/************************************************************************************
|
||||
* Private Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Public Functions
|
||||
************************************************************************************/
|
||||
|
||||
/************************************************************************************
|
||||
* Name: stm32_spiinitialize
|
||||
*
|
||||
* Description:
|
||||
* Called to configure SPI chip select GPIO pins for the board.
|
||||
*
|
||||
************************************************************************************/
|
||||
|
||||
void weak_function board_spiinitialize(void)
|
||||
{
|
||||
/* Setup CS */
|
||||
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
stm32_configgpio(USER_CSn);
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
stm32_configgpio(MMCSD_CSn);
|
||||
#endif
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: stm32_spi1/2/3select and stm32_spi1/2/3status
|
||||
*
|
||||
* Description:
|
||||
* The external functions, stm32_spi1/2/3select and stm32_spi1/2/3status
|
||||
* must be provided by board-specific logic. They are implementations of
|
||||
* the select and status methods of the SPI interface defined by struct
|
||||
* spi_ops_s (see include/nuttx/spi/spi.h). All other methods (including
|
||||
* stm32_spibus_initialize()) are provided by common STM32 logic. To use
|
||||
* this common SPI logic on your board:
|
||||
*
|
||||
* 1. Provide logic in stm32_boardinitialize() to configure SPI chip
|
||||
* select pins.
|
||||
* 2. Provide stm32_spi1/2/3select() and stm32_spi1/2/3status() functions
|
||||
* in your board-specific logic. These functions will perform chip
|
||||
* selection and status operations using GPIOs in the way your board
|
||||
* is configured.
|
||||
* 3. Add a calls to stm32_spibus_initialize() in your low level application
|
||||
* initialization logic
|
||||
* 4. The handle returned by stm32_spibus_initialize() may then be used to
|
||||
* bind the SPI driver to higher level logic (e.g., calling
|
||||
* mmcsd_spislotinitialize(), for example, will bind the SPI driver to
|
||||
* the SPI MMC/SD driver).
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
void stm32_spi1select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
|
||||
|
||||
if (devid == SPIDEV_USER) {
|
||||
stm32_gpiowrite(USER_CSn, !selected);
|
||||
}
|
||||
}
|
||||
|
||||
uint8_t stm32_spi1status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
void stm32_spi2select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
|
||||
#if defined(CONFIG_MMCSD)
|
||||
|
||||
if (devid == SPIDEV_MMCSD) {
|
||||
stm32_gpiowrite(MMCSD_CSn, !selected);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
uint8_t stm32_spi2status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
/* No switch on SD card socket so assume it is here */
|
||||
|
||||
return SPI_STATUS_PRESENT;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI3
|
||||
void stm32_spi3select(FAR struct spi_dev_s *dev, uint32_t devid, bool selected)
|
||||
{
|
||||
spiinfo("devid: %d CS: %s\n", (int)devid, selected ? "assert" : "de-assert");
|
||||
}
|
||||
|
||||
uint8_t stm32_spi3status(FAR struct spi_dev_s *dev, uint32_t devid)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
/****************************************************************************
|
||||
* Name: stm32_spi1cmddata
|
||||
*
|
||||
* Description:
|
||||
* Set or clear the SH1101A A0 or SD1306 D/C n bit to select data (true)
|
||||
* or command (false). This function must be provided by platform-specific
|
||||
* logic. This is an implementation of the cmddata method of the SPI
|
||||
* interface defined by struct spi_ops_s (see include/nuttx/spi/spi.h).
|
||||
*
|
||||
* Input Parameters:
|
||||
*
|
||||
* spi - SPI device that controls the bus the device that requires the CMD/
|
||||
* DATA selection.
|
||||
* devid - If there are multiple devices on the bus, this selects which one
|
||||
* to select cmd or data. NOTE: This design restricts, for example,
|
||||
* one one SPI display per SPI bus.
|
||||
* cmd - true: select command; false: select data
|
||||
*
|
||||
* Returned Value:
|
||||
* None
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifdef CONFIG_SPI_CMDDATA
|
||||
#ifdef CONFIG_STM32_SPI1
|
||||
int stm32_spi1cmddata(FAR struct spi_dev_s *dev, uint32_t devid, bool cmd)
|
||||
{
|
||||
return OK;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI2
|
||||
int stm32_spi2cmddata(FAR struct spi_dev_s *dev, uint32_t devid, bool cmd)
|
||||
{
|
||||
return OK;
|
||||
}
|
||||
#endif
|
||||
|
||||
#ifdef CONFIG_STM32_SPI3
|
||||
int stm32_spi3cmddata(FAR struct spi_dev_s *dev, uint32_t devid, bool cmd)
|
||||
{
|
||||
return -ENODEV;
|
||||
}
|
||||
#endif
|
||||
#endif /* CONFIG_SPI_CMDDATA */
|
||||
|
||||
#endif /* CONFIG_STM32_SPI1 || CONFIG_STM32_SPI2 */
|
|
@ -0,0 +1,100 @@
|
|||
|
||||
|
||||
# UAVCAN boot loadable Module ID
|
||||
set(uavcanblid_sw_version_major 0)
|
||||
set(uavcanblid_sw_version_minor 1)
|
||||
add_definitions(
|
||||
-DAPP_VERSION_MAJOR=${uavcanblid_sw_version_major}
|
||||
-DAPP_VERSION_MINOR=${uavcanblid_sw_version_minor}
|
||||
)
|
||||
|
||||
set(uavcanblid_hw_version_major 1)
|
||||
set(uavcanblid_hw_version_minor 0)
|
||||
set(uavcanblid_name "\"org.px4.fmu-v4_cannode\"")
|
||||
|
||||
add_definitions(
|
||||
-DHW_UAVCAN_NAME=${uavcanblid_name}
|
||||
-DHW_VERSION_MAJOR=${uavcanblid_hw_version_major}
|
||||
-DHW_VERSION_MINOR=${uavcanblid_hw_version_minor}
|
||||
)
|
||||
|
||||
px4_add_board(
|
||||
PLATFORM nuttx
|
||||
VENDOR px4
|
||||
MODEL fmu-v4
|
||||
LABEL cannode
|
||||
TOOLCHAIN arm-none-eabi
|
||||
ARCHITECTURE cortex-m4
|
||||
ROMFSROOT cannode
|
||||
UAVCAN_INTERFACES 1
|
||||
SERIAL_PORTS
|
||||
GPS1:/dev/ttyS3
|
||||
TEL1:/dev/ttyS1
|
||||
TEL2:/dev/ttyS2
|
||||
DRIVERS
|
||||
adc
|
||||
#barometer # all available barometer drivers
|
||||
barometer/ms5611
|
||||
bootloaders
|
||||
#differential_pressure # all available differential pressure drivers
|
||||
#distance_sensor # all available distance sensor drivers
|
||||
#dshot
|
||||
gps
|
||||
#imu # all available imu drivers
|
||||
#imu/adis16448
|
||||
#imu/adis16477
|
||||
#imu/adis16497
|
||||
#imu/invensense/icm20602
|
||||
#imu/invensense/icm20608-g
|
||||
imu/mpu6000
|
||||
imu/mpu9250
|
||||
#lights/rgbled
|
||||
#lights/rgbled_ncp5623c
|
||||
#magnetometer # all available magnetometer drivers
|
||||
#optical_flow # all available optical flow drivers
|
||||
#px4fmu
|
||||
#safety_button
|
||||
#tone_alarm
|
||||
uavcannode
|
||||
MODULES
|
||||
#ekf2
|
||||
#load_mon
|
||||
#sensors
|
||||
#temperature_compensation
|
||||
SYSTEMCMDS
|
||||
#bl_update
|
||||
#dmesg
|
||||
#dumpfile
|
||||
#esc_calib
|
||||
#hardfault_log
|
||||
i2cdetect
|
||||
#led_control
|
||||
#mixer
|
||||
#motor_ramp
|
||||
#motor_test
|
||||
mtd
|
||||
#nshterm
|
||||
param
|
||||
perf
|
||||
#pwm
|
||||
reboot
|
||||
#reflect
|
||||
#sd_bench
|
||||
#shutdown
|
||||
top
|
||||
#topic_listener
|
||||
#tune_control
|
||||
ver
|
||||
work_queue
|
||||
)
|
||||
|
||||
include(px4_make_uavcan_bootloader)
|
||||
px4_make_uavcan_bootloadable(
|
||||
BOARD ${PX4_BOARD}
|
||||
BIN ${PX4_BINARY_DIR}/${PX4_BOARD}.bin
|
||||
HWNAME ${uavcanblid_name}
|
||||
HW_MAJOR ${uavcanblid_hw_version_major}
|
||||
HW_MINOR ${uavcanblid_hw_version_minor}
|
||||
SW_MAJOR ${uavcanblid_sw_version_major}
|
||||
SW_MINOR ${uavcanblid_sw_version_minor}
|
||||
)
|
|
@ -46,4 +46,5 @@ target_link_libraries(drivers_board
|
|||
nuttx_arch # sdio
|
||||
nuttx_drivers # sdio
|
||||
px4_layer
|
||||
arch_io_pins
|
||||
)
|
||||
|
|
|
@ -1 +0,0 @@
|
|||
Subproject commit 2e5f9d6768b1dbffc006dc2ceeb2bfe120f22163
|
|
@ -66,9 +66,7 @@ void px4_log_initialize(void)
|
|||
log_message.severity = 6; //info
|
||||
strcpy((char *)log_message.text, "initialized uORB logging");
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
orb_log_message_pub = orb_advertise_queue(ORB_ID(log_message), &log_message, 2);
|
||||
#endif /* !PARAM_NO_ORB */
|
||||
|
||||
if (!orb_log_message_pub) {
|
||||
PX4_ERR("failed to advertise log_message");
|
||||
|
@ -136,9 +134,7 @@ __EXPORT void px4_log_modulename(int level, const char *moduleName, const char *
|
|||
va_end(argptr);
|
||||
log_message.text[max_length_pub - 1] = 0; //ensure 0-termination
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
orb_publish(ORB_ID(log_message), orb_log_message_pub, &log_message);
|
||||
#endif /* !PARAM_NO_ORB */
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -38,4 +38,5 @@ px4_add_module(
|
|||
ms4525_airspeed.cpp
|
||||
DEPENDS
|
||||
drivers__airspeed
|
||||
mathlib
|
||||
)
|
||||
|
|
|
@ -39,4 +39,5 @@ px4_add_module(
|
|||
MS5525_main.cpp
|
||||
DEPENDS
|
||||
drivers__airspeed
|
||||
mathlib
|
||||
)
|
||||
|
|
|
@ -39,4 +39,5 @@ px4_add_module(
|
|||
SDP3X_main.cpp
|
||||
DEPENDS
|
||||
drivers__airspeed
|
||||
mathlib
|
||||
)
|
||||
|
|
|
@ -42,4 +42,5 @@ px4_add_module(
|
|||
LidarLitePWM.cpp
|
||||
DEPENDS
|
||||
drivers_rangefinder
|
||||
#arch_io_pins # LidarLitePWM
|
||||
)
|
||||
|
|
|
@ -37,4 +37,6 @@ px4_add_module(
|
|||
-Wno-cast-align # TODO: fix and enable
|
||||
SRCS
|
||||
bmm150.cpp
|
||||
DEPENDS
|
||||
conversion
|
||||
)
|
||||
|
|
|
@ -40,4 +40,6 @@ px4_add_module(
|
|||
lis3mdl_spi.cpp
|
||||
lis3mdl_main.cpp
|
||||
lis3mdl.cpp
|
||||
DEPENDS
|
||||
conversion
|
||||
)
|
||||
|
|
|
@ -1 +0,0 @@
|
|||
./dsdlc_generated/
|
|
@ -31,16 +31,19 @@
|
|||
#
|
||||
############################################################################
|
||||
|
||||
px4_add_git_submodule(TARGET git_uavcan PATH "libuavcan")
|
||||
set(LIBUAVCAN_DIR ${PX4_SOURCE_DIR}/src/drivers/uavcan/libuavcan)
|
||||
set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers)
|
||||
|
||||
px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR})
|
||||
|
||||
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||
set(UAVCAN_PLATFORM "generic")
|
||||
|
||||
if(CONFIG_ARCH_CHIP)
|
||||
if (${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif (${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_DRIVER "stm32")
|
||||
set(UAVCAN_TIMER 5) # The default timer the 5
|
||||
endif()
|
||||
|
@ -57,31 +60,31 @@ endif()
|
|||
string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
|
||||
string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
||||
add_definitions(
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=${UAVCAN_PLATFORM}
|
||||
)
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=${UAVCAN_PLATFORM}
|
||||
)
|
||||
|
||||
add_compile_options(-Wno-cast-align) # TODO: fix and enable
|
||||
add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL)
|
||||
add_dependencies(uavcan prebuild_targets)
|
||||
|
||||
# driver
|
||||
add_subdirectory(uavcan_drivers/${UAVCAN_DRIVER}/driver EXCLUDE_FROM_ALL)
|
||||
add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL)
|
||||
target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC
|
||||
./libuavcan/libuavcan/include
|
||||
./libuavcan/libuavcan/include/dsdlc_generated
|
||||
)
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
)
|
||||
|
||||
|
||||
# generated DSDL
|
||||
set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/dsdl/uavcan")
|
||||
set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
|
||||
set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")
|
||||
|
||||
set(DSDLC_INPUT_FILES)
|
||||
|
@ -90,22 +93,22 @@ foreach(DSDLC_INPUT ${DSDLC_INPUTS})
|
|||
list(APPEND DSDLC_INPUT_FILES ${DSDLC_NEW_INPUT_FILES})
|
||||
endforeach(DSDLC_INPUT)
|
||||
add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
|
||||
COMMAND ${PYTHON} ${CMAKE_CURRENT_SOURCE_DIR}/libuavcan/libuavcan/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
|
||||
DEPENDS ${DSDLC_INPUT_FILES}
|
||||
COMMENT "PX4 UAVCAN dsdl compiler"
|
||||
)
|
||||
COMMAND ${PYTHON} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
|
||||
DEPENDS ${DSDLC_INPUT_FILES}
|
||||
COMMENT "PX4 UAVCAN dsdl compiler"
|
||||
)
|
||||
add_custom_target(px4_uavcan_dsdlc DEPENDS px4_uavcan_dsdlc_run.stamp)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcan
|
||||
MODULE drivers__uavcan
|
||||
MAIN uavcan
|
||||
INCLUDES
|
||||
${DSDLC_OUTPUT}
|
||||
libuavcan/libuavcan/include
|
||||
libuavcan/libuavcan/include/dsdlc_generated
|
||||
libuavcan/libuavcan_drivers/posix/include
|
||||
uavcan_drivers/${UAVCAN_DRIVER}/driver/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include
|
||||
${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include
|
||||
SRCS
|
||||
# Main
|
||||
uavcan_main.cpp
|
||||
|
|
|
@ -1 +0,0 @@
|
|||
./dsdlc_generated/
|
|
@ -1,6 +1,6 @@
|
|||
############################################################################
|
||||
#
|
||||
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
# Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
|
||||
#
|
||||
# Redistribution and use in source and binary forms, with or without
|
||||
# modification, are permitted provided that the following conditions
|
||||
|
@ -37,60 +37,88 @@ set(LIBUAVCAN_DIR_DRIVERS ${PX4_SOURCE_DIR}/src/drivers/uavcan/uavcan_drivers)
|
|||
px4_add_git_submodule(TARGET git_uavcan PATH ${LIBUAVCAN_DIR})
|
||||
|
||||
set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
|
||||
set(UAVCAN_PLATFORM generic CACHE STRING "uavcan platform")
|
||||
set(UAVCAN_PLATFORM "generic")
|
||||
|
||||
if(CONFIG_ARCH_CHIP)
|
||||
if(${CONFIG_ARCH_CHIP} MATCHES "kinetis")
|
||||
set(UAVCAN_DRIVER "kinetis")
|
||||
set(UAVCAN_TIMER 1)
|
||||
elseif(${CONFIG_ARCH_CHIP} MATCHES "stm32")
|
||||
set(UAVCAN_DRIVER "stm32")
|
||||
set(UAVCAN_TIMER 5) # The default timer the 5
|
||||
endif()
|
||||
endif()
|
||||
|
||||
if(NOT DEFINED UAVCAN_DRIVER)
|
||||
message(FATAL_ERROR "UAVCAN_DRIVER not set")
|
||||
endif()
|
||||
|
||||
if(NOT config_uavcan_num_ifaces)
|
||||
message(FATAL_ERROR "config_uavcan_num_ifaces not set")
|
||||
endif()
|
||||
|
||||
string(TOUPPER "${PX4_PLATFORM}" OS_UPPER)
|
||||
string(TOUPPER "${UAVCAN_DRIVER}" UAVCAN_DRIVER_UPPER)
|
||||
add_definitions(
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=generic
|
||||
-DUAVCAN_STM32_${OS_UPPER}=1
|
||||
-DUAVCAN_STM32_NUM_IFACES=1
|
||||
-DUAVCAN_STM32_TIMER_NUMBER=2
|
||||
-DUAVCAN_USE_CPP03=ON
|
||||
-DUAVCAN_USE_EXTERNAL_SNPRINT
|
||||
)
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_${OS_UPPER}=1
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_NUM_IFACES=${config_uavcan_num_ifaces}
|
||||
-DUAVCAN_${UAVCAN_DRIVER_UPPER}_TIMER_NUMBER=${UAVCAN_TIMER}
|
||||
-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
|
||||
-DUAVCAN_DRIVER=uavcan_${UAVCAN_DRIVER}
|
||||
-DUAVCAN_IMPLEMENT_PLACEMENT_NEW=1
|
||||
-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
|
||||
-DUAVCAN_NO_ASSERTIONS
|
||||
-DUAVCAN_PLATFORM=${UAVCAN_PLATFORM}
|
||||
)
|
||||
|
||||
add_compile_options(-Wno-cast-align) # TODO: fix and enable
|
||||
add_subdirectory(${LIBUAVCAN_DIR} uavcannode_libuavcan)
|
||||
add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/stm32/driver uavcanesc_uavcan_drivers)
|
||||
target_include_directories(uavcan_stm32_driver PUBLIC
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
)
|
||||
add_subdirectory(${LIBUAVCAN_DIR} libuavcan EXCLUDE_FROM_ALL)
|
||||
add_dependencies(uavcan prebuild_targets)
|
||||
|
||||
# driver
|
||||
add_subdirectory(${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver libuavcan_drivers EXCLUDE_FROM_ALL)
|
||||
target_include_directories(uavcan_${UAVCAN_DRIVER}_driver PUBLIC
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
)
|
||||
|
||||
|
||||
# generated DSDL
|
||||
set(DSDLC_INPUTS "${CMAKE_CURRENT_SOURCE_DIR}/dsdl/com" "${LIBUAVCAN_DIR}/dsdl/uavcan")
|
||||
set(DSDLC_OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/include/dsdlc_generated")
|
||||
|
||||
set(DSDLC_INPUT_FILES)
|
||||
foreach(DSDLC_INPUT ${DSDLC_INPUTS})
|
||||
file(GLOB_RECURSE DSDLC_NEW_INPUT_FILES ${CMAKE_CURRENT_SOURCE_DIR} "${DSDLC_INPUT}/*.uavcan")
|
||||
list(APPEND DSDLC_INPUT_FILES ${DSDLC_NEW_INPUT_FILES})
|
||||
endforeach(DSDLC_INPUT)
|
||||
add_custom_command(OUTPUT px4_uavcan_dsdlc_run.stamp
|
||||
COMMAND ${PYTHON} ${LIBUAVCAN_DIR}/libuavcan/dsdl_compiler/libuavcan_dsdlc ${DSDLC_INPUTS} -O${DSDLC_OUTPUT}
|
||||
COMMAND ${CMAKE_COMMAND} -E touch px4_uavcan_dsdlc_run.stamp
|
||||
DEPENDS ${DSDLC_INPUT_FILES}
|
||||
COMMENT "PX4 UAVCAN dsdl compiler"
|
||||
)
|
||||
add_custom_target(px4_uavcan_dsdlc DEPENDS px4_uavcan_dsdlc_run.stamp)
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__uavcannode
|
||||
MODULE drivers__uavcannode
|
||||
MAIN uavcannode
|
||||
STACK_MAIN 1048
|
||||
INCLUDES
|
||||
${DSDLC_OUTPUT}
|
||||
${LIBUAVCAN_DIR}/libuavcan/include
|
||||
${LIBUAVCAN_DIR}/libuavcan/include/dsdlc_generated
|
||||
${LIBUAVCAN_DIR}/libuavcan_drivers/posix/include
|
||||
${LIBUAVCAN_DIR_DRIVERS}/stm32/driver/include
|
||||
COMPILE_FLAGS
|
||||
-Wframe-larger-than=1500
|
||||
-Wno-deprecated-declarations
|
||||
-O3
|
||||
${LIBUAVCAN_DIR_DRIVERS}/${UAVCAN_DRIVER}/driver/include
|
||||
SRCS
|
||||
uavcannode_main.cpp
|
||||
indication_controller.cpp
|
||||
sim_controller.cpp
|
||||
led.cpp
|
||||
resources.cpp
|
||||
allocator.hpp
|
||||
uavcan_driver.hpp
|
||||
UavcanNode.cpp
|
||||
UavcanNode.hpp
|
||||
DEPENDS
|
||||
px4_uavcan_dsdlc
|
||||
drivers_bootloaders
|
||||
git_uavcan
|
||||
version
|
||||
|
||||
uavcan_stm32_driver
|
||||
|
||||
# within libuavcan
|
||||
libuavcan_dsdlc
|
||||
uavcan
|
||||
git_uavcan
|
||||
uavcan_${UAVCAN_DRIVER}_driver
|
||||
uavcan # within libuavcan
|
||||
)
|
||||
|
||||
set_property(GLOBAL APPEND PROPERTY PX4_MODULE_LIBRARIES uavcan_stm32_driver)
|
||||
|
|
|
@ -0,0 +1,520 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "UavcanNode.hpp"
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
#include <lib/ecl/geo/geo.h>
|
||||
#include <lib/version/version.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
*
|
||||
* Implements basic functionality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
/*
|
||||
* This is the AppImageDescriptor used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc of this application
|
||||
*/
|
||||
|
||||
boot_app_shared_section app_descriptor_t AppDescriptor = {
|
||||
.signature = {APP_DESCRIPTOR_SIGNATURE},
|
||||
.image_crc = 0,
|
||||
.image_size = 0,
|
||||
.vcs_commit = 0,
|
||||
.major_version = APP_VERSION_MAJOR,
|
||||
.minor_version = APP_VERSION_MINOR,
|
||||
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
|
||||
};
|
||||
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::uavcan),
|
||||
_node(can_driver, system_clock, _pool_allocator),
|
||||
_time_sync_slave(_node),
|
||||
_fw_update_listner(_node),
|
||||
_ahrs_magnetic_field_strength2_publisher(_node),
|
||||
_gnss_fix2_publisher(_node),
|
||||
_power_battery_info_publisher(_node),
|
||||
_air_data_static_pressure_publisher(_node),
|
||||
_air_data_static_temperature_publisher(_node),
|
||||
_cycle_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle time")),
|
||||
_interval_perf(perf_alloc(PC_INTERVAL, MODULE_NAME": cycle interval")),
|
||||
_reset_timer(_node)
|
||||
{
|
||||
int res = pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
}
|
||||
|
||||
UavcanNode::~UavcanNode()
|
||||
{
|
||||
if (_instance) {
|
||||
/* tell the task we want it to go away */
|
||||
_task_should_exit.store(true);
|
||||
ScheduleNow();
|
||||
|
||||
unsigned i = 10;
|
||||
|
||||
do {
|
||||
/* wait 5ms - it should wake every 10ms or so worst-case */
|
||||
usleep(5000);
|
||||
|
||||
if (--i == 0) {
|
||||
break;
|
||||
}
|
||||
|
||||
} while (_instance);
|
||||
}
|
||||
|
||||
perf_free(_cycle_perf);
|
||||
perf_free(_interval_perf);
|
||||
}
|
||||
|
||||
int UavcanNode::getHardwareVersion(uavcan::protocol::HardwareVersion &hwver)
|
||||
{
|
||||
int rv = -1;
|
||||
|
||||
if (UavcanNode::instance()) {
|
||||
|
||||
hwver.major = HW_VERSION_MAJOR;
|
||||
hwver.minor = HW_VERSION_MINOR;
|
||||
|
||||
mfguid_t mfgid = {};
|
||||
board_get_mfguid(mfgid);
|
||||
uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
|
||||
rv = 0;
|
||||
}
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
{
|
||||
if (_instance != nullptr) {
|
||||
PX4_WARN("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN driver init
|
||||
* Note that we instantiate and initialize CanInitHelper only once, because the STM32's bxCAN driver
|
||||
* shipped with libuavcan does not support deinitialization.
|
||||
*/
|
||||
static CanInitHelper *can = nullptr;
|
||||
|
||||
if (can == nullptr) {
|
||||
|
||||
can = new CanInitHelper();
|
||||
|
||||
if (can == nullptr) { // We don't have exceptions so bad_alloc cannot be thrown
|
||||
PX4_ERR("Out of memory");
|
||||
return -1;
|
||||
}
|
||||
|
||||
const int can_init_res = can->init(bitrate);
|
||||
|
||||
if (can_init_res < 0) {
|
||||
PX4_ERR("CAN driver init failed %i", can_init_res);
|
||||
return can_init_res;
|
||||
}
|
||||
}
|
||||
|
||||
// Node init
|
||||
_instance = new UavcanNode(can->driver, UAVCAN_DRIVER::SystemClock::instance());
|
||||
|
||||
if (_instance == nullptr) {
|
||||
PX4_ERR("Out of memory");
|
||||
return -1;
|
||||
}
|
||||
|
||||
const int node_init_res = _instance->init(node_id, can->driver.updateEvent());
|
||||
|
||||
if (node_init_res < 0) {
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
PX4_ERR("Node init failed %i", node_init_res);
|
||||
return node_init_res;
|
||||
}
|
||||
|
||||
// Keep the bit rate for reboots on BenginFirmware updates
|
||||
_instance->active_bitrate = bitrate;
|
||||
|
||||
_instance->ScheduleOnInterval(ScheduleIntervalMs * 1000);
|
||||
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
void UavcanNode::fill_node_info()
|
||||
{
|
||||
// software version
|
||||
uavcan::protocol::SoftwareVersion swver;
|
||||
|
||||
// Extracting the first 8 hex digits of the git hash and converting them to int
|
||||
char fw_git_short[9] = {};
|
||||
std::memmove(fw_git_short, px4_firmware_version_string(), 8);
|
||||
char *end = nullptr;
|
||||
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
|
||||
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
|
||||
swver.major = AppDescriptor.major_version;
|
||||
swver.minor = AppDescriptor.minor_version;
|
||||
swver.image_crc = AppDescriptor.image_crc;
|
||||
|
||||
_node.setSoftwareVersion(swver);
|
||||
|
||||
/* hardware version */
|
||||
uavcan::protocol::HardwareVersion hwver;
|
||||
getHardwareVersion(hwver);
|
||||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
void UavcanNode::busevent_signal_trampoline()
|
||||
{
|
||||
if (_instance) {
|
||||
// trigger the work queue (Note, this is called from IRQ context)
|
||||
_instance->ScheduleNow();
|
||||
}
|
||||
}
|
||||
|
||||
static void cb_reboot(const uavcan::TimerEvent &)
|
||||
{
|
||||
px4_systemreset(false);
|
||||
}
|
||||
|
||||
void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request>
|
||||
&req, uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp)
|
||||
{
|
||||
static bool inprogress = false;
|
||||
|
||||
rsp.error = rsp.ERROR_UNKNOWN;
|
||||
|
||||
if (req.image_file_remote_path.path.size()) {
|
||||
rsp.error = rsp.ERROR_IN_PROGRESS;
|
||||
|
||||
if (!inprogress) {
|
||||
inprogress = true;
|
||||
bootloader_app_shared_t shared;
|
||||
shared.bus_speed = active_bitrate;
|
||||
shared.node_id = _node.getNodeID().get();
|
||||
bootloader_app_shared_write(&shared, App);
|
||||
//rgb_led(255, 128, 0, 5);
|
||||
_reset_timer.setCallback(cb_reboot);
|
||||
_reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000));
|
||||
rsp.error = rsp.ERROR_OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
|
||||
{
|
||||
_node.setName(HW_UAVCAN_NAME);
|
||||
_node.setNodeID(node_id);
|
||||
|
||||
fill_node_info();
|
||||
|
||||
const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this,
|
||||
&UavcanNode::cb_beginfirmware_update));
|
||||
|
||||
if (srv_start_res < 0) {
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
// Restart handler
|
||||
class RestartRequestHandler: public uavcan::IRestartRequestHandler
|
||||
{
|
||||
bool handleRestartRequest(uavcan::NodeID request_source) override
|
||||
{
|
||||
PX4_INFO("UAVCAN: Restarting by request from %i\n", int(request_source.get()));
|
||||
usleep(20 * 1000 * 1000);
|
||||
px4_systemreset(false);
|
||||
return true; // Will never be executed BTW
|
||||
}
|
||||
} restart_request_handler;
|
||||
|
||||
void UavcanNode::Run()
|
||||
{
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
if (!_initialized) {
|
||||
|
||||
get_node().setRestartRequestHandler(&restart_request_handler);
|
||||
|
||||
// Set up the time synchronization
|
||||
const int slave_init_res = _time_sync_slave.start();
|
||||
|
||||
if (slave_init_res < 0) {
|
||||
PX4_ERR("Failed to start time_sync_slave");
|
||||
_task_should_exit.store(true);
|
||||
}
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
_sensor_baro_sub.registerCallback();
|
||||
_sensor_mag_sub.registerCallback();
|
||||
_vehicle_gps_position_sub.registerCallback();
|
||||
|
||||
_initialized = true;
|
||||
}
|
||||
|
||||
perf_begin(_cycle_perf);
|
||||
perf_count(_interval_perf);
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
parameter_update_s pupdate;
|
||||
_parameter_update_sub.copy(&pupdate);
|
||||
|
||||
// update parameters from storage
|
||||
}
|
||||
|
||||
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||
|
||||
if (spin_res < 0) {
|
||||
PX4_ERR("node spin error %i", spin_res);
|
||||
}
|
||||
|
||||
// sensor_baro -> uavcan::equipment::air_data::StaticTemperature
|
||||
if (_sensor_baro_sub.updated()) {
|
||||
sensor_baro_s baro;
|
||||
|
||||
if (_sensor_baro_sub.copy(&baro)) {
|
||||
uavcan::equipment::air_data::StaticPressure static_pressure{};
|
||||
static_pressure.static_pressure = baro.pressure * 100; // millibar -> pascals
|
||||
_air_data_static_pressure_publisher.broadcast(static_pressure);
|
||||
|
||||
if (hrt_elapsed_time(&_last_static_temperature_publish) > 1_s) {
|
||||
uavcan::equipment::air_data::StaticTemperature static_temperature{};
|
||||
static_temperature.static_temperature = baro.temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS;
|
||||
_air_data_static_temperature_publisher.broadcast(static_temperature);
|
||||
_last_static_temperature_publish = hrt_absolute_time();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// sensor_mag -> uavcan::equipment::ahrs::MagneticFieldStrength2
|
||||
if (_sensor_mag_sub.updated()) {
|
||||
sensor_mag_s mag;
|
||||
|
||||
if (_sensor_mag_sub.copy(&mag)) {
|
||||
uavcan::equipment::ahrs::MagneticFieldStrength2 magnetic_field{};
|
||||
magnetic_field.sensor_id = mag.device_id;
|
||||
magnetic_field.magnetic_field_ga[0] = mag.x;
|
||||
magnetic_field.magnetic_field_ga[1] = mag.y;
|
||||
magnetic_field.magnetic_field_ga[2] = mag.z;
|
||||
_ahrs_magnetic_field_strength2_publisher.broadcast(magnetic_field);
|
||||
}
|
||||
}
|
||||
|
||||
// vehicle_gps_position -> uavcan::equipment::gnss::Fix2
|
||||
if (_vehicle_gps_position_sub.updated()) {
|
||||
vehicle_gps_position_s gps;
|
||||
|
||||
if (_vehicle_gps_position_sub.copy(&gps)) {
|
||||
uavcan::equipment::gnss::Fix2 fix2{};
|
||||
|
||||
//fix2.gnss_timestamp = gps.time_utc_usec;
|
||||
fix2.latitude_deg_1e8 = gps.lat * 10;
|
||||
fix2.longitude_deg_1e8 = gps.lon * 10;
|
||||
fix2.height_msl_mm = gps.alt;
|
||||
fix2.height_ellipsoid_mm = gps.alt_ellipsoid;
|
||||
fix2.status = gps.fix_type;
|
||||
fix2.ned_velocity[0] = gps.vel_n_m_s;
|
||||
fix2.ned_velocity[1] = gps.vel_e_m_s;
|
||||
fix2.ned_velocity[2] = gps.vel_d_m_s;
|
||||
|
||||
_gnss_fix2_publisher.broadcast(fix2);
|
||||
}
|
||||
}
|
||||
|
||||
perf_end(_cycle_perf);
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
if (_task_should_exit.load()) {
|
||||
ScheduleClear();
|
||||
_instance = nullptr;
|
||||
}
|
||||
}
|
||||
|
||||
void UavcanNode::print_info()
|
||||
{
|
||||
pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
// Memory status
|
||||
printf("Pool allocator status:\n");
|
||||
printf("\tCapacity hard/soft: %u/%u blocks\n",
|
||||
_pool_allocator.getBlockCapacityHardLimit(), _pool_allocator.getBlockCapacity());
|
||||
printf("\tReserved: %u blocks\n", _pool_allocator.getNumReservedBlocks());
|
||||
printf("\tAllocated: %u blocks\n", _pool_allocator.getNumAllocatedBlocks());
|
||||
|
||||
printf("\n");
|
||||
|
||||
// UAVCAN node perfcounters
|
||||
printf("UAVCAN node status:\n");
|
||||
printf("\tInternal failures: %llu\n", _node.getInternalFailureCount());
|
||||
printf("\tTransfer errors: %llu\n", _node.getDispatcher().getTransferPerfCounter().getErrorCount());
|
||||
printf("\tRX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getRxTransferCount());
|
||||
printf("\tTX transfers: %llu\n", _node.getDispatcher().getTransferPerfCounter().getTxTransferCount());
|
||||
|
||||
printf("\n");
|
||||
|
||||
// CAN driver status
|
||||
for (unsigned i = 0; i < _node.getDispatcher().getCanIOManager().getCanDriver().getNumIfaces(); i++) {
|
||||
printf("CAN%u status:\n", unsigned(i + 1));
|
||||
|
||||
auto iface = _node.getDispatcher().getCanIOManager().getCanDriver().getIface(i);
|
||||
printf("\tHW errors: %llu\n", iface->getErrorCount());
|
||||
|
||||
auto iface_perf_cnt = _node.getDispatcher().getCanIOManager().getIfacePerfCounters(i);
|
||||
printf("\tIO errors: %llu\n", iface_perf_cnt.errors);
|
||||
printf("\tRX frames: %llu\n", iface_perf_cnt.frames_rx);
|
||||
printf("\tTX frames: %llu\n", iface_perf_cnt.frames_tx);
|
||||
}
|
||||
|
||||
printf("\n");
|
||||
|
||||
perf_print_counter(_cycle_perf);
|
||||
perf_print_counter(_interval_perf);
|
||||
|
||||
pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
void UavcanNode::shrink()
|
||||
{
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
_pool_allocator.shrink();
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
static void print_usage()
|
||||
{
|
||||
PX4_INFO("usage: \n"
|
||||
"\tuavcannode {start|status|stop|arm|disarm}");
|
||||
}
|
||||
|
||||
extern "C" int uavcannode_start(int argc, char *argv[])
|
||||
{
|
||||
//board_app_initialize(nullptr);
|
||||
|
||||
// CAN bitrate
|
||||
int32_t bitrate = 0;
|
||||
|
||||
// Node ID
|
||||
int32_t node_id = 0;
|
||||
|
||||
// Did the bootloader auto baud and get a node ID Allocated
|
||||
bootloader_app_shared_t shared;
|
||||
int valid = bootloader_app_shared_read(&shared, BootLoader);
|
||||
|
||||
if (valid == 0) {
|
||||
|
||||
bitrate = shared.bus_speed;
|
||||
node_id = shared.node_id;
|
||||
|
||||
// Invalidate to prevent deja vu
|
||||
bootloader_app_shared_invalidate();
|
||||
|
||||
} else {
|
||||
// Node ID
|
||||
(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
|
||||
(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
|
||||
}
|
||||
|
||||
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
|
||||
PX4_ERR("Invalid Node ID %i", node_id);
|
||||
return 1;
|
||||
}
|
||||
|
||||
// Start
|
||||
PX4_INFO("Node ID %u, bitrate %u", node_id, bitrate);
|
||||
int rv = UavcanNode::start(node_id, bitrate);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "start")) {
|
||||
if (UavcanNode::instance()) {
|
||||
PX4_ERR("already started");
|
||||
return 1;
|
||||
}
|
||||
|
||||
return uavcannode_start(argc, argv);
|
||||
}
|
||||
|
||||
/* commands below require the app to be started */
|
||||
UavcanNode *const inst = UavcanNode::instance();
|
||||
|
||||
if (!inst) {
|
||||
PX4_ERR("application not running");
|
||||
return 1;
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
|
||||
inst->print_info();
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "stop")) {
|
||||
delete inst;
|
||||
return 0;
|
||||
}
|
||||
|
||||
print_usage();
|
||||
return 1;
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 PX4 Development Team. All rights reserved.
|
||||
* Copyright (c) 2015-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -31,14 +31,6 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#include <drivers/device/device.h>
|
||||
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
||||
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
|
||||
#include <uavcan/node/timer.hpp>
|
||||
/**
|
||||
* @file uavcan_main.hpp
|
||||
*
|
||||
|
@ -48,16 +40,44 @@
|
|||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN 1
|
||||
#define UAVCAN_DEVICE_PATH "/dev/uavcan/node"
|
||||
#pragma once
|
||||
|
||||
// we add 1 to allow for busevent
|
||||
#define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+1)
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/atomic.h>
|
||||
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
|
||||
|
||||
#include "uavcan_driver.hpp"
|
||||
#include "allocator.hpp"
|
||||
|
||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||
#include <uavcan/protocol/global_time_sync_slave.hpp>
|
||||
#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
|
||||
#include <uavcan/node/timer.hpp>
|
||||
#include <uavcan/protocol/node_status_monitor.hpp>
|
||||
#include <uavcan/protocol/param/GetSet.hpp>
|
||||
#include <uavcan/protocol/param/ExecuteOpcode.hpp>
|
||||
#include <uavcan/protocol/RestartNode.hpp>
|
||||
#include <uavcan/equipment/ahrs/MagneticFieldStrength2.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticPressure.hpp>
|
||||
#include <uavcan/equipment/air_data/StaticTemperature.hpp>
|
||||
#include <uavcan/equipment/gnss/Fix2.hpp>
|
||||
#include <uavcan/equipment/power/BatteryInfo.hpp>
|
||||
|
||||
|
||||
#include <lib/parameters/param.h>
|
||||
#include <lib/perf/perf_counter.h>
|
||||
|
||||
#include <uORB/Subscription.hpp>
|
||||
#include <uORB/SubscriptionCallback.hpp>
|
||||
#include <uORB/topics/parameter_update.h>
|
||||
#include <uORB/topics/sensor_baro.h>
|
||||
#include <uORB/topics/sensor_mag.h>
|
||||
#include <uORB/topics/vehicle_gps_position.h>
|
||||
|
||||
/**
|
||||
* A UAVCAN node.
|
||||
*/
|
||||
class UavcanNode : public cdev::CDev
|
||||
class UavcanNode : public px4::ScheduledWorkItem
|
||||
{
|
||||
/*
|
||||
* This memory is reserved for uavcan to use as over flow for message
|
||||
|
@ -68,69 +88,70 @@ class UavcanNode : public cdev::CDev
|
|||
* free -and multiply it times getBlockSize to get the number of bytes
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned MemPoolSize = 2048;
|
||||
static constexpr unsigned MemPoolSize = 2048;
|
||||
|
||||
static constexpr unsigned MaxBitRatePerSec = 1000000;
|
||||
static constexpr unsigned bitPerFrame = 148;
|
||||
static constexpr unsigned FramePerSecond = MaxBitRatePerSec / bitPerFrame;
|
||||
static constexpr unsigned FramePerMSecond = ((FramePerSecond / 1000) + 1);
|
||||
|
||||
static constexpr unsigned ScheduleIntervalMs = 10;
|
||||
|
||||
/*
|
||||
* This memory is reserved for uavcan to use for queuing CAN frames.
|
||||
* At 1Mbit there is approximately one CAN frame every 200 uS.
|
||||
* At 1Mbit there is approximately one CAN frame every 145 uS.
|
||||
* The number of buffers sets how long you can go without calling
|
||||
* node_spin_xxxx. Since our task is the only one running and the
|
||||
* driver will light the fd when there is a CAN frame we can nun with
|
||||
* driver will light the callback when there is a CAN frame we can nun with
|
||||
* a minimum number of buffers to conserver memory. Each buffer is
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a maximum required
|
||||
* poll rate of ~1 mS
|
||||
*
|
||||
*/
|
||||
static constexpr unsigned RxQueueLenPerIface = 5;
|
||||
|
||||
/*
|
||||
* This memory is uses for the tasks stack size
|
||||
* 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
|
||||
* of ~1 mS
|
||||
* 1000000/200
|
||||
*/
|
||||
|
||||
static constexpr unsigned StackSize = 2500;
|
||||
static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * ScheduleIntervalMs;
|
||||
|
||||
public:
|
||||
typedef uavcan::Node<MemPoolSize> Node;
|
||||
typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
typedef UAVCAN_DRIVER::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
|
||||
typedef uavcan::protocol::file::BeginFirmwareUpdate BeginFirmwareUpdate;
|
||||
|
||||
UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
|
||||
|
||||
virtual ~UavcanNode();
|
||||
|
||||
virtual int ioctl(file *filp, int cmd, unsigned long arg);
|
||||
|
||||
static int start(uavcan::NodeID node_id, uint32_t bitrate);
|
||||
|
||||
Node &get_node() { return _node; }
|
||||
|
||||
int teardown();
|
||||
uavcan::Node<> &get_node() { return _node; }
|
||||
|
||||
void print_info();
|
||||
|
||||
static UavcanNode *instance() { return _instance; }
|
||||
void shrink();
|
||||
|
||||
static UavcanNode *instance() { return _instance; }
|
||||
static int getHardwareVersion(uavcan::protocol::HardwareVersion &hwver);
|
||||
|
||||
static void busevent_signal_trampoline();
|
||||
|
||||
/* The bit rate that can be passed back to the bootloader */
|
||||
int32_t active_bitrate{0};
|
||||
|
||||
int32_t active_bitrate;
|
||||
|
||||
|
||||
protected:
|
||||
void Run() override;
|
||||
private:
|
||||
void fill_node_info();
|
||||
int init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events);
|
||||
void node_spin_once();
|
||||
int run();
|
||||
static void busevent_signal_trampoline();
|
||||
int init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events);
|
||||
|
||||
px4::atomic_bool _task_should_exit{false}; ///< flag to indicate to tear down the CAN driver
|
||||
|
||||
px4_sem_t _sem; ///< semaphore for scheduling the task
|
||||
int _task = -1; ///< handle to the OS task
|
||||
bool _task_should_exit = false; ///< flag to indicate to tear down the CAN driver
|
||||
bool _initialized{false}; ///< number of actuators currently available
|
||||
|
||||
static UavcanNode *_instance; ///< singleton pointer
|
||||
Node _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
uavcan_node::Allocator _pool_allocator;
|
||||
|
||||
uavcan::Node<> _node; ///< library instance
|
||||
pthread_mutex_t _node_mutex;
|
||||
|
||||
uavcan::GlobalTimeSyncSlave _time_sync_slave;
|
||||
|
||||
typedef uavcan::MethodBinder<UavcanNode *,
|
||||
|
@ -142,10 +163,27 @@ private:
|
|||
void cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request> &req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp);
|
||||
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::ahrs::MagneticFieldStrength2> _ahrs_magnetic_field_strength2_publisher;
|
||||
uavcan::Publisher<uavcan::equipment::gnss::Fix2> _gnss_fix2_publisher;
|
||||
uavcan::Publisher<uavcan::equipment::power::BatteryInfo> _power_battery_info_publisher;
|
||||
uavcan::Publisher<uavcan::equipment::air_data::StaticPressure> _air_data_static_pressure_publisher;
|
||||
uavcan::Publisher<uavcan::equipment::air_data::StaticTemperature> _air_data_static_temperature_publisher;
|
||||
hrt_abstime _last_static_temperature_publish{0};
|
||||
|
||||
|
||||
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_baro_sub{this, ORB_ID(sensor_baro)};
|
||||
uORB::SubscriptionCallbackWorkItem _sensor_mag_sub{this, ORB_ID(sensor_mag)};
|
||||
uORB::SubscriptionCallbackWorkItem _vehicle_gps_position_sub{this, ORB_ID(vehicle_gps_position)};
|
||||
|
||||
perf_counter_t _cycle_perf;
|
||||
perf_counter_t _interval_perf;
|
||||
|
||||
public:
|
||||
|
||||
/* A timer used to reboot after the response is sent */
|
||||
|
||||
uavcan::TimerEventForwarder<void (*)(const uavcan::TimerEvent &)> _reset_timer;
|
||||
|
||||
};
|
|
@ -1,7 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -32,12 +31,40 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
#if defined(CONFIG_NSH_LIBRARY)
|
||||
#define stack_check()
|
||||
#define free_check() free_main(0,0)
|
||||
#else
|
||||
void stack_check(void);
|
||||
void free_check(void);
|
||||
#endif
|
||||
__END_DECLS
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <systemlib/err.h>
|
||||
#include <uavcan/uavcan.hpp>
|
||||
#include <uavcan/helpers/heap_based_pool_allocator.hpp>
|
||||
|
||||
// TODO: Entire UAVCAN application should be moved into a namespace later; this is the first step.
|
||||
namespace uavcan_node
|
||||
{
|
||||
|
||||
struct AllocatorSynchronizer {
|
||||
const ::irqstate_t state = ::enter_critical_section();
|
||||
~AllocatorSynchronizer() { ::leave_critical_section(state); }
|
||||
};
|
||||
|
||||
struct Allocator : public uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer> {
|
||||
static constexpr unsigned CapacitySoftLimit = 250;
|
||||
static constexpr unsigned CapacityHardLimit = 500;
|
||||
|
||||
Allocator() :
|
||||
uavcan::HeapBasedPoolAllocator<uavcan::MemPoolBlockSize, AllocatorSynchronizer>(CapacitySoftLimit, CapacityHardLimit)
|
||||
{ }
|
||||
|
||||
~Allocator()
|
||||
{
|
||||
if (getNumAllocatedBlocks() > 0) {
|
||||
PX4_ERR("UAVCAN LEAKS MEMORY: %u BLOCKS (%u BYTES) LOST",
|
||||
getNumAllocatedBlocks(), getNumAllocatedBlocks() * uavcan::MemPoolBlockSize);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
}
|
|
@ -1,96 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "indication_controller.hpp"
|
||||
#include <uavcan/equipment/indication/LightsCommand.hpp>
|
||||
#include "led.hpp"
|
||||
|
||||
namespace
|
||||
{
|
||||
unsigned self_light_index = 0;
|
||||
|
||||
void cb_light_command(const uavcan::ReceivedDataStructure<uavcan::equipment::indication::LightsCommand> &msg)
|
||||
{
|
||||
uavcan::uint32_t red = 0;
|
||||
uavcan::uint32_t green = 0;
|
||||
uavcan::uint32_t blue = 0;
|
||||
|
||||
for (auto &cmd : msg.commands) {
|
||||
if (cmd.light_id == self_light_index) {
|
||||
using uavcan::equipment::indication::RGB565;
|
||||
|
||||
red = uavcan::uint32_t(float(cmd.color.red) *
|
||||
(255.0F / float(RGB565::FieldTypes::red::max())) + 0.5F);
|
||||
|
||||
green = uavcan::uint32_t(float(cmd.color.green) *
|
||||
(255.0F / float(RGB565::FieldTypes::green::max())) + 0.5F);
|
||||
|
||||
blue = uavcan::uint32_t(float(cmd.color.blue) *
|
||||
(255.0F / float(RGB565::FieldTypes::blue::max())) + 0.5F);
|
||||
|
||||
red = uavcan::min<uavcan::uint32_t>(red, 0xFFU);
|
||||
green = uavcan::min<uavcan::uint32_t>(green, 0xFFU);
|
||||
blue = uavcan::min<uavcan::uint32_t>(blue, 0xFFU);
|
||||
}
|
||||
|
||||
if (cmd.light_id == self_light_index + 1) {
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "rgb:%d %d %d hz %d\n", red, green, blue, int(cmd.color.red));
|
||||
}
|
||||
|
||||
rgb_led(red, green, blue, int(cmd.color.red));
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int init_indication_controller(uavcan::INode &node)
|
||||
{
|
||||
static uavcan::Subscriber<uavcan::equipment::indication::LightsCommand> sub_light(node);
|
||||
|
||||
self_light_index = 0;
|
||||
|
||||
int res = 0;
|
||||
|
||||
res = sub_light.start(cb_light_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
|
@ -1,40 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
|
||||
|
||||
int init_indication_controller(uavcan::INode &node);
|
|
@ -1,75 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <nuttx/config.h>
|
||||
|
||||
#include <stdint.h>
|
||||
|
||||
|
||||
#include "hardware/stm32_tim.h"
|
||||
|
||||
|
||||
#include "led.hpp"
|
||||
|
||||
void rgb_led(int r, int g, int b, int freqs)
|
||||
{
|
||||
|
||||
long fosc = 72000000;
|
||||
long prescale = 2048;
|
||||
long p1s = fosc / prescale;
|
||||
long p0p5s = p1s / 2;
|
||||
stm32_tim_channel_t mode = (stm32_tim_channel_t)(STM32_TIM_CH_OUTPWM | STM32_TIM_CH_POLARITY_NEG);
|
||||
static struct stm32_tim_dev_s *tim = 0;
|
||||
|
||||
if (tim == 0) {
|
||||
tim = stm32_tim_init(3);
|
||||
STM32_TIM_SETMODE(tim, STM32_TIM_MODE_UP);
|
||||
STM32_TIM_SETCLOCK(tim, p1s - 8);
|
||||
STM32_TIM_SETPERIOD(tim, p1s);
|
||||
STM32_TIM_SETCOMPARE(tim, 1, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, 0);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, 0);
|
||||
STM32_TIM_SETCHANNEL(tim, 1, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 2, mode);
|
||||
STM32_TIM_SETCHANNEL(tim, 3, mode);
|
||||
}
|
||||
|
||||
long p = freqs == 0 ? p1s : p1s / freqs;
|
||||
STM32_TIM_SETPERIOD(tim, p);
|
||||
|
||||
p = freqs == 0 ? p1s + 1 : p0p5s / freqs;
|
||||
|
||||
STM32_TIM_SETCOMPARE(tim, 1, (r * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 2, (g * p) / 255);
|
||||
STM32_TIM_SETCOMPARE(tim, 3, (b * p) / 255);
|
||||
}
|
|
@ -1,37 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
__BEGIN_DECLS
|
||||
void rgb_led(int r, int g, int b, int freqs);
|
||||
__END_DECLS
|
|
@ -1,189 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <nuttx/progmem.h>
|
||||
#include <nuttx/compiler.h>
|
||||
#include <stdlib.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#include <nuttx/arch.h>
|
||||
|
||||
#include <systemlib/cpuload.h>
|
||||
#include "resources.hpp"
|
||||
|
||||
#if !defined(CONFIG_NSH_LIBRARY)
|
||||
|
||||
static const char *
|
||||
tstate_name(const tstate_t s)
|
||||
{
|
||||
switch (s) {
|
||||
case TSTATE_TASK_INVALID: return "init";
|
||||
|
||||
case TSTATE_TASK_PENDING: return "PEND";
|
||||
|
||||
case TSTATE_TASK_READYTORUN: return "READY";
|
||||
|
||||
case TSTATE_TASK_RUNNING: return "RUN";
|
||||
|
||||
case TSTATE_TASK_INACTIVE: return "inact";
|
||||
|
||||
case TSTATE_WAIT_SEM: return "w:sem";
|
||||
#ifndef CONFIG_DISABLE_SIGNALS
|
||||
|
||||
case TSTATE_WAIT_SIG: return "w:sig";
|
||||
#endif
|
||||
#ifndef CONFIG_DISABLE_MQUEUE
|
||||
|
||||
case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
|
||||
|
||||
case TSTATE_WAIT_MQNOTFULL: return "w:mqf";
|
||||
#endif
|
||||
#ifdef CONFIG_PAGING
|
||||
|
||||
case TSTATE_WAIT_PAGEFILL: return "w:pgf";
|
||||
#endif
|
||||
|
||||
default:
|
||||
return "ERROR";
|
||||
}
|
||||
}
|
||||
|
||||
void stack_check(void)
|
||||
{
|
||||
|
||||
for (int i = 0; i < CONFIG_MAX_TASKS; i++) {
|
||||
|
||||
if (system_load.tasks[i].tcb) {
|
||||
size_t stack_size = system_load.tasks[i].tcb->adj_stack_size;
|
||||
ssize_t stack_free = 0;
|
||||
|
||||
#if CONFIG_ARCH_INTERRUPTSTACK > 3
|
||||
|
||||
if (system_load.tasks[i].tcb->pid == 0) {
|
||||
stack_size = (CONFIG_ARCH_INTERRUPTSTACK & ~3);
|
||||
stack_free = up_check_intstack_remain();
|
||||
|
||||
} else {
|
||||
#endif
|
||||
stack_free = up_check_tcbstack_remain(system_load.tasks[i].tcb);
|
||||
#if CONFIG_ARCH_INTERRUPTSTACK > 3
|
||||
}
|
||||
|
||||
#endif
|
||||
::syslog(LOG_INFO, "%4d %*-s %8lld %5u/%5u %3u (%3u) ",
|
||||
system_load.tasks[i].tcb->pid,
|
||||
CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
|
||||
(system_load.tasks[i].total_runtime / 1000),
|
||||
stack_size - stack_free,
|
||||
stack_size,
|
||||
system_load.tasks[i].tcb->sched_priority,
|
||||
system_load.tasks[i].tcb->base_priority);
|
||||
|
||||
#if CONFIG_RR_INTERVAL > 0
|
||||
/* print scheduling info with RR time slice */
|
||||
::syslog(LOG_INFO, " %6d\n", system_load.tasks[i].tcb->timeslice);
|
||||
#else
|
||||
/* print task state instead */
|
||||
::syslog(LOG_INFO, " %-6s\n", tstate_name((tstate_t)system_load.tasks[i].tcb->task_state));
|
||||
#endif
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
static void free_getprogmeminfo(struct mallinfo *mem)
|
||||
{
|
||||
size_t page = 0, stpage = 0xFFFF;
|
||||
size_t pagesize = 0;
|
||||
ssize_t status;
|
||||
|
||||
mem->arena = 0;
|
||||
mem->fordblks = 0;
|
||||
mem->uordblks = 0;
|
||||
mem->mxordblk = 0;
|
||||
|
||||
for (status = 0, page = 0; status >= 0; page++) {
|
||||
status = up_progmem_ispageerased(page);
|
||||
pagesize = up_progmem_pagesize(page);
|
||||
|
||||
mem->arena += pagesize;
|
||||
|
||||
/* Is this beginning of new free space section */
|
||||
|
||||
if (status == 0) {
|
||||
if (stpage == 0xFFFF) { stpage = page; }
|
||||
|
||||
mem->fordblks += pagesize;
|
||||
|
||||
} else if (status != 0) {
|
||||
mem->uordblks += pagesize;
|
||||
|
||||
if (stpage != 0xFFFF && up_progmem_isuniform()) {
|
||||
stpage = page - stpage;
|
||||
|
||||
if (stpage > (size_t) mem->mxordblk) {
|
||||
mem->mxordblk = stpage;
|
||||
}
|
||||
|
||||
stpage = 0xFFFF;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
mem->mxordblk *= pagesize;
|
||||
}
|
||||
|
||||
void free_check(void)
|
||||
{
|
||||
struct mallinfo data;
|
||||
struct mallinfo prog;
|
||||
|
||||
#ifdef CONFIG_CAN_PASS_STRUCTS
|
||||
data = mallinfo();
|
||||
#else
|
||||
(void)mallinfo(&data);
|
||||
#endif
|
||||
|
||||
free_getprogmeminfo(&prog);
|
||||
|
||||
|
||||
::syslog(LOG_INFO, " total used free largest\n");
|
||||
|
||||
::syslog(LOG_INFO, "Data: %11d%11d%11d%11d\n",
|
||||
data.arena, data.uordblks, data.fordblks, data.mxordblk);
|
||||
|
||||
::syslog(LOG_INFO, "Prog: %11d%11d%11d%11d\n",
|
||||
prog.arena, prog.uordblks, prog.fordblks, prog.mxordblk);
|
||||
}
|
||||
#endif /* CONFIG_NSH_LIBRARY */
|
|
@ -1,156 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane<david_s5@nscdg.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
|
||||
#include <syslog.h>
|
||||
|
||||
#include "sim_controller.hpp"
|
||||
#include <uavcan/equipment/esc/RawCommand.hpp>
|
||||
#include <uavcan/equipment/esc/RPMCommand.hpp>
|
||||
#include <uavcan/equipment/esc/Status.hpp>
|
||||
#include "led.hpp"
|
||||
|
||||
|
||||
uavcan::Publisher<uavcan::equipment::esc::Status> *pub_status;
|
||||
namespace
|
||||
{
|
||||
unsigned self_index = 0;
|
||||
int rpm = 0;
|
||||
|
||||
static void cb_raw_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RawCommand> &msg)
|
||||
{
|
||||
if (msg.cmd.size() <= self_index) {
|
||||
rgb_led(0, 0, 0, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
const float scaled = msg.cmd[self_index] / float(
|
||||
uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max());
|
||||
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "scaled:%d\n", (int)scaled);
|
||||
}
|
||||
|
||||
if (scaled > 0) {
|
||||
} else {
|
||||
}
|
||||
}
|
||||
|
||||
static void cb_rpm_command(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::RPMCommand> &msg)
|
||||
{
|
||||
if (msg.rpm.size() <= self_index) {
|
||||
return;
|
||||
}
|
||||
|
||||
rpm = msg.rpm[self_index];
|
||||
static int c = 0;
|
||||
|
||||
if (c++ % 100 == 0) {
|
||||
::syslog(LOG_INFO, "rpm:%d\n", rpm);
|
||||
}
|
||||
|
||||
if (rpm > 0) {
|
||||
rgb_led(255, 0, 0, rpm);
|
||||
|
||||
} else {
|
||||
rgb_led(0, 0, 0, 0);
|
||||
}
|
||||
}
|
||||
|
||||
void cb_10Hz(const uavcan::TimerEvent &event)
|
||||
{
|
||||
uavcan::equipment::esc::Status msg;
|
||||
|
||||
msg.esc_index = self_index;
|
||||
msg.rpm = rpm;
|
||||
msg.voltage = 3.3F;
|
||||
msg.current = 0.001F;
|
||||
msg.temperature = 24.0F;
|
||||
msg.power_rating_pct = static_cast<unsigned>(.5F * 100 + 0.5F);
|
||||
msg.error_count = 0;
|
||||
|
||||
if (rpm != 0) {
|
||||
// Lower the publish rate to 1Hz if the motor is not running
|
||||
static uavcan::MonotonicTime prev_pub_ts;
|
||||
|
||||
if ((event.scheduled_time - prev_pub_ts).toMSec() >= 990) {
|
||||
prev_pub_ts = event.scheduled_time;
|
||||
pub_status->broadcast(msg);
|
||||
}
|
||||
|
||||
} else {
|
||||
pub_status->broadcast(msg);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
int init_sim_controller(uavcan::INode &node)
|
||||
{
|
||||
|
||||
typedef void (*cb)(const uavcan::TimerEvent &);
|
||||
static uavcan::Subscriber<uavcan::equipment::esc::RawCommand> sub_raw_command(node);
|
||||
static uavcan::Subscriber<uavcan::equipment::esc::RPMCommand> sub_rpm_command(node);
|
||||
static uavcan::TimerEventForwarder<cb> timer_10hz(node);
|
||||
|
||||
self_index = 0;
|
||||
|
||||
int res = 0;
|
||||
|
||||
res = sub_raw_command.start(cb_raw_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
res = sub_rpm_command.start(cb_rpm_command);
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
pub_status = new uavcan::Publisher<uavcan::equipment::esc::Status>(node);
|
||||
res = pub_status->init();
|
||||
|
||||
if (res != 0) {
|
||||
return res;
|
||||
}
|
||||
|
||||
timer_10hz.setCallback(&cb_10Hz);
|
||||
timer_10hz.startPeriodic(uavcan::MonotonicDuration::fromMSec(100));
|
||||
|
||||
return 0;
|
||||
}
|
|
@ -1,8 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2015 PX4 Development Team. All rights reserved.
|
||||
* Author: Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane<david_s5@nscdg.com>
|
||||
* Copyright (C) 2018 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -33,8 +31,15 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
|
||||
int init_sim_controller(uavcan::INode &node);
|
||||
#if defined(UAVCAN_KINETIS_NUTTX)
|
||||
# include <uavcan_kinetis/uavcan_kinetis.hpp>
|
||||
#elif defined(UAVCAN_STM32_NUTTX)
|
||||
# include <uavcan_stm32/uavcan_stm32.hpp>
|
||||
#else
|
||||
# error "Unsupported driver"
|
||||
#endif
|
|
@ -1,590 +0,0 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
#ifdef __PX4_NUTTX
|
||||
#include <nuttx/clock.h>
|
||||
#
|
||||
#else
|
||||
#include <px4_platform_common/workqueue.h>
|
||||
#endif
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cstring>
|
||||
#include <fcntl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <parameters/param.h>
|
||||
#include <lib/mixer/MixerGroup.hpp>
|
||||
#include <version/version.h>
|
||||
__BEGIN_DECLS
|
||||
#include <nuttx/board.h>
|
||||
#include <arch/chip/chip.h>
|
||||
__END_DECLS
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <drivers/drv_pwm_output.h>
|
||||
|
||||
#include "uavcannode_main.hpp"
|
||||
#include "indication_controller.hpp"
|
||||
#include "sim_controller.hpp"
|
||||
#include "resources.hpp"
|
||||
#include "led.hpp"
|
||||
|
||||
#include "boot_app_shared.h"
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
/**
|
||||
* @file uavcan_main.cpp
|
||||
*
|
||||
* Implements basic functionality of UAVCAN node.
|
||||
*
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
* David Sidrane <david_s5@nscdg.com>
|
||||
*/
|
||||
|
||||
#define RESOURCE_DEBUG
|
||||
#if defined(RESOURCE_DEBUG)
|
||||
#define resources(s) ::syslog(LOG_INFO," %s\n",(s)); \
|
||||
if (UavcanNode::instance()) { \
|
||||
syslog(LOG_INFO,"UAVCAN getPeakNumUsedBlocks() in bytes %d\n", \
|
||||
UAVCAN_MEM_POOL_BLOCK_SIZE * UavcanNode::instance()->get_node().getAllocator().getPeakNumUsedBlocks()); \
|
||||
} \
|
||||
free_check(); \
|
||||
stack_check();
|
||||
#else
|
||||
#define resources(s)
|
||||
#endif
|
||||
|
||||
/*
|
||||
* This is the AppImageDescriptor used
|
||||
* by the make_can_boot_descriptor.py tool to set
|
||||
* the application image's descriptor so that the
|
||||
* uavcan bootloader has the ability to validate the
|
||||
* image crc, size etc of this application
|
||||
*/
|
||||
|
||||
boot_app_shared_section app_descriptor_t AppDescriptor = {
|
||||
.signature = {APP_DESCRIPTOR_SIGNATURE},
|
||||
.image_crc = 0,
|
||||
.image_size = 0,
|
||||
.vcs_commit = 0,
|
||||
.major_version = APP_VERSION_MAJOR,
|
||||
.minor_version = APP_VERSION_MINOR,
|
||||
.reserved = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff }
|
||||
};
|
||||
|
||||
/*
|
||||
* UavcanNode
|
||||
*/
|
||||
UavcanNode *UavcanNode::_instance;
|
||||
|
||||
UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
|
||||
CDev(UAVCAN_DEVICE_PATH),
|
||||
active_bitrate(0),
|
||||
_node(can_driver, system_clock),
|
||||
_node_mutex(),
|
||||
_time_sync_slave(_node),
|
||||
_fw_update_listner(_node),
|
||||
_reset_timer(_node)
|
||||
{
|
||||
const int res = pthread_mutex_init(&_node_mutex, nullptr);
|
||||
|
||||
if (res < 0) {
|
||||
std::abort();
|
||||
}
|
||||
|
||||
px4_sem_init(&_sem, 0, 0);
|
||||
/* semaphore use case is a signal */
|
||||
px4_sem_setprotocol(&_sem, SEM_PRIO_NONE);
|
||||
}
|
||||
|
||||
UavcanNode::~UavcanNode()
|
||||
{
|
||||
if (_task != -1) {
|
||||
/* tell the task we want it to go away */
|
||||
_task_should_exit = true;
|
||||
|
||||
unsigned i = 10;
|
||||
|
||||
do {
|
||||
/* wait 5ms - it should wake every 10ms or so worst-case */
|
||||
::usleep(5000);
|
||||
|
||||
/* if we have given up, kill it */
|
||||
if (--i == 0) {
|
||||
task_delete(_task);
|
||||
break;
|
||||
}
|
||||
|
||||
} while (_task != -1);
|
||||
}
|
||||
|
||||
_instance = nullptr;
|
||||
px4_sem_destroy(&_sem);
|
||||
}
|
||||
|
||||
int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
|
||||
{
|
||||
|
||||
|
||||
if (_instance != nullptr) {
|
||||
warnx("Already started");
|
||||
return -1;
|
||||
}
|
||||
|
||||
/*
|
||||
* CAN driver init
|
||||
*/
|
||||
static CanInitHelper can;
|
||||
static bool can_initialized = false;
|
||||
|
||||
if (!can_initialized) {
|
||||
const int can_init_res = can.init(bitrate);
|
||||
|
||||
if (can_init_res < 0) {
|
||||
warnx("CAN driver init failed %i", can_init_res);
|
||||
return can_init_res;
|
||||
}
|
||||
|
||||
can_initialized = true;
|
||||
}
|
||||
|
||||
/*
|
||||
* Node init
|
||||
*/
|
||||
_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
|
||||
|
||||
if (_instance == nullptr) {
|
||||
warnx("Out of memory");
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
resources("Before _instance->init:");
|
||||
const int node_init_res = _instance->init(node_id, can.driver.updateEvent());
|
||||
resources("After _instance->init:");
|
||||
|
||||
if (node_init_res < 0) {
|
||||
delete _instance;
|
||||
_instance = nullptr;
|
||||
warnx("Node init failed %i", node_init_res);
|
||||
return node_init_res;
|
||||
}
|
||||
|
||||
|
||||
/* Keep the bit rate for reboots on BenginFirmware updates */
|
||||
|
||||
_instance->active_bitrate = bitrate;
|
||||
|
||||
/*
|
||||
* Start the task. Normally it should never exit.
|
||||
*/
|
||||
static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
|
||||
_instance->_task = px4_task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
|
||||
static_cast<main_t>(run_trampoline), nullptr);
|
||||
|
||||
if (_instance->_task < 0) {
|
||||
warnx("start failed: %d", errno);
|
||||
return -errno;
|
||||
}
|
||||
|
||||
return OK;
|
||||
}
|
||||
|
||||
void UavcanNode::fill_node_info()
|
||||
{
|
||||
/* software version */
|
||||
uavcan::protocol::SoftwareVersion swver;
|
||||
|
||||
// Extracting the first 8 hex digits of the git hash and converting them to int
|
||||
char fw_git_short[9] = {};
|
||||
std::memmove(fw_git_short, px4_firmware_version_string(), 8);
|
||||
char *end = nullptr;
|
||||
swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
|
||||
swver.optional_field_flags |= swver.OPTIONAL_FIELD_FLAG_VCS_COMMIT;
|
||||
swver.major = AppDescriptor.major_version;
|
||||
swver.minor = AppDescriptor.minor_version;
|
||||
swver.image_crc = AppDescriptor.image_crc;
|
||||
|
||||
warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
|
||||
|
||||
_node.setSoftwareVersion(swver);
|
||||
|
||||
/* hardware version */
|
||||
uavcan::protocol::HardwareVersion hwver;
|
||||
|
||||
hwver.major = HW_VERSION_MAJOR;
|
||||
hwver.minor = HW_VERSION_MINOR;
|
||||
|
||||
mfguid_t mfgid = {};
|
||||
board_get_mfguid(mfgid);
|
||||
uavcan::copy(mfgid, mfgid + sizeof(mfgid), hwver.unique_id.begin());
|
||||
|
||||
_node.setHardwareVersion(hwver);
|
||||
}
|
||||
|
||||
static void cb_reboot(const uavcan::TimerEvent &)
|
||||
{
|
||||
px4_systemreset(false);
|
||||
|
||||
}
|
||||
|
||||
void UavcanNode::cb_beginfirmware_update(const uavcan::ReceivedDataStructure<UavcanNode::BeginFirmwareUpdate::Request>
|
||||
&req,
|
||||
uavcan::ServiceResponseDataStructure<UavcanNode::BeginFirmwareUpdate::Response> &rsp)
|
||||
{
|
||||
static bool inprogress = false;
|
||||
|
||||
rsp.error = rsp.ERROR_UNKNOWN;
|
||||
|
||||
if (req.image_file_remote_path.path.size()) {
|
||||
rsp.error = rsp.ERROR_IN_PROGRESS;
|
||||
|
||||
if (!inprogress) {
|
||||
inprogress = true;
|
||||
bootloader_app_shared_t shared;
|
||||
shared.bus_speed = active_bitrate;
|
||||
shared.node_id = _node.getNodeID().get();
|
||||
bootloader_app_shared_write(&shared, App);
|
||||
rgb_led(255, 128, 0, 5);
|
||||
_reset_timer.setCallback(cb_reboot);
|
||||
_reset_timer.startOneShotWithDelay(uavcan::MonotonicDuration::fromMSec(1000));
|
||||
rsp.error = rsp.ERROR_OK;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int UavcanNode::init(uavcan::NodeID node_id, uavcan_stm32::BusEvent &bus_events)
|
||||
{
|
||||
int ret = -1;
|
||||
|
||||
// Do regular cdev init
|
||||
ret = CDev::init();
|
||||
|
||||
if (ret != OK) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
_node.setName(HW_UAVCAN_NAME);
|
||||
|
||||
_node.setNodeID(node_id);
|
||||
|
||||
fill_node_info();
|
||||
|
||||
const int srv_start_res = _fw_update_listner.start(BeginFirmwareUpdateCallBack(this,
|
||||
&UavcanNode::cb_beginfirmware_update));
|
||||
|
||||
if (srv_start_res < 0) {
|
||||
return ret;
|
||||
}
|
||||
|
||||
bus_events.registerSignalCallback(UavcanNode::busevent_signal_trampoline);
|
||||
|
||||
return _node.start();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Restart handler
|
||||
*/
|
||||
class RestartRequestHandler: public uavcan::IRestartRequestHandler
|
||||
{
|
||||
bool handleRestartRequest(uavcan::NodeID request_source) override
|
||||
{
|
||||
::syslog(LOG_INFO, "UAVCAN: Restarting by request from %i\n", int(request_source.get()));
|
||||
::usleep(20 * 1000 * 1000);
|
||||
px4_systemreset(false);
|
||||
return true; // Will never be executed BTW
|
||||
}
|
||||
} restart_request_handler;
|
||||
|
||||
void UavcanNode::node_spin_once()
|
||||
{
|
||||
const int spin_res = _node.spin(uavcan::MonotonicTime());
|
||||
|
||||
if (spin_res < 0) {
|
||||
warnx("node spin error %i", spin_res);
|
||||
}
|
||||
}
|
||||
|
||||
static void signal_callback(void *arg)
|
||||
{
|
||||
/* Note: we are in IRQ context here */
|
||||
px4_sem_t *sem = (px4_sem_t *)arg;
|
||||
int semaphore_value;
|
||||
|
||||
if (px4_sem_getvalue(sem, &semaphore_value) == 0 && semaphore_value > 1) {
|
||||
return;
|
||||
}
|
||||
|
||||
px4_sem_post(sem);
|
||||
}
|
||||
|
||||
|
||||
void
|
||||
UavcanNode::busevent_signal_trampoline()
|
||||
{
|
||||
if (_instance) {
|
||||
signal_callback(&_instance->_sem);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
int UavcanNode::run()
|
||||
{
|
||||
|
||||
get_node().setRestartRequestHandler(&restart_request_handler);
|
||||
|
||||
while (init_indication_controller(get_node()) < 0) {
|
||||
::syslog(LOG_INFO, "UAVCAN: Indication controller init failed\n");
|
||||
::sleep(1);
|
||||
}
|
||||
|
||||
while (init_sim_controller(get_node()) < 0) {
|
||||
::syslog(LOG_INFO, "UAVCAN: sim controller init failed\n");
|
||||
::sleep(1);
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
/*
|
||||
* Set up the time synchronization
|
||||
*/
|
||||
|
||||
const int slave_init_res = _time_sync_slave.start();
|
||||
|
||||
if (slave_init_res < 0) {
|
||||
warnx("Failed to start time_sync_slave");
|
||||
_task_should_exit = true;
|
||||
}
|
||||
|
||||
_node.setModeOperational();
|
||||
|
||||
uint32_t start_tick = clock_systimer();
|
||||
|
||||
hrt_call timer_call{};
|
||||
hrt_call_every(&timer_call, 50_ms, 50_ms, signal_callback, &_sem);
|
||||
|
||||
while (!_task_should_exit) {
|
||||
// Mutex is unlocked while the thread is blocked on IO multiplexing
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
while (px4_sem_wait(&_sem) != 0);
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
node_spin_once(); // Non-blocking
|
||||
|
||||
|
||||
// Do Something
|
||||
|
||||
|
||||
if (clock_systimer() - start_tick > TICK_PER_SEC) {
|
||||
start_tick = clock_systimer();
|
||||
resources("Udate:");
|
||||
/*
|
||||
* Printing the slave status information once a second
|
||||
*/
|
||||
const bool active = _time_sync_slave.isActive(); // Whether it can sync with a remote master
|
||||
|
||||
const int master_node_id = _time_sync_slave.getMasterNodeID().get(); // Returns an invalid Node ID if (active == false)
|
||||
|
||||
const long msec_since_last_adjustment = (_node.getMonotonicTime() - _time_sync_slave.getLastAdjustmentTime()).toMSec();
|
||||
const uavcan::UtcTime utc = uavcan_stm32::clock::getUtc();
|
||||
syslog(LOG_INFO, "Time:%lld\n"
|
||||
" Time sync slave status:\n"
|
||||
" Active: %d\n"
|
||||
" Master Node ID: %d\n"
|
||||
" Last clock adjustment was %ld ms ago\n",
|
||||
utc .toUSec(), int(active), master_node_id, msec_since_last_adjustment);
|
||||
syslog(LOG_INFO, "UTC %lu sec Rate corr: %fPPM Jumps: %lu Locked: %i\n\n",
|
||||
static_cast<unsigned long>(utc.toMSec() / 1000),
|
||||
static_cast<double>(uavcan_stm32::clock::getUtcRateCorrectionPPM()),
|
||||
uavcan_stm32::clock::getUtcJumpCount(),
|
||||
int(uavcan_stm32::clock::isUtcLocked()));
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
hrt_cancel(&timer_call);
|
||||
teardown();
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
|
||||
exit(0);
|
||||
}
|
||||
|
||||
int
|
||||
UavcanNode::teardown()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
|
||||
int
|
||||
UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
|
||||
{
|
||||
int ret = OK;
|
||||
|
||||
lock();
|
||||
|
||||
switch (cmd) {
|
||||
|
||||
|
||||
|
||||
default:
|
||||
ret = -ENOTTY;
|
||||
break;
|
||||
}
|
||||
|
||||
unlock();
|
||||
|
||||
if (ret == -ENOTTY) {
|
||||
ret = CDev::ioctl(filp, cmd, arg);
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
void
|
||||
UavcanNode::print_info()
|
||||
{
|
||||
if (!_instance) {
|
||||
warnx("not running, start first");
|
||||
}
|
||||
|
||||
(void)pthread_mutex_lock(&_node_mutex);
|
||||
|
||||
|
||||
(void)pthread_mutex_unlock(&_node_mutex);
|
||||
}
|
||||
|
||||
/*
|
||||
* App entry point
|
||||
*/
|
||||
static void print_usage()
|
||||
{
|
||||
warnx("usage: \n"
|
||||
"\tuavcannode {start|status|stop|arm|disarm}");
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_start(int argc, char *argv[]);
|
||||
|
||||
int uavcannode_start(int argc, char *argv[])
|
||||
{
|
||||
resources("Before board_app_initialize");
|
||||
|
||||
board_app_initialize(NULL);
|
||||
|
||||
resources("After board_app_initialize");
|
||||
|
||||
// CAN bitrate
|
||||
int32_t bitrate = 0;
|
||||
// Node ID
|
||||
int32_t node_id = 0;
|
||||
|
||||
// Did the bootloader auto baud and get a node ID Allocated
|
||||
|
||||
bootloader_app_shared_t shared;
|
||||
int valid = bootloader_app_shared_read(&shared, BootLoader);
|
||||
|
||||
if (valid == 0) {
|
||||
|
||||
bitrate = shared.bus_speed;
|
||||
node_id = shared.node_id;
|
||||
|
||||
// Invalidate to prevent deja vu
|
||||
|
||||
bootloader_app_shared_invalidate();
|
||||
|
||||
} else {
|
||||
|
||||
// Node ID
|
||||
(void)param_get(param_find("CANNODE_NODE_ID"), &node_id);
|
||||
(void)param_get(param_find("CANNODE_BITRATE"), &bitrate);
|
||||
}
|
||||
|
||||
if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
|
||||
warnx("Invalid Node ID %i", node_id);
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
// Start
|
||||
warnx("Node ID %u, bitrate %u", node_id, bitrate);
|
||||
int rv = UavcanNode::start(node_id, bitrate);
|
||||
resources("After UavcanNode::start");
|
||||
::sleep(1);
|
||||
return rv;
|
||||
}
|
||||
|
||||
extern "C" __EXPORT int uavcannode_main(int argc, char *argv[]);
|
||||
int uavcannode_main(int argc, char *argv[])
|
||||
{
|
||||
if (argc < 2) {
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "start")) {
|
||||
|
||||
if (UavcanNode::instance()) {
|
||||
errx(1, "already started");
|
||||
}
|
||||
|
||||
return uavcannode_start(argc, argv);
|
||||
}
|
||||
|
||||
/* commands below require the app to be started */
|
||||
UavcanNode *const inst = UavcanNode::instance();
|
||||
|
||||
if (!inst) {
|
||||
errx(1, "application not running");
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
|
||||
inst->print_info();
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
if (!std::strcmp(argv[1], "stop")) {
|
||||
delete inst;
|
||||
::exit(0);
|
||||
}
|
||||
|
||||
print_usage();
|
||||
::exit(1);
|
||||
}
|
|
@ -1,6 +1,6 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2014 PX4 Development Team. All rights reserved.
|
||||
* Copyright (C) 2014-2020 PX4 Development Team. All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
|
@ -31,13 +31,6 @@
|
|||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @author Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <px4_platform_common/px4_config.h>
|
||||
#include <parameters/param.h>
|
||||
|
||||
/**
|
||||
* UAVCAN Node ID.
|
||||
*
|
||||
|
|
|
@ -37,6 +37,7 @@ px4_add_library(drivers_accelerometer
|
|||
)
|
||||
target_link_libraries(drivers_accelerometer
|
||||
PRIVATE
|
||||
conversion
|
||||
drivers__device
|
||||
mathlib
|
||||
)
|
||||
)
|
||||
|
|
|
@ -35,4 +35,4 @@ px4_add_library(drivers_gyroscope
|
|||
PX4Gyroscope.cpp
|
||||
PX4Gyroscope.hpp
|
||||
)
|
||||
target_link_libraries(drivers_gyroscope PRIVATE drivers__device)
|
||||
target_link_libraries(drivers_gyroscope PRIVATE conversion drivers__device)
|
||||
|
|
|
@ -60,15 +60,10 @@
|
|||
|
||||
using namespace time_literals;
|
||||
|
||||
//#define PARAM_NO_ORB ///< if defined, avoid uorb dependency. This disables publication of parameter_update on param change
|
||||
//#define PARAM_NO_AUTOSAVE ///< if defined, do not autosave (avoids LP work queue dependency)
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
# include "uORB/uORB.h"
|
||||
# include "uORB/topics/parameter_update.h"
|
||||
# include <uORB/topics/actuator_armed.h>
|
||||
# include <uORB/Subscription.hpp>
|
||||
#endif
|
||||
#include "uORB/uORB.h"
|
||||
#include "uORB/topics/parameter_update.h"
|
||||
#include <uORB/topics/actuator_armed.h>
|
||||
#include <uORB/Subscription.hpp>
|
||||
|
||||
#if defined(FLASH_BASED_PARAMS)
|
||||
#include "flashparams/flashparams.h"
|
||||
|
@ -90,14 +85,12 @@ static char *param_user_file = nullptr;
|
|||
#define PARAM_CLOSE close
|
||||
#endif
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
#include <px4_platform_common/workqueue.h>
|
||||
/* autosaving variables */
|
||||
static hrt_abstime last_autosave_timestamp = 0;
|
||||
static struct work_s autosave_work {};
|
||||
static volatile bool autosave_scheduled = false;
|
||||
static bool autosave_disabled = false;
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
/**
|
||||
* Array of static parameter info.
|
||||
|
@ -146,11 +139,9 @@ UT_array *param_values{nullptr};
|
|||
/** array info for the modified parameters array */
|
||||
const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr};
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
/** parameter update topic handle */
|
||||
static orb_advert_t param_topic = nullptr;
|
||||
static unsigned int param_instance = 0;
|
||||
#endif
|
||||
|
||||
static void param_set_used_internal(param_t param);
|
||||
|
||||
|
@ -300,7 +291,6 @@ param_find_changed(param_t param)
|
|||
static void
|
||||
_param_notify_changes()
|
||||
{
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
parameter_update_s pup = {};
|
||||
pup.timestamp = hrt_absolute_time();
|
||||
pup.instance = param_instance++;
|
||||
|
@ -315,8 +305,6 @@ _param_notify_changes()
|
|||
} else {
|
||||
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -606,7 +594,6 @@ param_get(param_t param, void *val)
|
|||
return result;
|
||||
}
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
/**
|
||||
* worker callback method to save the parameters
|
||||
* @param arg unused
|
||||
|
@ -616,8 +603,6 @@ autosave_worker(void *arg)
|
|||
{
|
||||
bool disabled = false;
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
|
||||
if (!param_get_default_file()) {
|
||||
// In case we save to FLASH, defer param writes until disarmed,
|
||||
// as writing to FLASH can stall the entire CPU (in rare cases around 300ms on STM32F7)
|
||||
|
@ -629,8 +614,6 @@ autosave_worker(void *arg)
|
|||
}
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
param_lock_writer();
|
||||
last_autosave_timestamp = hrt_absolute_time();
|
||||
autosave_scheduled = false;
|
||||
|
@ -648,7 +631,6 @@ autosave_worker(void *arg)
|
|||
PX4_ERR("param auto save failed (%i)", ret);
|
||||
}
|
||||
}
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
/**
|
||||
* Automatically save the parameters after a timeout and limited rate.
|
||||
|
@ -659,8 +641,6 @@ autosave_worker(void *arg)
|
|||
static void
|
||||
param_autosave()
|
||||
{
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
|
||||
if (autosave_scheduled || autosave_disabled) {
|
||||
return;
|
||||
}
|
||||
|
@ -680,13 +660,11 @@ param_autosave()
|
|||
|
||||
autosave_scheduled = true;
|
||||
work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
}
|
||||
|
||||
void
|
||||
param_control_autosave(bool enable)
|
||||
{
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
param_lock_writer();
|
||||
|
||||
if (!enable && autosave_scheduled) {
|
||||
|
@ -696,7 +674,6 @@ param_control_autosave(bool enable)
|
|||
|
||||
autosave_disabled = !enable;
|
||||
param_unlock_writer();
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
}
|
||||
|
||||
static int
|
||||
|
@ -1410,15 +1387,12 @@ void param_print_status()
|
|||
utarray_len(param_values), param_values->n, param_values->n * sizeof(UT_icd));
|
||||
}
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on");
|
||||
|
||||
if (!autosave_disabled && (last_autosave_timestamp > 0)) {
|
||||
PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6);
|
||||
}
|
||||
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
perf_print_counter(param_export_perf);
|
||||
perf_print_counter(param_find_perf);
|
||||
perf_print_counter(param_get_perf);
|
||||
|
|
|
@ -59,13 +59,8 @@
|
|||
#include <perf/perf_counter.h>
|
||||
#include <systemlib/uthash/utarray.h>
|
||||
|
||||
//#define PARAM_NO_ORB ///< if defined, avoid uorb dependency. This disables publication of parameter_update on param change
|
||||
//#define PARAM_NO_AUTOSAVE ///< if defined, do not autosave (avoids LP work queue dependency)
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
# include "uORB/uORB.h"
|
||||
# include "uORB/topics/parameter_update.h"
|
||||
#endif
|
||||
#include "uORB/uORB.h"
|
||||
#include "uORB/topics/parameter_update.h"
|
||||
|
||||
#if defined(FLASH_BASED_PARAMS)
|
||||
#include "flashparams/flashparams.h"
|
||||
|
@ -90,14 +85,12 @@ static char *param_user_file = nullptr;
|
|||
#endif
|
||||
#define PARAM_CLOSE close
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
#include <px4_platform_common/workqueue.h>
|
||||
/* autosaving variables */
|
||||
static hrt_abstime last_autosave_timestamp = 0;
|
||||
static struct work_s autosave_work;
|
||||
static bool autosave_scheduled = false;
|
||||
static bool autosave_disabled = false;
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
/**
|
||||
* Array of static parameter info.
|
||||
|
@ -156,11 +149,8 @@ UT_array *param_values{nullptr};
|
|||
/** array info for the modified parameters array */
|
||||
const UT_icd param_icd = {sizeof(param_wbuf_s), nullptr, nullptr, nullptr};
|
||||
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
/** parameter update topic handle */
|
||||
static orb_advert_t param_topic = nullptr;
|
||||
static unsigned int param_instance = 0;
|
||||
#endif
|
||||
|
||||
static void param_set_used_internal(param_t param);
|
||||
|
||||
|
@ -330,7 +320,6 @@ param_find_changed(param_t param)
|
|||
static void
|
||||
_param_notify_changes()
|
||||
{
|
||||
#if !defined(PARAM_NO_ORB)
|
||||
parameter_update_s pup = {};
|
||||
pup.timestamp = hrt_absolute_time();
|
||||
pup.instance = param_instance++;
|
||||
|
@ -345,8 +334,6 @@ _param_notify_changes()
|
|||
} else {
|
||||
orb_publish(ORB_ID(parameter_update), param_topic, &pup);
|
||||
}
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
void
|
||||
|
@ -663,7 +650,6 @@ param_get(param_t param, void *val)
|
|||
return result;
|
||||
}
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
/**
|
||||
* worker callback method to save the parameters
|
||||
* @param arg unused
|
||||
|
@ -690,7 +676,6 @@ autosave_worker(void *arg)
|
|||
PX4_ERR("param save failed (%i)", ret);
|
||||
}
|
||||
}
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
/**
|
||||
* Automatically save the parameters after a timeout and limited rate.
|
||||
|
@ -701,8 +686,6 @@ autosave_worker(void *arg)
|
|||
static void
|
||||
param_autosave()
|
||||
{
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
|
||||
if (autosave_scheduled || autosave_disabled) {
|
||||
return;
|
||||
}
|
||||
|
@ -722,13 +705,11 @@ param_autosave()
|
|||
|
||||
autosave_scheduled = true;
|
||||
work_queue(LPWORK, &autosave_work, (worker_t)&autosave_worker, nullptr, USEC2TICK(delay));
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
}
|
||||
|
||||
void
|
||||
param_control_autosave(bool enable)
|
||||
{
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
param_lock_writer();
|
||||
|
||||
if (!enable && autosave_scheduled) {
|
||||
|
@ -738,7 +719,6 @@ param_control_autosave(bool enable)
|
|||
|
||||
autosave_disabled = !enable;
|
||||
param_unlock_writer();
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
}
|
||||
|
||||
static int
|
||||
|
@ -1497,15 +1477,12 @@ void param_print_status()
|
|||
utarray_len(param_values), param_values->n, param_values->n * sizeof(UT_icd));
|
||||
}
|
||||
|
||||
#ifndef PARAM_NO_AUTOSAVE
|
||||
PX4_INFO("auto save: %s", autosave_disabled ? "off" : "on");
|
||||
|
||||
if (!autosave_disabled && (last_autosave_timestamp > 0)) {
|
||||
PX4_INFO("last auto save: %.3f seconds ago", hrt_elapsed_time(&last_autosave_timestamp) * 1e-6);
|
||||
}
|
||||
|
||||
#endif /* PARAM_NO_AUTOSAVE */
|
||||
|
||||
perf_print_counter(param_export_perf);
|
||||
perf_print_counter(param_find_perf);
|
||||
perf_print_counter(param_get_perf);
|
||||
|
|
|
@ -35,4 +35,9 @@ px4_add_library(vehicle_acceleration
|
|||
VehicleAcceleration.cpp
|
||||
VehicleAcceleration.hpp
|
||||
)
|
||||
target_link_libraries(vehicle_acceleration PRIVATE sensor_corrections px4_work_queue)
|
||||
target_link_libraries(vehicle_acceleration
|
||||
PRIVATE
|
||||
mathlib
|
||||
sensor_corrections
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
|
@ -35,4 +35,9 @@ px4_add_library(vehicle_angular_velocity
|
|||
VehicleAngularVelocity.cpp
|
||||
VehicleAngularVelocity.hpp
|
||||
)
|
||||
target_link_libraries(vehicle_angular_velocity PRIVATE sensor_corrections px4_work_queue)
|
||||
target_link_libraries(vehicle_angular_velocity
|
||||
PRIVATE
|
||||
mathlib
|
||||
sensor_corrections
|
||||
px4_work_queue
|
||||
)
|
||||
|
|
Loading…
Reference in New Issue