mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-16 17:37:34 +08:00
rft: spacecraft into a single module with multiple controllers
This commit is contained in:
Pedro-Roque
@@ -92,7 +92,7 @@ if(CONFIG_MODULES_ROVER_ACKERMANN)
|
||||
)
|
||||
endif()
|
||||
|
||||
if(CONFIG_MODULES_SC_CONTROL_ALLOCATOR)
|
||||
if(CONFIG_MODULES_SPACECRAFT)
|
||||
px4_add_romfs_files(
|
||||
rc.sc_apps
|
||||
rc.sc_defaults
|
||||
|
||||
@@ -14,4 +14,4 @@ CONFIG_MODULES_VTOL_ATT_CONTROL=n
|
||||
CONFIG_MODULES_DIFFERENTIAL_DRIVE=n
|
||||
CONFIG_EKF2_AUX_GLOBAL_POSITION=y
|
||||
CONFIG_MODULES_CONTROL_ALLOCATOR=n
|
||||
CONFIG_MODULES_SC_CONTROL_ALLOCATOR=y
|
||||
CONFIG_MODULES_SPACECRAFT=y
|
||||
|
||||
@@ -1,91 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 ControlAllocator.cpp
|
||||
*
|
||||
* Control allocator.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "ControlAllocator.hpp"
|
||||
|
||||
int ControlAllocator::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ControlAllocator::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int ControlAllocator::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int ControlAllocator::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements control allocation for spacecraft vehicles.
|
||||
It takes torque and thrust setpoints as inputs and outputs
|
||||
actuator setpoint messages.
|
||||
)DESCR_STR"
|
||||
);
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("sc_control_allocator", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Control Allocator app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int sc_control_allocator_main(int argc, char *argv[]);
|
||||
|
||||
int sc_control_allocator_main(int argc, char *argv[])
|
||||
{
|
||||
return ControlAllocator::main(argc, argv);
|
||||
}
|
||||
@@ -1,12 +0,0 @@
|
||||
menuconfig MODULES_SC_CONTROL_ALLOCATOR
|
||||
bool "sc_control_allocator"
|
||||
default n
|
||||
---help---
|
||||
Enable support for control_allocator
|
||||
|
||||
menuconfig USER_SC_CONTROL_ALLOCATOR
|
||||
bool "control_allocator running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SC_CONTROL_ALLOCATOR
|
||||
---help---
|
||||
Put sc_control_allocator in userspace memory
|
||||
+4
-4
@@ -34,15 +34,15 @@
|
||||
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
|
||||
|
||||
px4_add_module(
|
||||
MODULE modules__sc_control_allocator
|
||||
MAIN sc_control_allocator
|
||||
MODULE modules__spacecraft
|
||||
MAIN spacecraft
|
||||
COMPILE_FLAGS
|
||||
${MAX_CUSTOM_OPT_LEVEL}
|
||||
STACK_MAIN
|
||||
3000
|
||||
SRCS
|
||||
ControlAllocator.cpp
|
||||
ControlAllocator.hpp
|
||||
SpacecraftHandler.cpp
|
||||
SpacecraftHandler.hpp
|
||||
MODULE_CONFIG
|
||||
module.yaml
|
||||
DEPENDS
|
||||
@@ -0,0 +1,12 @@
|
||||
menuconfig MODULES_SPACECRAFT
|
||||
bool "spacecraft"
|
||||
default n
|
||||
---help---
|
||||
Enable support for spacecraft
|
||||
|
||||
menuconfig USER_SPACECRAFT
|
||||
bool "spacecraft running as userspace module"
|
||||
default y
|
||||
depends on BOARD_PROTECTED && MODULES_SPACECRAFT
|
||||
---help---
|
||||
Put spacecraft in userspace memory
|
||||
@@ -0,0 +1,340 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013-2019 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 SpacecraftHandler.cpp
|
||||
*
|
||||
* Control allocator.
|
||||
*
|
||||
* @author Julien Lecoeur <julien.lecoeur@gmail.com>
|
||||
*/
|
||||
|
||||
#include "SpacecraftHandler.hpp"
|
||||
|
||||
|
||||
SpacecraftHandler::SpacecraftHandler() :
|
||||
ModuleParams(nullptr),
|
||||
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
|
||||
{
|
||||
updateParams();
|
||||
_rover_differential_setpoint_pub.advertise();
|
||||
}
|
||||
|
||||
bool SpacecraftHandler::init()
|
||||
{
|
||||
ScheduleOnInterval(10_ms); // 100 Hz
|
||||
return true;
|
||||
}
|
||||
|
||||
void SpacecraftHandler::updateParams()
|
||||
{
|
||||
ModuleParams::updateParams();
|
||||
_max_yaw_rate = _param_rd_max_yaw_rate.get() * M_DEG_TO_RAD_F;
|
||||
}
|
||||
|
||||
void SpacecraftHandler::Run()
|
||||
{
|
||||
if (should_exit()) {
|
||||
ScheduleClear();
|
||||
exit_and_cleanup();
|
||||
return;
|
||||
}
|
||||
|
||||
updateSubscriptions();
|
||||
|
||||
// Generate and publish attitude, rate and speed setpoints
|
||||
hrt_abstime timestamp = hrt_absolute_time();
|
||||
|
||||
switch (_nav_state) {
|
||||
case vehicle_status_s::NAVIGATION_STATE_MANUAL: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
rover_differential_setpoint_s rover_differential_setpoint{};
|
||||
rover_differential_setpoint.timestamp = timestamp;
|
||||
rover_differential_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
|
||||
rover_differential_setpoint.yaw_setpoint = NAN;
|
||||
|
||||
if (_max_yaw_rate > FLT_EPSILON && _param_rd_max_thr_yaw_r.get() > FLT_EPSILON) {
|
||||
const float scaled_yaw_rate_input = math::interpolate<float>(manual_control_setpoint.roll,
|
||||
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
const float speed_diff = scaled_yaw_rate_input * _param_rd_wheel_track.get() / 2.f;
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = math::interpolate<float>(speed_diff,
|
||||
-_param_rd_max_thr_yaw_r.get(), _param_rd_max_thr_yaw_r.get(), -1.f, 1.f);
|
||||
|
||||
} else {
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = manual_control_setpoint.roll;
|
||||
|
||||
}
|
||||
|
||||
rover_differential_setpoint.yaw_rate_setpoint = NAN;
|
||||
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
|
||||
}
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_ACRO: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
rover_differential_setpoint_s rover_differential_setpoint{};
|
||||
rover_differential_setpoint.timestamp = timestamp;
|
||||
rover_differential_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
|
||||
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(manual_control_setpoint.roll,
|
||||
-1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
|
||||
rover_differential_setpoint.yaw_setpoint = NAN;
|
||||
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
|
||||
}
|
||||
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_STAB: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
rover_differential_setpoint_s rover_differential_setpoint{};
|
||||
rover_differential_setpoint.timestamp = timestamp;
|
||||
rover_differential_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_differential_setpoint.forward_speed_setpoint_normalized = manual_control_setpoint.throttle;
|
||||
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
|
||||
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
|
||||
rover_differential_setpoint.yaw_setpoint = NAN;
|
||||
|
||||
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
|
||||
|| fabsf(rover_differential_setpoint.forward_speed_setpoint_normalized) < FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_yaw_ctl = false;
|
||||
|
||||
|
||||
} else { // Closed loop yaw control if the yaw rate input is zero (keep current yaw)
|
||||
if (!_yaw_ctl) {
|
||||
_stab_desired_yaw = _vehicle_yaw;
|
||||
_yaw_ctl = true;
|
||||
}
|
||||
|
||||
rover_differential_setpoint.yaw_setpoint = _stab_desired_yaw;
|
||||
rover_differential_setpoint.yaw_rate_setpoint = NAN;
|
||||
|
||||
}
|
||||
|
||||
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
|
||||
}
|
||||
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_POSCTL: {
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
|
||||
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
|
||||
rover_differential_setpoint_s rover_differential_setpoint{};
|
||||
rover_differential_setpoint.timestamp = timestamp;
|
||||
rover_differential_setpoint.forward_speed_setpoint = math::interpolate<float>(manual_control_setpoint.throttle,
|
||||
-1.f, 1.f, -_param_rd_max_speed.get(), _param_rd_max_speed.get());
|
||||
rover_differential_setpoint.forward_speed_setpoint_normalized = NAN;
|
||||
rover_differential_setpoint.yaw_rate_setpoint = math::interpolate<float>(math::deadzone(manual_control_setpoint.roll,
|
||||
STICK_DEADZONE), -1.f, 1.f, -_max_yaw_rate, _max_yaw_rate);
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = NAN;
|
||||
rover_differential_setpoint.yaw_setpoint = NAN;
|
||||
|
||||
if (fabsf(rover_differential_setpoint.yaw_rate_setpoint) > FLT_EPSILON
|
||||
|| fabsf(rover_differential_setpoint.forward_speed_setpoint) < FLT_EPSILON) { // Closed loop yaw rate control
|
||||
_yaw_ctl = false;
|
||||
|
||||
|
||||
} else { // Course control if the yaw rate input is zero (keep driving on a straight line)
|
||||
if (!_yaw_ctl) {
|
||||
_pos_ctl_course_direction = Vector2f(cos(_vehicle_yaw), sin(_vehicle_yaw));
|
||||
_pos_ctl_start_position_ned = _curr_pos_ned;
|
||||
_yaw_ctl = true;
|
||||
}
|
||||
|
||||
// Construct a 'target waypoint' for course control s.t. it is never within the maximum lookahead of the rover
|
||||
const float vector_scaling = sqrtf(powf(_param_pp_lookahd_max.get(),
|
||||
2) + powf(_posctl_pure_pursuit.getCrosstrackError(), 2)) + _posctl_pure_pursuit.getDistanceOnLineSegment();
|
||||
const Vector2f target_waypoint_ned = _pos_ctl_start_position_ned + sign(
|
||||
rover_differential_setpoint.forward_speed_setpoint) *
|
||||
vector_scaling * _pos_ctl_course_direction;
|
||||
// Calculate yaw setpoint
|
||||
const float yaw_setpoint = _posctl_pure_pursuit.calcDesiredHeading(target_waypoint_ned,
|
||||
_pos_ctl_start_position_ned, _curr_pos_ned, fabsf(_vehicle_forward_speed));
|
||||
rover_differential_setpoint.yaw_setpoint = sign(rover_differential_setpoint.forward_speed_setpoint) >= 0 ?
|
||||
yaw_setpoint : matrix::wrap_pi(M_PI_F + yaw_setpoint); // Flip yaw setpoint when driving backwards
|
||||
rover_differential_setpoint.yaw_rate_setpoint = NAN;
|
||||
|
||||
}
|
||||
|
||||
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
|
||||
}
|
||||
|
||||
} break;
|
||||
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
|
||||
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
|
||||
_rover_differential_guidance.computeGuidance(_vehicle_yaw, _vehicle_forward_speed, _nav_state);
|
||||
break;
|
||||
|
||||
default: // Unimplemented nav states will stop the rover
|
||||
_rover_differential_control.resetControllers();
|
||||
_yaw_ctl = false;
|
||||
rover_differential_setpoint_s rover_differential_setpoint{};
|
||||
rover_differential_setpoint.forward_speed_setpoint = NAN;
|
||||
rover_differential_setpoint.forward_speed_setpoint_normalized = 0.f;
|
||||
rover_differential_setpoint.yaw_rate_setpoint = NAN;
|
||||
rover_differential_setpoint.speed_diff_setpoint_normalized = 0.f;
|
||||
rover_differential_setpoint.yaw_setpoint = NAN;
|
||||
_rover_differential_setpoint_pub.publish(rover_differential_setpoint);
|
||||
break;
|
||||
}
|
||||
|
||||
if (!_armed) { // Reset when disarmed
|
||||
_rover_differential_control.resetControllers();
|
||||
_yaw_ctl = false;
|
||||
}
|
||||
|
||||
_rover_differential_control.computeMotorCommands(_vehicle_yaw, _vehicle_yaw_rate, _vehicle_forward_speed);
|
||||
|
||||
}
|
||||
|
||||
void SpacecraftHandler::updateSubscriptions()
|
||||
{
|
||||
|
||||
if (_parameter_update_sub.updated()) {
|
||||
parameter_update_s parameter_update;
|
||||
_parameter_update_sub.copy(¶meter_update);
|
||||
updateParams();
|
||||
}
|
||||
|
||||
if (_vehicle_status_sub.updated()) {
|
||||
vehicle_status_s vehicle_status{};
|
||||
_vehicle_status_sub.copy(&vehicle_status);
|
||||
|
||||
if (vehicle_status.nav_state != _nav_state) { // Reset on mode change
|
||||
_rover_differential_control.resetControllers();
|
||||
_yaw_ctl = false;
|
||||
}
|
||||
|
||||
_nav_state = vehicle_status.nav_state;
|
||||
_armed = vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED;
|
||||
}
|
||||
|
||||
if (_vehicle_angular_velocity_sub.updated()) {
|
||||
vehicle_angular_velocity_s vehicle_angular_velocity{};
|
||||
_vehicle_angular_velocity_sub.copy(&vehicle_angular_velocity);
|
||||
_vehicle_yaw_rate = fabsf(vehicle_angular_velocity.xyz[2]) > YAW_RATE_THRESHOLD ? vehicle_angular_velocity.xyz[2] : 0.f;
|
||||
}
|
||||
|
||||
if (_vehicle_attitude_sub.updated()) {
|
||||
vehicle_attitude_s vehicle_attitude{};
|
||||
_vehicle_attitude_sub.copy(&vehicle_attitude);
|
||||
_vehicle_attitude_quaternion = matrix::Quatf(vehicle_attitude.q);
|
||||
_vehicle_yaw = matrix::Eulerf(_vehicle_attitude_quaternion).psi();
|
||||
}
|
||||
|
||||
if (_vehicle_local_position_sub.updated()) {
|
||||
vehicle_local_position_s vehicle_local_position{};
|
||||
_vehicle_local_position_sub.copy(&vehicle_local_position);
|
||||
_curr_pos_ned = Vector2f(vehicle_local_position.x, vehicle_local_position.y);
|
||||
Vector3f velocity_in_local_frame(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
|
||||
Vector3f velocity_in_body_frame = _vehicle_attitude_quaternion.rotateVectorInverse(velocity_in_local_frame);
|
||||
_vehicle_forward_speed = fabsf(velocity_in_body_frame(0)) > SPEED_THRESHOLD ? velocity_in_body_frame(0) : 0.f;
|
||||
}
|
||||
}
|
||||
|
||||
int SpacecraftHandler::task_spawn(int argc, char *argv[])
|
||||
{
|
||||
SpacecraftHandler *instance = new SpacecraftHandler();
|
||||
|
||||
if (instance) {
|
||||
_object.store(instance);
|
||||
_task_id = task_id_is_work_queue;
|
||||
|
||||
if (instance->init()) {
|
||||
return PX4_OK;
|
||||
}
|
||||
|
||||
} else {
|
||||
PX4_ERR("alloc failed");
|
||||
}
|
||||
|
||||
delete instance;
|
||||
_object.store(nullptr);
|
||||
_task_id = -1;
|
||||
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
int SpacecraftHandler::print_status()
|
||||
{
|
||||
PX4_INFO("Running");
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
int SpacecraftHandler::custom_command(int argc, char *argv[])
|
||||
{
|
||||
return print_usage("unknown command");
|
||||
}
|
||||
|
||||
int SpacecraftHandler::print_usage(const char *reason)
|
||||
{
|
||||
if (reason) {
|
||||
PX4_WARN("%s\n", reason);
|
||||
}
|
||||
|
||||
PRINT_MODULE_DESCRIPTION(
|
||||
R"DESCR_STR(
|
||||
### Description
|
||||
This implements control allocation for spacecraft vehicles.
|
||||
It takes torque and thrust setpoints as inputs and outputs
|
||||
actuator setpoint messages.
|
||||
)DESCR_STR"
|
||||
);
|
||||
|
||||
PRINT_MODULE_USAGE_NAME("sc_control_allocator", "controller");
|
||||
PRINT_MODULE_USAGE_COMMAND("start");
|
||||
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Control Allocator app start / stop handling function
|
||||
*/
|
||||
extern "C" __EXPORT int spacecraft_main(int argc, char *argv[]);
|
||||
|
||||
int spacecraft_main(int argc, char *argv[])
|
||||
{
|
||||
return SpacecraftHandler::main(argc, argv);
|
||||
}
|
||||
+55
-9
@@ -32,7 +32,7 @@
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file ControlAllocator.hpp
|
||||
* @file SpacecraftHandler.hpp
|
||||
*
|
||||
* Control allocator.
|
||||
*
|
||||
@@ -62,13 +62,20 @@
|
||||
#include <uORB/topics/vehicle_status.h>
|
||||
#include <uORB/topics/failure_detector_status.h>
|
||||
|
||||
class ControlAllocator : public ModuleBase<ControlAllocator>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
// Constants
|
||||
static constexpr float STICK_DEADZONE =
|
||||
0.1f; // [0, 1] Percentage of stick input range that will be interpreted as zero around the stick centered value
|
||||
static constexpr float YAW_RATE_THRESHOLD =
|
||||
0.02f; // [rad/s] The minimum threshold for the yaw rate measurement not to be interpreted as zero
|
||||
static constexpr float SPEED_THRESHOLD =
|
||||
0.1f; // [m/s] The minimum threshold for the speed measurement not to be interpreted as zero
|
||||
|
||||
class SpacecraftHandler : public ModuleBase<SpacecraftHandler>, public ModuleParams, public px4::ScheduledWorkItem
|
||||
{
|
||||
|
||||
public:
|
||||
|
||||
ControlAllocator();
|
||||
|
||||
virtual ~ControlAllocator();
|
||||
SpacecraftHandler();
|
||||
~SpacecraftHandler() override = default;
|
||||
|
||||
/** @see ModuleBase */
|
||||
static int task_spawn(int argc, char *argv[]);
|
||||
@@ -79,9 +86,48 @@ public:
|
||||
/** @see ModuleBase */
|
||||
static int print_usage(const char *reason = nullptr);
|
||||
|
||||
/** @see ModuleBase::print_status() */
|
||||
int print_status() override;
|
||||
bool init();
|
||||
|
||||
protected:
|
||||
void updateParams() override;
|
||||
|
||||
private:
|
||||
void Run() override;
|
||||
|
||||
/**
|
||||
* @brief Update uORB subscriptions.
|
||||
*/
|
||||
void updateSubscriptions();
|
||||
|
||||
// uORB Subscriptions
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
|
||||
|
||||
// uORB Publications
|
||||
uORB::Publication<rover_differential_setpoint_s> _rover_differential_setpoint_pub{ORB_ID(rover_differential_setpoint)};
|
||||
|
||||
// Instances
|
||||
SpacecraftHandlerGuidance _rover_differential_guidance{this};
|
||||
SpacecraftHandlerControl _rover_differential_control{this};
|
||||
PurePursuit _posctl_pure_pursuit{this}; // Pure pursuit library
|
||||
|
||||
// Variables
|
||||
Vector2f _curr_pos_ned{};
|
||||
matrix::Quatf _vehicle_attitude_quaternion{};
|
||||
float _vehicle_yaw_rate{0.f};
|
||||
float _vehicle_forward_speed{0.f};
|
||||
float _vehicle_yaw{0.f};
|
||||
float _max_yaw_rate{0.f};
|
||||
int _nav_state{0};
|
||||
bool _armed{false};
|
||||
bool _yaw_ctl{false}; // Indicates if the rover is doing yaw or yaw rate control in Stabilized and Position mode
|
||||
float _stab_desired_yaw{0.f}; // Yaw setpoint for Stabilized mode
|
||||
Vector2f _pos_ctl_course_direction{}; // Course direction for Position mode
|
||||
Vector2f _pos_ctl_start_position_ned{}; // Initial rover position for course control in Position mode
|
||||
|
||||
private: /**< loop duration performance counter */
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user