Browse Source

enable px4 flight for excelsior(legacy)

sbg
wangxdflight 8 years ago committed by Lorenz Meier
parent
commit
b693e29d64
  1. 3
      Makefile
  2. 79
      cmake/configs/posix_eagle_legacy.cmake
  3. 3
      cmake/configs/posix_excelsior_default.cmake
  4. 14
      cmake/configs/posix_excelsior_legacy.cmake
  5. 70
      cmake/configs/posix_sdflight_legacy.cmake
  6. 114
      cmake/configs/qurt_eagle_legacy.cmake
  7. 14
      cmake/configs/qurt_excelsior_legacy.cmake
  8. 108
      cmake/configs/qurt_sdflight_legacy.cmake
  9. 32
      cmake/posix/px4_impl_posix.cmake
  10. 13
      posix-configs/excelsior/mainapp.config
  11. 92
      posix-configs/excelsior/px4.config
  12. 8
      src/modules/commander/accelerometer_calibration.cpp
  13. 6
      src/modules/commander/gyro_calibration.cpp
  14. 2
      src/modules/dataman/dataman.c
  15. 2
      src/modules/logger/logger.h
  16. 6
      src/modules/mavlink/mavlink_messages.cpp
  17. 2
      src/modules/mavlink/mavlink_orb_subscription.cpp
  18. 4
      src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp
  19. 4
      src/modules/sdlog2/sdlog2.c
  20. 2
      src/modules/uORB/uORBDevices.cpp
  21. 2
      src/modules/uORB/uORBMain.cpp
  22. 2
      src/platforms/posix/main.cpp
  23. 2
      src/platforms/px4_defines.h
  24. 37
      src/platforms/qurt/px4_layer/shmem_qurt.c

3
Makefile

@ -153,10 +153,11 @@ broadcast: posix_sitl_broadcast @@ -153,10 +153,11 @@ broadcast: posix_sitl_broadcast
eagle_default: posix_eagle_default qurt_eagle_default
eagle_legacy_default: posix_eagle_legacy qurt_eagle_legacy
excelsior_default: posix_excelsior_default qurt_excelsior_default
excelsior_legacy_default: posix_excelsior_legacy qurt_excelsior_legacy
# All targets with just dependencies but no recipe must either be marked as phony (or have the special @: as recipe).
.PHONY: all posix broadcast eagle_default eagle_legacy_default excelsior_default all_nuttx_targets
.PHONY: all posix broadcast eagle_default eagle_legacy_default excelsior_legacy_default excelsior_default all_nuttx_targets
# Other targets
# --------------------------------------------------------------------

79
cmake/configs/posix_eagle_legacy.cmake

@ -1,78 +1,15 @@ @@ -1,78 +1,15 @@
include(posix/px4_impl_posix)
# Eagle is the code name of a board currently in development.
#
# This cmake config builds for POSIX, so the part of the flight stack running
# on the Linux side of the Snapdragon.
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
set(config_generate_parameters_scope ALL)
# Get $QC_SOC_TARGET from environment if existing.
if (DEFINED ENV{QC_SOC_TARGET})
set(QC_SOC_TARGET $ENV{QC_SOC_TARGET})
else()
set(QC_SOC_TARGET "APQ8074")
endif()
include(configs/posix_sdflight_legacy)
set(CONFIG_SHMEM "1")
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-linux-gnueabihf.cmake)
# This definition allows to differentiate if this just the usual POSIX build
# or if it is for the Snapdragon.
add_definitions(
-D__PX4_POSIX_EAGLE
-D__PX4_POSIX_EAGLE
-D__USING_SNAPDRAGON_LEGACY_DRIVER
)
set(config_module_list
drivers/device
drivers/blinkm
drivers/pwm_out_sim
drivers/rgbled
drivers/led
drivers/boards/sitl
drivers/qshell/posix
systemcmds/param
systemcmds/mixer
systemcmds/ver
systemcmds/topic_listener
modules/mavlink
modules/attitude_estimator_q
modules/position_estimator_inav
modules/local_position_estimator
modules/ekf2
modules/mc_pos_control
modules/mc_att_control
modules/param
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/muorb/krait
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander
modules/navigator
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/conversion
lib/ecl
lib/geo
lib/geo_lookup
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
platforms/common
platforms/posix/px4_layer
platforms/posix/work_queue
)
)

3
cmake/configs/posix_excelsior_default.cmake

@ -3,8 +3,9 @@ @@ -3,8 +3,9 @@
# This cmake config builds for POSIX, so the part of the flight stack running
# on the Linux side of the Snapdragon.
include(configs/posix_sdflight_default)
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-oemllib32-linux-gnueabi.cmake)
# This definition allows to differentiate the specific board.
add_definitions(
-D__PX4_POSIX_EXCELSIOR
)
)

14
cmake/configs/posix_excelsior_legacy.cmake

@ -0,0 +1,14 @@ @@ -0,0 +1,14 @@
# Excelsior is the code name of a board currently in development.
#
# This cmake config builds for POSIX, so the part of the flight stack running
# on the Linux side of the Snapdragon.
include(configs/posix_sdflight_legacy)
set(CMAKE_TOOLCHAIN_FILE ${PX4_SOURCE_DIR}/cmake/cmake_hexagon/toolchain/Toolchain-arm-oemllib32-linux-gnueabi.cmake)
# This definition allows to differentiate if this just the usual POSIX build
# or if it is for the Snapdragon.
add_definitions(
-D__PX4_POSIX_EXCELSIOR
-D__USING_SNAPDRAGON_LEGACY_DRIVER
)

70
cmake/configs/posix_sdflight_legacy.cmake

@ -0,0 +1,70 @@ @@ -0,0 +1,70 @@
include(posix/px4_impl_posix)
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
set(config_generate_parameters_scope ALL)
# Get $QC_SOC_TARGET from environment if existing.
if (DEFINED ENV{QC_SOC_TARGET})
set(QC_SOC_TARGET $ENV{QC_SOC_TARGET})
else()
set(QC_SOC_TARGET "APQ8074")
endif()
set(CONFIG_SHMEM "1")
set(config_module_list
drivers/device
drivers/blinkm
drivers/pwm_out_sim
drivers/rgbled
drivers/led
drivers/boards/sitl
drivers/qshell/posix
systemcmds/param
systemcmds/mixer
systemcmds/ver
systemcmds/topic_listener
modules/mavlink
modules/attitude_estimator_q
modules/position_estimator_inav
modules/local_position_estimator
modules/ekf2
modules/mc_pos_control
modules/mc_att_control
modules/param
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/muorb/krait
modules/sensors
modules/dataman
modules/sdlog2
modules/logger
modules/simulator
modules/commander
modules/navigator
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/conversion
lib/ecl
lib/geo
lib/geo_lookup
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
platforms/common
platforms/posix/px4_layer
platforms/posix/work_queue
)

114
cmake/configs/qurt_eagle_legacy.cmake

@ -1,114 +1,12 @@ @@ -1,114 +1,12 @@
include(qurt/px4_impl_qurt)
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
else()
set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
endif()
set(CONFIG_SHMEM "1")
set(config_generate_parameters_scope ALL)
# Get $QC_SOC_TARGET from environment if existing.
if (DEFINED ENV{QC_SOC_TARGET})
set(QC_SOC_TARGET $ENV{QC_SOC_TARGET})
else()
set(QC_SOC_TARGET "APQ8074")
endif()
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
include(toolchain/Toolchain-qurt)
include(qurt_flags)
include_directories(${HEXAGON_SDK_INCLUDES})
# Eagle is the code name of a board currently in development.
#
# This cmake config builds for QURT which is the operating system running on
# the DSP side.
# The config between different QURT builds is shared.
include(configs/qurt_sdflight_legacy)
add_definitions(
-D__USING_SNAPDRAGON_LEGACY_DRIVER
-D__PX4_QURT
-D__PX4_QURT_EAGLE
)
set(config_module_list
#
# Board support modules
#
drivers/device
modules/sensors
platforms/posix/drivers/df_mpu9250_wrapper
platforms/posix/drivers/df_bmp280_wrapper
#
# System commands
#
systemcmds/param
#
# Estimation modules
#
modules/attitude_estimator_q
modules/position_estimator_inav
modules/local_position_estimator
modules/ekf2
#
# Vehicle Control
#
modules/mc_att_control
modules/mc_pos_control
#
# Library modules
#
modules/param
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/commander
modules/land_detector
#
# PX4 drivers
#
drivers/gps
drivers/pwm_out_rc_in
drivers/qshell/qurt
#
# FC_ADDON drivers
#
platforms/qurt/fc_addon/rc_receiver
platforms/qurt/fc_addon/uart_esc
#
# Libraries
#
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/geo
lib/ecl
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
#
# QuRT port
#
platforms/common
platforms/qurt/px4_layer
platforms/posix/work_queue
#
# sources for muorb over fastrpc
#
modules/muorb/adsp
)
set(config_df_driver_list
mpu9250
bmp280
)

14
cmake/configs/qurt_excelsior_legacy.cmake

@ -0,0 +1,14 @@ @@ -0,0 +1,14 @@
# Excelsior is the code name of a board currently in development.
#
# This cmake config builds for QURT which is the operating system running on
# the DSP side.
# The config between different QURT builds is shared.
include(configs/qurt_sdflight_legacy)
add_definitions(
-D__USING_SNAPDRAGON_LEGACY_DRIVER
-D__PX4_QURT
-D__PX4_QURT_EXCELSIOR
)

108
cmake/configs/qurt_sdflight_legacy.cmake

@ -0,0 +1,108 @@ @@ -0,0 +1,108 @@
include(qurt/px4_impl_qurt)
if ("$ENV{HEXAGON_SDK_ROOT}" STREQUAL "")
message(FATAL_ERROR "Enviroment variable HEXAGON_SDK_ROOT must be set")
else()
set(HEXAGON_SDK_ROOT $ENV{HEXAGON_SDK_ROOT})
endif()
set(CONFIG_SHMEM "1")
set(config_generate_parameters_scope ALL)
# Get $QC_SOC_TARGET from environment if existing.
if (DEFINED ENV{QC_SOC_TARGET})
set(QC_SOC_TARGET $ENV{QC_SOC_TARGET})
else()
set(QC_SOC_TARGET "APQ8074")
endif()
set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${PX4_SOURCE_DIR}/cmake/cmake_hexagon")
include(toolchain/Toolchain-qurt)
include(qurt_flags)
include_directories(${HEXAGON_SDK_INCLUDES})
set(config_module_list
#
# Board support modules
#
drivers/device
modules/sensors
platforms/posix/drivers/df_mpu9250_wrapper
platforms/posix/drivers/df_bmp280_wrapper
#
# System commands
#
systemcmds/param
#
# Estimation modules
#
modules/attitude_estimator_q
modules/position_estimator_inav
modules/local_position_estimator
modules/ekf2
#
# Vehicle Control
#
modules/mc_att_control
modules/mc_pos_control
#
# Library modules
#
modules/param
modules/systemlib
modules/systemlib/mixer
modules/uORB
modules/commander
modules/land_detector
#
# PX4 drivers
#
drivers/gps
drivers/pwm_out_rc_in
drivers/qshell/qurt
#
# FC_ADDON drivers
#
platforms/qurt/fc_addon/rc_receiver
platforms/qurt/fc_addon/uart_esc
#
# Libraries
#
lib/controllib
lib/mathlib
lib/mathlib/math/filter
lib/geo
lib/ecl
lib/geo_lookup
lib/conversion
lib/terrain_estimation
lib/runway_takeoff
lib/tailsitter_recovery
lib/version
lib/DriverFramework/framework
#
# QuRT port
#
platforms/common
platforms/qurt/px4_layer
platforms/posix/work_queue
#
# sources for muorb over fastrpc
#
modules/muorb/adsp
)
set(config_df_driver_list
mpu9250
bmp280
)

32
cmake/posix/px4_impl_posix.cmake

@ -222,7 +222,7 @@ endif() @@ -222,7 +222,7 @@ endif()
set(added_exe_linker_flags)
# This block sets added_c_flags (appends to others).
if ("${BOARD}" STREQUAL "eagle" OR "${BOARD}" STREQUAL "excelsior")
if ("${BOARD}" STREQUAL "eagle")
if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
@ -239,9 +239,35 @@ if ("${BOARD}" STREQUAL "eagle" OR "${BOARD}" STREQUAL "excelsior") @@ -239,9 +239,35 @@ if ("${BOARD}" STREQUAL "eagle" OR "${BOARD}" STREQUAL "excelsior")
)
list(APPEND added_exe_linker_flags
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib/arm-linux-gnueabihf
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib/arm-linux-gnueabihf
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/usr/lib
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib
--sysroot=${HEXAGON_ARM_SYSROOT}
)
# This block sets added_c_flags (appends to others).
elseif ("${BOARD}" STREQUAL "excelsior")
if ("$ENV{HEXAGON_ARM_SYSROOT}" STREQUAL "")
message(FATAL_ERROR "HEXAGON_ARM_SYSROOT not set")
else()
set(HEXAGON_ARM_SYSROOT $ENV{HEXAGON_ARM_SYSROOT})
endif()
# Add the toolchain specific flags
set(added_cflags ${POSIX_CMAKE_C_FLAGS} --sysroot=${HEXAGON_ARM_SYSROOT}/lib32-apq8096 -mfloat-abi=softfp -mfpu=neon -mthumb-interwork)
list(APPEND added_cxx_flags
${POSIX_CMAKE_CXX_FLAGS}
--sysroot=${HEXAGON_ARM_SYSROOT}/lib32-apq8096 -mfloat-abi=softfp -mfpu=neon -mthumb-interwork
)
list(APPEND added_exe_linker_flags
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib32-apq8096/usr/lib
-Wl,-rpath-link,${HEXAGON_ARM_SYSROOT}/lib32-apq8096/lib
--sysroot=${HEXAGON_ARM_SYSROOT}/lib32-apq8096 -mfloat-abi=softfp -mfpu=neon -mthumb-interwork
)
elseif ("${BOARD}" STREQUAL "rpi" AND "$ENV{RPI_USE_CLANG}" STREQUAL "1")

13
posix-configs/excelsior/mainapp.config

@ -0,0 +1,13 @@ @@ -0,0 +1,13 @@
uorb start
muorb start
sdlog2 start -r 100 -e -t -a -b 200
param set MAV_BROADCAST 1
param set SDLOG_PRIO_BOOST 3
dataman start
navigator start
mavlink start -u 14556 -r 1000000
sleep 1
mavlink stream -u 14556 -s HIGHRES_IMU -r 50
mavlink stream -u 14556 -s ATTITUDE -r 50
mavlink stream -u 14556 -s RC_CHANNELS -r 20
mavlink boot_complete

92
posix-configs/excelsior/px4.config

@ -0,0 +1,92 @@ @@ -0,0 +1,92 @@
uorb start
qshell start
param set CAL_GYRO0_ID 100
param set CAL_ACC0_ID 100
param set CAL_MAG0_ID 101
param set SYS_AUTOSTART 4001
param set MAV_TYPE 2
param set MC_YAW_P 12
param set MC_YAWRATE_P 0.08
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_D 0
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.001
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.001
param set RC_MAP_THROTTLE 1
param set RC_MAP_ROLL 2
param set RC_MAP_PITCH 3
param set RC_MAP_YAW 4
param set RC_MAP_MODE_SW 5
param set RC_MAP_POSCTL_SW 6
param set RC1_MAX 1900
param set RC1_MIN 1099
param set RC1_TRIM 1099
param set RC1_REV 1
param set RC2_MAX 1900
param set RC2_MIN 1099
param set RC2_TRIM 1500
param set RC2_REV -1
param set RC3_MAX 1900
param set RC3_MIN 1099
param set RC3_TRIM 1500
param set RC3_REV 1
param set RC4_MAX 1900
param set RC4_MIN 1099
param set RC4_TRIM 1500
param set RC4_REV -1
param set RC5_MAX 1900
param set RC5_MIN 1099
param set RC5_TRIM 1500
param set RC5_REV 1
param set RC6_MAX 1900
param set RC6_MIN 1099
param set RC6_TRIM 1099
param set RC6_REV 1
param set ATT_W_MAG 0.00
param set SENS_BOARD_ROT 4
param set CAL_GYRO0_XOFF -0.0028
param set CAL_GYRO0_YOFF -0.0047
param set CAL_GYRO0_ZOFF -0.0034
param set CAL_GYRO0_XSCALE 1.0000
param set CAL_GYRO0_YSCALE 1.0000
param set CAL_GYRO0_ZSCALE 1.0000
param set CAL_MAG0_XOFF 0.0000
param set CAL_MAG0_YOFF 0.0000
param set CAL_MAG0_ZOFF 0.0000
param set CAL_MAG0_XSCALE 1.0000
param set CAL_MAG0_YSCALE 1.0000
param set CAL_MAG0_ZSCALE 1.0000
param set CAL_ACC0_XOFF -0.0941
param set CAL_ACC0_YOFF 0.1168
param set CAL_ACC0_ZOFF -0.0398
param set CAL_ACC0_XSCALE 1.0004
param set CAL_ACC0_YSCALE 0.9974
param set CAL_ACC0_ZSCALE 0.9951
param set RC_RECEIVER_TYPE 1
param set UART_ESC_MODEL 2
param set UART_ESC_BAUDRTE 250000
param set UART_ESC_MOTOR1 4
param set UART_ESC_MOTOR2 2
param set UART_ESC_MOTOR3 1
param set UART_ESC_MOTOR4 3
sleep 1
df_mpu9250_wrapper start
df_bmp280_wrapper start
sensors start
commander start
ekf2 start
land_detector start multicopter
mc_pos_control start
mc_att_control start
uart_esc start -D /dev/tty-1
rc_receiver start -D /dev/tty-101
sleep 1
list_devices
list_files
list_tasks
list_topics

8
src/modules/commander/accelerometer_calibration.cpp

@ -169,7 +169,7 @@ typedef struct { @@ -169,7 +169,7 @@ typedef struct {
int do_accel_calibration(orb_advert_t *mavlink_log_pub)
{
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
int fd;
#endif
@ -189,7 +189,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) @@ -189,7 +189,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
/* reset all offsets to zero and all scales to one */
fd = px4_open(str, 0);
@ -323,7 +323,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) @@ -323,7 +323,7 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
return PX4_ERROR;
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = px4_open(str, 0);
@ -413,7 +413,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub @@ -413,7 +413,7 @@ calibrate_return do_accel_calibration_measurements(orb_advert_t *mavlink_log_pub
break;
}
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)|| defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct accel_report accel_report;
orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &accel_report);

6
src/modules/commander/gyro_calibration.cpp

@ -180,7 +180,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -180,7 +180,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale_zero));
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
@ -233,7 +233,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -233,7 +233,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
for (unsigned s = 0; s < gyro_count; s++) {
worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR) || defined(__PX4_POSIX_RPI) || defined(__PX4_POSIX_BEBOP)
// For QURT respectively the driver framework, we need to get the device ID by copying one report.
struct gyro_report gyro_report;
orb_copy(ORB_ID(sensor_gyro), worker_data.gyro_sensor_sub[s], &gyro_report);
@ -331,7 +331,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub) @@ -331,7 +331,7 @@ int do_gyro_calibration(orb_advert_t *mavlink_log_pub)
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (PX4_OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR) && !defined(__PX4_POSIX_RPI) && !defined(__PX4_POSIX_BEBOP)
/* apply new scaling and offsets */
(void)sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);

2
src/modules/dataman/dataman.c

@ -177,7 +177,7 @@ static px4_sem_t g_sys_state_mutex; @@ -177,7 +177,7 @@ static px4_sem_t g_sys_state_mutex;
/* The data manager store file handle and file name */
static int g_fd = -1;
static int g_task_fd = -1;
#ifdef __PX4_POSIX_EAGLE
#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
static const char *default_device_path = PX4_ROOTFSDIR"/dataman";
#else
static const char *default_device_path = PX4_ROOTFSDIR"/fs/microsd/dataman";

2
src/modules/logger/logger.h

@ -222,7 +222,7 @@ private: @@ -222,7 +222,7 @@ private:
static constexpr size_t MAX_TOPICS_NUM = 64; /**< Maximum number of logged topics */
static constexpr unsigned MAX_NO_LOGFOLDER = 999; /**< Maximum number of log dirs */
static constexpr unsigned MAX_NO_LOGFILE = 999; /**< Maximum number of log files */
#ifdef __PX4_POSIX_EAGLE
#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/log";
#else
static constexpr const char *LOG_ROOT = PX4_ROOTFSDIR"/fs/microsd/log";

6
src/modules/mavlink/mavlink_messages.cpp

@ -377,7 +377,7 @@ private: @@ -377,7 +377,7 @@ private:
unsigned _write_err_count = 0;
static const unsigned write_err_threshold = 5;
#ifndef __PX4_POSIX_EAGLE
#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
FILE *_fp = nullptr;
#endif
@ -387,7 +387,7 @@ protected: @@ -387,7 +387,7 @@ protected:
~MavlinkStreamStatustext()
{
#ifndef __PX4_POSIX_EAGLE
#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
if (_fp != nullptr) {
fclose(_fp);
@ -413,7 +413,7 @@ protected: @@ -413,7 +413,7 @@ protected:
mavlink_msg_statustext_send_struct(_mavlink->get_channel(), &msg);
// TODO: the logging doesn't work on Snapdragon yet because of file paths.
#ifndef __PX4_POSIX_EAGLE
#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
/* write log messages in first instance to disk
* timestamp each message with gps time
*/

2
src/modules/mavlink/mavlink_orb_subscription.cpp

@ -170,7 +170,7 @@ MavlinkOrbSubscription::is_published() @@ -170,7 +170,7 @@ MavlinkOrbSubscription::is_published()
// We are checking now
_last_pub_check = now;
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE)
#if defined(__PX4_QURT) || defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
// Snapdragon has currently no support for orb_exists, therefore
// we're not using it.

4
src/modules/muorb/krait/px4muorb_KraitRpcWrapper.cpp

@ -84,6 +84,10 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us); @@ -84,6 +84,10 @@ int calc_timer_diff_to_dsp_us(int32_t *time_diff_us);
int calc_timer_diff_to_dsp_us(int32_t *time_diff_us)
{
#if defined(__PX4_POSIX_EXCELSIOR)
*time_diff_us = 0;
return 0;
#endif
int fd = open(DSP_TIMER_FILE, O_RDONLY);
if (fd < 0) {

4
src/modules/sdlog2/sdlog2.c

@ -156,7 +156,7 @@ static bool _extended_logging = false; @@ -156,7 +156,7 @@ static bool _extended_logging = false;
static bool _gpstime_only = false;
static int32_t _utc_offset = 0;
#ifndef __PX4_POSIX_EAGLE
#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
#define MOUNTPOINT PX4_ROOTFSDIR"/fs/microsd"
#else
#define MOUNTPOINT "/root"
@ -745,7 +745,7 @@ void sdlog2_start_log() @@ -745,7 +745,7 @@ void sdlog2_start_log()
/* initialize log buffer emptying thread */
pthread_attr_init(&logwriter_attr);
#ifndef __PX4_POSIX_EAGLE
#if !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
struct sched_param param;
(void)pthread_attr_getschedparam(&logwriter_attr, &param);
/* low priority, as this is expensive disk I/O. */

2
src/modules/uORB/uORBDevices.cpp

@ -449,6 +449,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v @@ -449,6 +449,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
return ERROR;
}
#if !defined(__PX4_QURT_EXCELSIOR) && !defined(__PX4_POSIX_EXCELSIOR)
/*
* if the write is successful, send the data over the Multi-ORB link
*/
@ -462,6 +463,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v @@ -462,6 +463,7 @@ uORB::DeviceNode::publish(const orb_metadata *meta, orb_advert_t handle, const v
}
}
#endif
return PX4_OK;
}

2
src/modules/uORB/uORBMain.cpp

@ -81,7 +81,7 @@ uorb_main(int argc, char *argv[]) @@ -81,7 +81,7 @@ uorb_main(int argc, char *argv[])
return -errno;
}
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE)
#if !defined(__PX4_QURT) && !defined(__PX4_POSIX_EAGLE) && !defined(__PX4_POSIX_EXCELSIOR)
/* FIXME: this fails on Snapdragon (see https://github.com/PX4/Firmware/issues/5406),
* so we disable logging messages to the ulog for now. This needs further investigations.
*/

2
src/platforms/posix/main.cpp

@ -273,7 +273,7 @@ bool px4_exit_requested(void) @@ -273,7 +273,7 @@ bool px4_exit_requested(void)
static void set_cpu_scaling()
{
#ifdef __PX4_POSIX_EAGLE
#if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
// On Snapdragon we miss updates in sdlog2 unless all 4 CPUs are run
// at the maximum frequency all the time.
// Interestingely, cpu0 and cpu3 set the scaling for all 4 CPUs on Snapdragon.

2
src/platforms/px4_defines.h

@ -192,7 +192,7 @@ __BEGIN_DECLS @@ -192,7 +192,7 @@ __BEGIN_DECLS
extern long PX4_TICKS_PER_SEC;
__END_DECLS
# if defined(__PX4_POSIX_EAGLE)
# if defined(__PX4_POSIX_EAGLE) || defined(__PX4_POSIX_EXCELSIOR)
# define PX4_ROOTFSDIR "/home/linaro"
# elif defined(__PX4_POSIX_BEBOP)
# define PX4_ROOTFSDIR "/data/ftp/internal_000"

37
src/platforms/qurt/px4_layer/shmem_qurt.c

@ -49,10 +49,11 @@ @@ -49,10 +49,11 @@
//#define SHMEM_DEBUG
//#define PARAM_LOCK_DEBUG
static atomic_word_t mem_lock;
int mem_fd;
unsigned char *map_base, *virt_addr;
struct shmem_info *shmem_info_p;
static void *map_memory(off_t target);
int get_shmem_lock(const char *caller_file_name, int caller_line_number);
void release_shmem_lock(const char *caller_file_name, int caller_line_number);
void init_shared_memory(void);
@ -82,24 +83,16 @@ struct param_wbuf_s { @@ -82,24 +83,16 @@ struct param_wbuf_s {
};
extern struct param_wbuf_s *param_find_changed(param_t param);
static void *map_memory(off_t target)
{
return (void *)(target + LOCK_SIZE);
}
int get_shmem_lock(const char *caller_file_name, int caller_line_number)
{
unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
unsigned int i = 0;
#ifdef PARAM_LOCK_DEBUG
PX4_INFO("lock value %d before get from %s, line: %d\n", *(unsigned int *)0xfbfc000, strrchr(caller_file_name, '/'),
PX4_INFO("lock value %d before get from %s, line: %d\n", mem_lock.value, strrchr(caller_file_name, '/'),
caller_line_number);
#endif
while (!atomic_compare_and_set(lock, 1, 0)) {
while (!atomic_compare_and_set(&mem_lock, 1, 0)) {
i++;
usleep(1000);
@ -124,9 +117,7 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number) @@ -124,9 +117,7 @@ int get_shmem_lock(const char *caller_file_name, int caller_line_number)
void release_shmem_lock(const char *caller_file_name, int caller_line_number)
{
unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
*lock = 1;
atomic_set(&mem_lock, 1);
#ifdef PARAM_LOCK_DEBUG
PX4_INFO("release lock, file name: %s, line number: %d.\n",
@ -138,25 +129,31 @@ void release_shmem_lock(const char *caller_file_name, int caller_line_number) @@ -138,25 +129,31 @@ void release_shmem_lock(const char *caller_file_name, int caller_line_number)
void init_shared_memory(void)
{
//PX4_INFO("Value at lock address is %d\n", *(unsigned int*)0xfbfc000);
int i;
if (shmem_info_p) {
return;
}
virt_addr = map_memory(MAP_ADDRESS);
//virt_addr = map_memory(MAP_ADDRESS);
map_base = malloc(0x4000); //16KB
if (map_base == NULL) {
PX4_INFO("adsp memory malloc failed\n");
return;
}
virt_addr = map_base;
shmem_info_p = (struct shmem_info *) virt_addr;
//init lock as 1
unsigned char *lock = (unsigned char *)(MAP_ADDRESS + LOCK_OFFSET);
*lock = 1;
atomic_init(&mem_lock, 1);
for (i = 0; i < MAX_SHMEM_PARAMS / 8 + 1; i++) {
shmem_info_p->krait_changed_index[i] = 0;
}
//PX4_INFO("adsp memory mapped\n");
PX4_INFO("adsp memory mapped\n");
}
void copy_params_to_shmem(struct param_info_s *param_info_base)

Loading…
Cancel
Save