Compare commits

..

8 Commits

Author SHA1 Message Date
chfriedrich98 5f1edc883d cleanup subscribers and publishers 2024-03-25 16:19:55 +01:00
chfriedrich98 a200a446c7 folder restructure 2024-03-25 16:19:55 +01:00
chfriedrich98 3cd886794e cleanup 2024-03-25 16:19:55 +01:00
chfriedrich98 1ae520b764 cleanup 2024-03-25 16:19:55 +01:00
chfriedrich98 1f332d561e folder restructure 2024-03-25 16:19:55 +01:00
chfriedrich98 6efca3a196 cleanup 2024-03-25 16:19:55 +01:00
PerFrivik dfc58b3dfc Ackermann Rover: Updated airframe 2024-03-25 16:19:55 +01:00
PerFrivik 407bfc2096 Rebased on main 2024-03-25 16:19:55 +01:00
38 changed files with 812 additions and 27 deletions

View File

@ -0,0 +1,44 @@
#!/bin/sh
# @name Rover Gazebo
# @type Rover
# @class Rover
. ${R}etc/init.d/rc.rover_ackermann_defaults
PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
PX4_SIM_MODEL=${PX4_SIM_MODEL:=rover}
param set-default SIM_GZ_EN 1 # Gazebo bridge
# Simulated sensors
param set-default SENS_EN_GPSSIM 1
param set-default SENS_EN_BAROSIM 0
param set-default SENS_EN_MAGSIM 1
param set-default SENS_EN_ARSPDSIM 1
# Actuator mapping
param set-default SIM_GZ_WH_FUNC1 101 # right wheel
param set-default SIM_GZ_WH_MIN1 0
param set-default SIM_GZ_WH_MAX1 200
param set-default SIM_GZ_WH_DIS1 100
param set-default SIM_GZ_WH_FUNC2 102 # left wheel
param set-default SIM_GZ_WH_MIN2 0
param set-default SIM_GZ_WH_MAX2 200
param set-default SIM_GZ_WH_DIS2 100
param set-default SIM_GZ_WH_FUNC3 103 # left wheel
param set-default SIM_GZ_WH_MIN3 0
param set-default SIM_GZ_WH_MAX3 200
param set-default SIM_GZ_WH_DIS3 100
param set-default SIM_GZ_WH_FUNC4 104 # left wheel
param set-default SIM_GZ_WH_MIN4 0
param set-default SIM_GZ_WH_MAX4 200
param set-default SIM_GZ_WH_DIS4 100
param set-default SIM_GZ_SV_FUNC1 201
param set-default SIM_GZ_SV_FUNC2 202
# param set-default SIM_GZ_WH_REV 1 # reverse right wheel

View File

@ -83,6 +83,7 @@ px4_add_romfs_files(
4009_gz_r1_rover
4010_gz_x500_mono_cam
4011_gz_lawnmower
4012_gz_rover
6011_gazebo-classic_typhoon_h480
6011_gazebo-classic_typhoon_h480.post

View File

@ -84,6 +84,13 @@ if(CONFIG_MODULES_DIFFERENTIAL_DRIVE)
)
endif()
if(CONFIG_MODULES_ACKERMANN_DRIVE)
px4_add_romfs_files(
rc.rover_ackermann_apps
rc.rover_ackermann_defaults
)
endif()
if(CONFIG_MODULES_UUV_ATT_CONTROL)
px4_add_romfs_files(
rc.uuv_apps

View File

@ -0,0 +1,11 @@
#!/bin/sh
# Standard apps for a ackermann drive rover.
# Start the attitude and position estimator.
ekf2 start &
# Start rover ackermann drive controller.
ackermann_drive start
# Start Land Detector.
land_detector start rover

View File

@ -0,0 +1,12 @@
#!/bin/sh
# Ackermann rover parameters.
set VEHICLE_TYPE rover_ackermann
param set-default MAV_TYPE 10 # MAV_TYPE_GROUND_ROVER
param set-default CA_AIRFRAME 5 # Rover (Ackermann)
param set-default CA_R_REV 3 # Right and left motors reversible
param set-default EKF2_MAG_TYPE 1 # make sure magnetometer is fused even when not flying

View File

@ -41,6 +41,15 @@ then
. ${R}etc/init.d/rc.rover_differential_apps
fi
#
# Ackermann Rover setup.
#
if [ $VEHICLE_TYPE = rover_ackermann ]
then
# Start ackermann drive rover apps.
. ${R}etc/init.d/rc.rover_ackermann_apps
fi
#
# VTOL setup.
#

View File

@ -13,6 +13,7 @@ CONFIG_MODULES_COMMANDER=y
CONFIG_MODULES_CONTROL_ALLOCATOR=y
CONFIG_MODULES_DATAMAN=y
CONFIG_MODULES_DIFFERENTIAL_DRIVE=y
CONFIG_MODULES_ACKERMANN_DRIVE=y
CONFIG_MODULES_EKF2=y
CONFIG_EKF2_VERBOSE_STATUS=y
CONFIG_EKF2_AUX_GLOBAL_POSITION=y

View File

@ -364,13 +364,6 @@ PX4IO::~PX4IO()
bool PX4IO::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
unsigned num_outputs, unsigned num_control_groups_updated)
{
for (size_t i = 0; i < num_outputs; i++) {
if (!_mixing_output.isFunctionSet(i)) {
// do not run any signal on disabled channels
outputs[i] = 0;
}
}
if (!_test_fmu_fail) {
/* output to the servos */
io_reg_set(PX4IO_PAGE_DIRECT_PWM, 0, outputs, num_outputs);

View File

@ -455,8 +455,7 @@ bool MixingOutput::update()
}
}
// Send output if any function mapped or one last disabling sample
if (!all_disabled || !_was_all_disabled) {
if (!all_disabled) {
if (!_armed.armed && !_armed.manual_lockdown) {
_actuator_test.overrideValues(outputs, _max_num_outputs);
}
@ -464,8 +463,6 @@ bool MixingOutput::update()
limitAndUpdateOutputs(outputs, has_updates);
}
_was_all_disabled = all_disabled;
return true;
}

View File

@ -288,7 +288,6 @@ private:
hrt_abstime _lowrate_schedule_interval{300_ms};
ActuatorTest _actuator_test{_function_assignment};
uint32_t _reversible_mask{0}; ///< per-output bits. If set, the output is configured to be reversible (motors only)
bool _was_all_disabled{false};
uORB::SubscriptionCallbackWorkItem *_subscription_callback{nullptr}; ///< current scheduling callback

View File

@ -191,15 +191,11 @@ TEST_F(MixerModuleTest, basic)
mixing_output.setAllMaxValues(MAX_VALUE);
EXPECT_EQ(test_module.num_updates, 0);
// all functions disabled: expect to get one single update to process disabling the output signal
// all functions disabled: not expected to get an update
mixing_output.update();
mixing_output.updateSubscriptions(false);
mixing_output.update();
EXPECT_EQ(test_module.num_updates, 1);
mixing_output.update();
mixing_output.updateSubscriptions(false);
mixing_output.update();
EXPECT_EQ(test_module.num_updates, 1);
EXPECT_EQ(test_module.num_updates, 0);
test_module.reset();
// configure motor, ensure all still disarmed

View File

@ -0,0 +1,154 @@
/****************************************************************************
*
* 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 "AckermannDrive.hpp"
using namespace time_literals;
using namespace matrix;
AckermannDrive::AckermannDrive() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl)
{
updateParams();
}
bool AckermannDrive::init()
{
ScheduleOnInterval(10_ms); // 100 Hz
return true;
}
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()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
}
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[])
{
AckermannDrive *instance = new AckermannDrive();
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 AckermannDrive::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int AckermannDrive::print_usage(const char *reason)
{
if (reason) {
PX4_ERR("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Rover state machine.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("ackermann_drive", "controller");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int ackermann_drive_main(int argc, char *argv[])
{
return AckermannDrive::main(argc, argv);
}

View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
// PX4 includes
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// uORB includes
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_drive_setpoint.h>
// Standard library includes
#include <math.h>
// Local includes
#include "AckermannDriveControl/AckermannDriveControl.hpp"
//#include "AckermannDriveGuidance/AckermannDriveGuidance.hpp" //TO BE IMPLEMENTED
#include "AckermannDriveKinematics/AckermannDriveKinematics.hpp"
using namespace time_literals;
class AckermannDrive : public ModuleBase<AckermannDrive>, public ModuleParams,
public px4::ScheduledWorkItem
{
public:
AckermannDrive();
~AckermannDrive() override = default;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();
protected:
void updateParams() override;
private:
void Run() override;
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}; // TO BE IMPLEMENTED
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(
(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
)
};

View File

@ -0,0 +1,55 @@
/****************************************************************************
*
* 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 "AckermannDriveControl.hpp"
using namespace time_literals;
AckermannDriveControl::AckermannDriveControl(ModuleParams *parent) : ModuleParams(parent)
{
updateParams();
}
void AckermannDriveControl::updateParams()
{
ModuleParams::updateParams();
}
void AckermannDriveControl::control()
{
_differential_drive_setpoint_sub.update(&_differential_drive_setpoint);
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);
}

View File

@ -0,0 +1,71 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
#pragma once
// PX4 includes
#include <px4_platform_common/px4_config.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
// uORB includes
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/topics/differential_drive_setpoint.h>
// Standard library
#include <math.h>
class AckermannDriveControl : public ModuleParams
{
public:
AckermannDriveControl(ModuleParams *parent);
~AckermannDriveControl() = default;
void control();
protected:
void updateParams() override;
private:
uORB::Subscription _differential_drive_setpoint_sub{ORB_ID(differential_drive_setpoint)};
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{};
};

View File

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

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();
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);
}

View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* 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
//px4 includes
#include <px4_platform_common/module_params.h>
// uORB includes
#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>
//standard includes
#include <matrix/matrix/math.hpp>
/**
* @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{};
DEFINE_PARAMETERS(
(ParamInt<px4::params::CA_R_REV>) _param_r_rev
)
};

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})

View File

@ -0,0 +1,51 @@
############################################################################
#
# 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.
#
############################################################################
add_subdirectory(AckermannDriveControl)
add_subdirectory(AckermannDriveKinematics)
#add_subdirectory(AckermannDriveGuidance) #TO BE IMPLEMENTED
px4_add_module(
MODULE modules__ackermann_drive
MAIN ackermann_drive
SRCS
AckermannDrive.cpp
AckermannDrive.hpp
DEPENDS
AckermannDriveControl
AckermannDriveKinematics
#AckermannDriveGuidance #TO BE IMPLEMENTED
px4_work_queue
#MODULE_CONFIG
# module.yaml
)

View File

@ -0,0 +1,6 @@
menuconfig MODULES_ACKERMANN_DRIVE
bool "ackermann_drive"
default n
depends on MODULES_CONTROL_ALLOCATOR
---help---
Enable support for control of differential drive rovers

View File

@ -0,0 +1,55 @@
module_name: Ackermann Drive Control
parameters:
- group: Rover Ackermann Drive
definitions:
RDD_WHEEL_BASE:
description:
short: Wheel base
long: Distance from the center of the right wheel to the center of the left wheel
type: float
unit: m
min: 0.001
max: 100
increment: 0.001
decimal: 3
default: 0.5
RDD_WHEEL_RADIUS:
description:
short: Wheel radius
long: Size of the wheel, half the diameter of the wheel
type: float
unit: m
min: 0.001
max: 100
increment: 0.001
decimal: 3
default: 0.1
RDD_SPEED_SCALE:
description:
short: Manual speed scale
type: float
min: 0
max: 1.0
increment: 0.01
decimal: 2
default: 1.0
RDD_ANG_SCALE:
description:
short: Manual angular velocity scale
type: float
min: 0
max: 1.0
increment: 0.01
decimal: 2
default: 1.0
RDD_WHL_SPEED:
description:
short: Maximum wheel speed
type: float
unit: rad/s
min: 0
max: 100
increment: 0.01
decimal: 2
default: 10

View File

@ -180,19 +180,15 @@ mixer_tick()
* Run the mixers.
*/
if (source == MIX_FAILSAFE) {
// Set failsafe value if the PWM output isn't disabled
/* copy failsafe values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
if (r_page_servos[i] != 0) {
r_page_servos[i] = r_page_servo_failsafe[i];
}
r_page_servos[i] = r_page_servo_failsafe[i];
}
} else if (source == MIX_DISARMED) {
// Set disarmed value if the PWM output isn't disabled
/* copy disarmed values to the servo outputs */
for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) {
if (r_page_servos[i] != 0) {
r_page_servos[i] = r_page_servo_disarmed[i];
}
r_page_servos[i] = r_page_servo_disarmed[i];
}
}

View File

@ -33,4 +33,4 @@ actuator_output:
min: { min: 0, max: 200, default: 0 }
max: { min: 0, max: 200, default: 200 }
failsafe: { min: 0, max: 200 }
num_channels: 2
num_channels: 4