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

folder restructure

This commit is contained in:
chfriedrich98 2024-03-25 10:22:23 +01:00
parent 3cd886794e
commit a200a446c7
8 changed files with 249 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
src/modules/ackermann_drive/AckermannDrive.cpp
View File

@ -52,6 +52,8 @@ bool AckermannDrive::init()
void AckermannDrive::updateParams()
{
ModuleParams::updateParams();
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
}
void AckermannDrive::Run()
@ -61,7 +63,43 @@ void AckermannDrive::Run()
exit_and_cleanup();
}
_ackermann_drive_control.Update();
hrt_abstime now = hrt_absolute_time();
if (_parameter_update_sub.updated()) {
parameter_update_s pupdate;
if(_parameter_update_sub.copy(&pupdate)){
updateParams();
}
}
if (_vehicle_control_mode_sub.updated()) {
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_armed = vehicle_control_mode.flag_armed;
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
}
}
if (_manual_driving) {
// Manual mode
// directly produce setpoints from the manual control setpoint (joystick)
if (_manual_control_setpoint_sub.updated()) {
differential_drive_setpoint_s _differential_drive_setpoint{};
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
_differential_drive_setpoint.timestamp = now;
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
_max_angular_velocity;
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
}
}
}
_ackermann_drive_control.control();
_ackermann_drive_kinematics.allocate();
}
int AckermannDrive::task_spawn(int argc, char *argv[])

26
src/modules/ackermann_drive/AckermannDrive.hpp
View File

@ -58,6 +58,8 @@
// Local includes
#include "AckermannDriveControl/AckermannDriveControl.hpp"
//#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp"
#include "AckermannDriveKinematics/AckermannDriveKinematics.hpp"
using namespace time_literals;
@ -85,9 +87,29 @@ protected:
private:
void Run() override;
differential_drive_control::AckermannDriveControl _ackermann_drive_control{this}; //TO BE REMOVED
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
AckermannDriveControl _ackermann_drive_control{this};
//AckermannDriveGuidance _ackermann_drive_guidance{this};
AckermannDriveKinematics _ackermann_drive_kinematics{this};
bool _armed = false;
bool _manual_driving = false;
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_AIRFRAME>) _param_ca_airframe //TO BE REMOVED
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
(ParamFloat<px4::params::RDD_ANG_SCALE>) _param_rdd_ang_velocity_scale,
(ParamFloat<px4::params::RDD_WHEEL_SPEED>) _param_rdd_max_wheel_speed,
(ParamFloat<px4::params::RDD_WHEEL_BASE>) _param_rdd_wheel_base,
(ParamFloat<px4::params::RDD_WHEEL_RADIUS>) _param_rdd_wheel_radius,
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

57
src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.cpp
View File

@ -44,62 +44,13 @@ AckermannDriveControl::AckermannDriveControl(ModuleParams *parent) : ModuleParam
void AckermannDriveControl::updateParams()
{
ModuleParams::updateParams();
_max_speed = _param_rdd_max_wheel_speed.get() * _param_rdd_wheel_radius.get();
_max_angular_velocity = _max_speed / (_param_rdd_wheel_base.get() / 2.f);
}
void AckermannDriveControl::Update()
void AckermannDriveControl::control()
{
hrt_abstime now = hrt_absolute_time();
if (_parameter_update_sub.updated()) {
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
updateParams();
}
if (_vehicle_control_mode_sub.updated()) {
vehicle_control_mode_s vehicle_control_mode;
if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
_armed = vehicle_control_mode.flag_armed;
_manual_driving = vehicle_control_mode.flag_control_manual_enabled; // change this when more modes are supported
}
}
if (_manual_driving) {
// Manual mode
// directly produce setpoints from the manual control setpoint (joystick)
if (_manual_control_setpoint_sub.updated()) {
manual_control_setpoint_s manual_control_setpoint{};
if (_manual_control_setpoint_sub.copy(&manual_control_setpoint)) {
_differential_drive_setpoint.timestamp = now;
_differential_drive_setpoint.speed = manual_control_setpoint.throttle * _param_rdd_speed_scale.get() * _max_speed;
_differential_drive_setpoint.yaw_rate = manual_control_setpoint.roll * _param_rdd_ang_velocity_scale.get() *
_max_angular_velocity;
_differential_drive_setpoint_pub.publish(_differential_drive_setpoint);
}
}
}
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
double steering = _differential_drive_setpoint.yaw_rate;
double speed = _differential_drive_setpoint.speed;
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
actuator_motors.control[0] = speed;
actuator_motors.control[1] = speed;
actuator_motors.timestamp = now;
_actuator_motors_pub.publish(actuator_motors);
// publish data to actuator_servos (output module)
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = -steering;
actuator_servos.control[1] = -steering;
actuator_servos.timestamp = now;
_actuator_servos_pub.publish(actuator_servos);
differential_drive_setpoint_s differential_drive_control_output = _differential_drive_setpoint;
differential_drive_control_output.timestamp = hrt_absolute_time();
_differential_drive_control_output_pub.publish(differential_drive_control_output);
}

23
src/modules/ackermann_drive/AckermannDriveControl/AckermannDriveControl.hpp
View File

@ -59,17 +59,13 @@
// Local includes
// #include <AckermannDriveKinematics.hpp>
namespace differential_drive_control
{
class AckermannDriveControl : public ModuleParams
{
public:
AckermannDriveControl(ModuleParams *parent);
~AckermannDriveControl() override = default;
~AckermannDriveControl() = default;
void Update();
void control();
protected:
void updateParams() override;
@ -77,22 +73,11 @@ protected:
private:
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_control_output_pub{ORB_ID(differential_drive_control_output)};
uORB::Publication<differential_drive_setpoint_s> _differential_drive_setpoint_pub{ORB_ID(differential_drive_setpoint)};
differential_drive_setpoint_s _differential_drive_setpoint{};
// DifferentialDriveKinematics _differential_drive_kinematics{};
bool _armed = false;
bool _manual_driving = false;
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::RDD_SPEED_SCALE>) _param_rdd_speed_scale,
@ -103,5 +88,3 @@ private:
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};
} // namespace differential_drive_control

67
src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.cpp Normal file
View File

@ -0,0 +1,67 @@
/****************************************************************************
*
* Copyright (c) 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
* 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 "AckermannDriveKinematics.hpp"
using namespace time_literals;
using namespace matrix;
AckermannDriveKinematics::AckermannDriveKinematics(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
}
void AckermannDriveKinematics::allocate()
{
if (_differential_drive_control_output_sub.updated()) {
_differential_drive_control_output_sub.copy(&_differential_drive_control_output);
}
double steering = _differential_drive_control_output.yaw_rate;
double speed = _differential_drive_control_output.speed;
hrt_abstime now = hrt_absolute_time();
actuator_motors_s actuator_motors{};
actuator_motors.reversible_flags = _param_r_rev.get(); // should be 3 see rc.rover_differential_defaults
actuator_motors.control[0] = speed;
actuator_motors.control[1] = speed;
actuator_motors.timestamp = now;
_actuator_motors_pub.publish(actuator_motors);
// publish data to actuator_servos (output module)
actuator_servos_s actuator_servos{};
actuator_servos.control[0] = -steering;
actuator_servos.control[1] = -steering;
actuator_servos.timestamp = now;
_actuator_servos_pub.publish(actuator_servos);
}

72
src/modules/ackermann_drive/AckermannDriveKinematics/AckermannDriveKinematics.hpp Normal file
View File

@ -0,0 +1,72 @@
/****************************************************************************
*
* Copyright (c) 2023-2024 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.
*
****************************************************************************/
#pragma once
#include <matrix/matrix/math.hpp>
#include <px4_platform_common/module_params.h>
#include <uORB/PublicationMulti.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/actuator_motors.h>
#include <uORB/topics/actuator_servos.h>
#include <uORB/topics/differential_drive_setpoint.h>
/**
* @brief Differential Drive Kinematics class for computing the kinematics of a differential drive robot.
*
* This class provides functions to set the wheel base and radius, and to compute the inverse kinematics
* given linear velocity and yaw rate.
*/
class AckermannDriveKinematics : public ModuleParams
{
public:
AckermannDriveKinematics(ModuleParams *parent);
~AckermannDriveKinematics() = default;
void allocate();
private:
uORB::Subscription _differential_drive_control_output_sub{ORB_ID(differential_drive_control_output)};
uORB::PublicationMulti<actuator_motors_s> _actuator_motors_pub{ORB_ID(actuator_motors)};
uORB::Publication<actuator_servos_s> _actuator_servos_pub{ORB_ID(actuator_servos)};
differential_drive_setpoint_s _differential_drive_control_output{};
float _max_speed{0.f};
float _max_angular_velocity{0.f};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

38
src/modules/ackermann_drive/AckermannDriveKinematics/CMakeLists.txt Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# Copyright (c) 2023-2024 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.
#
############################################################################
px4_add_library(AckermannDriveKinematics
AckermannDriveKinematics.cpp
)
target_include_directories(AckermannDriveKinematics PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

3
src/modules/ackermann_drive/CMakeLists.txt
View File

@ -31,7 +31,7 @@
#
############################################################################
# add_subdirectory(DifferentialDriveKinematics)
add_subdirectory(AckermannDriveKinematics)
add_subdirectory(AckermannDriveControl)
@ -43,6 +43,7 @@ px4_add_module(
AckermannDrive.hpp
DEPENDS
AckermannDriveControl
AckermannDriveKinematics
px4_work_queue
# MODULE_CONFIG
# module.yaml