dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

standalone external modes 

Signed-off-by: frederik <frederik@auterion.com>
This commit is contained in:
frederik 2023-11-14 13:56:21 +01:00
parent 8e0a2e38fe
commit 4e8554f0a6
7 changed files with 130 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

59
ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
View File

@ -38,6 +38,7 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
# set local coordinate frame reference
if [ -n "${PX4_HOME_LAT}" ]; then
param set SIM_GZ_HOME_LAT ${PX4_HOME_LAT}
@ -59,36 +60,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
. ../gz_env.sh
fi
# "gz sim" only avaiilable in Garden and later
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
then
# "gz sim" from Garden on
gz_command="gz"
gz_sub_command="sim"
else
echo "ERROR [init] Gazebo gz please install gz-garden"
exit 1
fi
# Only start up Gazebo if STANDALONE set to false
if [ "$STANDALONE" != '1' ]; then
# look for running ${gz_command} gazebo world
gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
# shellcheck disable=SC2153
if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
if [ -z "${HEADLESS}" ]; then
# HEADLESS not set, starting gui
${gz_command} ${gz_sub_command} -g &
# "gz sim" only avaiilable in Garden and later
GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
then
# "gz sim" from Garden on
gz_command="gz"
gz_sub_command="sim"
else
echo "ERROR [init] Gazebo gz please install gz-garden"
exit 1
fi
else
echo "INFO [init] gazebo already running world: ${gz_world}"
PX4_GZ_WORLD=${gz_world}
# look for running ${gz_command} gazebo world
gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
# shellcheck disable=SC2153
if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
echo "INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
if [ -z "${HEADLESS}" ]; then
# HEADLESS not set, starting gui
${gz_command} ${gz_sub_command} -g &
fi
else
echo "INFO [init] gazebo already running world: ${gz_world}"
PX4_GZ_WORLD=${gz_world}
fi
fi
# start gz_bridge

16
Tools/simulation/gz/models/rc_cessna/model.sdf
View File

@ -69,7 +69,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/body.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/body.dae</uri>
</mesh>
</geometry>
<material>
@ -184,7 +184,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://rc_cessna/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -229,7 +229,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/left_aileron.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_aileron.dae</uri>
</mesh>
</geometry>
<material>
@ -385,7 +385,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/right_aileron.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_aileron.dae</uri>
</mesh>
</geometry>
<material>
@ -412,7 +412,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/left_flap.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/left_flap.dae</uri>
</mesh>
</geometry>
<material>
@ -439,7 +439,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/right_flap.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/right_flap.dae</uri>
</mesh>
</geometry>
<material>
@ -466,7 +466,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/elevators.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/elevators.dae</uri>
</mesh>
</geometry>
<material>
@ -493,7 +493,7 @@
<geometry>
<mesh>
<scale>0.1 0.1 0.1</scale>
<uri>model://rc_cessna/meshes/rudder.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/RC%20Cessna/tip/files/meshes/rudder.dae</uri>
</mesh>
</geometry>
<material>

16
Tools/simulation/gz/models/standard_vtol/model.sdf
View File

@ -43,7 +43,7 @@
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://standard_vtol/meshes/x8_wing.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_wing.dae</uri>
</mesh>
</geometry>
<material>
@ -183,7 +183,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -246,7 +246,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -309,7 +309,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -372,7 +372,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -436,7 +436,7 @@
<geometry>
<mesh>
<scale>0.8 0.8 0.8</scale>
<uri>model://standard_vtol/meshes/iris_prop_ccw.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/iris_prop_ccw.dae</uri>
</mesh>
</geometry>
<material>
@ -483,7 +483,7 @@
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://standard_vtol/meshes/x8_elevon_left.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_left.dae</uri>
</mesh>
</geometry>
<material>
@ -510,7 +510,7 @@
<geometry>
<mesh>
<scale>0.001 0.001 0.001</scale>
<uri>model://standard_vtol/meshes/x8_elevon_right.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/Standard%20VTOL/tip/files/meshes/x8_elevon_right.dae</uri>
</mesh>
</geometry>
<material>

32
Tools/simulation/gz/models/x500/model.sdf
View File

@ -23,7 +23,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/NXP-HGD-CF.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/NXP-HGD-CF.dae</uri>
</mesh>
</geometry>
</visual>
@ -32,7 +32,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Base.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
@ -41,7 +41,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Base.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
@ -50,7 +50,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Base.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
@ -59,7 +59,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Base.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Base.dae</uri>
</mesh>
</geometry>
</visual>
@ -77,7 +77,7 @@
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
</metal>
</pbr>
</material>
@ -96,7 +96,7 @@
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500/materials/textures/nxp.png</albedo_map>
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/nxp.png</albedo_map>
</metal>
</pbr>
</material>
@ -115,7 +115,7 @@
<specular>1.0 1.0 1.0</specular>
<pbr>
<metal>
<albedo_map>model://x500/materials/textures/rd.png</albedo_map>
<albedo_map>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/materials/textures/rd.png</albedo_map>
</metal>
</pbr>
</material>
@ -250,7 +250,7 @@
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500/meshes/1345_prop_ccw.stl</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
</mesh>
</geometry>
<material>
@ -265,7 +265,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Bell.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
@ -322,7 +322,7 @@
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500/meshes/1345_prop_ccw.stl</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_ccw.stl</uri>
</mesh>
</geometry>
<material>
@ -337,7 +337,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Bell.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
@ -394,7 +394,7 @@
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500/meshes/1345_prop_cw.stl</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
</mesh>
</geometry>
<material>
@ -409,7 +409,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Bell.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>
@ -466,7 +466,7 @@
<geometry>
<mesh>
<scale>0.8461538461538461 0.8461538461538461 0.8461538461538461</scale>
<uri>model://x500/meshes/1345_prop_cw.stl</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/1345_prop_cw.stl</uri>
</mesh>
</geometry>
<material>
@ -481,7 +481,7 @@
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://x500/meshes/5010Bell.dae</uri>
<uri>https://fuel.gazebosim.org/1.0/PX4/models/x500/1/files/meshes/5010Bell.dae</uri>
</mesh>
</geometry>
</visual>

15
src/modules/commander/CMakeLists.txt
View File

@ -1,6 +1,6 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
# Copyright (c) 2015-2023 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,11 +31,12 @@
#
############################################################################
add_subdirectory(Arming)
add_subdirectory(failsafe)
add_subdirectory(failure_detector)
add_subdirectory(HealthAndArmingChecks)
add_subdirectory(failsafe)
add_subdirectory(Arming)
add_subdirectory(ModeUtil)
add_subdirectory(MulticopterThrowLaunch)
px4_add_module(
MODULE modules__commander
@ -59,20 +60,22 @@ px4_add_module(
mag_calibration.cpp
rc_calibration.cpp
Safety.cpp
UserModeIntention.cpp
worker_thread.cpp
MODULE_CONFIG
module.yaml
DEPENDS
ArmAuthorization
circuit_breaker
failsafe
failure_detector
geo
health_and_arming_checks
hysteresis
ArmAuthorization
mode_util
MulticopterThrowLaunch
sensor_calibration
world_magnetic_model
mode_util
failsafe
)
px4_add_unit_gtest(SRC mag_calibration_test.cpp LINKLIBS modules__commander)

11
src/modules/commander/Commander.hpp
View File

@ -34,14 +34,13 @@
#pragma once
/* Helper classes */
#include "failure_detector/FailureDetector.hpp"
#include "failsafe/failsafe.h"
#include "Safety.hpp"
#include "worker_thread.hpp"
#include "failure_detector/FailureDetector.hpp"
#include "HealthAndArmingChecks/HealthAndArmingChecks.hpp"
#include "HomePosition.hpp"
#include "ModeManagement.hpp"
#include "UserModeIntention.hpp"
#include "worker_thread.hpp"
#include <lib/controllib/blocks.hpp>
#include <lib/hysteresis/hysteresis.h>
@ -81,7 +80,6 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
using math::constrain;
@ -176,6 +174,10 @@ private:
void safetyButtonUpdate();
bool isThrowLaunchInProgress() const;
void throwLaunchUpdate();
void vtolStatusUpdate();
void updateTunes();
@ -216,6 +218,7 @@ private:
FailsafeBase &_failsafe{_failsafe_instance};
FailureDetector _failure_detector{this};
HealthAndArmingChecks _health_and_arming_checks{this, _vehicle_status};
MulticopterThrowLaunch _multicopter_throw_launch{this};
Safety _safety{};
WorkerThread _worker_thread{};
ModeManagement _mode_management{

58
src/modules/simulation/gz_bridge/GZBridge.cpp
View File

@ -42,6 +42,8 @@
#include <iostream>
#include <string>
#include <cstdlib>
#include <fstream>
GZBridge::GZBridge(const char *world, const char *name, const char *model,
const char *pose_str) :
@ -72,7 +74,18 @@ int GZBridge::init()
// service call to create model
gz::msgs::EntityFactory req{};
req.set_sdf_filename(_model_sim + "/model.sdf");
std::string filename = "../../../Tools/simulation/gz/models/" + _model_sim + "/model.sdf";
std::ifstream file(filename);
std::string fileContent;
if (file.is_open()) {
// Read the file content into a string
fileContent = std::string((std::istreambuf_iterator<char>(file)), (std::istreambuf_iterator<char>()));
file.close();
}
req.set_sdf(fileContent);
req.set_name(_model_name); // New name for the entity, overrides the name on the SDF.
@ -116,15 +129,44 @@ int GZBridge::init()
bool result;
std::string create_service = "/world/" + _world_name + "/create";
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
bool gz_called = false;
// Check if STANDALONE has been set.
char *standalone_val = std::getenv("STANDALONE");
if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) {
// Check if Gazebo has been called and if not attempt to reconnect.
while (gz_called == false) {
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
return PX4_ERROR;
} else {
gz_called = true;
}
}
// If Gazebo has not been called, wait 2 seconds and try again.
else {
PX4_WARN("Service call timed out as Gazebo has not been detected.");
system_usleep(2000000);
}
}
}
// If STANDALONE has not been set, do not retry to reconnect.
else {
if (_node.Request(create_service, req, 1000, rep, result)) {
if (!rep.data() || !result) {
PX4_ERR("EntityFactory service call failed");
return PX4_ERROR;
}
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
} else {
PX4_ERR("Service call timed out");
return PX4_ERROR;
}
}