Hi,
I’m trying to build a custom mode inside the flight controller. This mode is semi-automated. In the end, it needs to change the maximum allowed angle and incorporate some following behavior using an AprilTag.
Right now, I’m working on a simple setup to control only in Stabilized mode using attitude_setpoint, and I’m publishing this directly to the uORB topic.
When I do this, I can see in the MAVLink shell that the attitude_setpoint topic is receiving the data. However, I’m facing some issues:
- I don’t hear any change in the motors, and I don’t see any changes in the motor outputs.
- When I switch back to Stabilize mode, it no longer works properly.
I’m new to flight controller programming, and I’d appreciate any help.
Thanks,
Gix
/****************************************************************************
*
* Copyright (c) 2018-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.
*
****************************************************************************/
/**
* @file FlightTaskManualAltitude.cpp
*/
#include "FlightTaskAprilTagFollow.hpp"
#include <float.h>
#include <mathlib/mathlib.h>
#include <geo/geo.h>
#define map(x, in_min, in_max, out_min, out_max) ((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
using namespace matrix;
bool FlightTaskAprilTagFollow::updateInitialize()
{
bool ret = FlightTask::updateInitialize();
_sticks.checkAndUpdateStickInputs();
if (_sticks_data_required) {
ret = ret && _sticks.isAvailable();
}
return ret;
}
bool FlightTaskAprilTagFollow::activate(const trajectory_setpoint_s &last_setpoint)
{
bool ret = FlightTask::activate(last_setpoint);
_euler_angles[0] = _sticks.getRoll()*30;
_euler_angles[1] = _sticks.getPitch()*30;
_euler_angles[2] = _sticks.getYaw()*30;
matrix::Euler<float> euler(math::radians(_euler_angles[0]), math::radians(_euler_angles[1]), math::radians(_euler_angles[2]));
_q = matrix::Quaternion<float>(euler);
update();
return ret;
}
bool FlightTaskAprilTagFollow::update() {
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint.timestamp = hrt_absolute_time();
attitude_setpoint.q_d[0] = _q(0);
attitude_setpoint.q_d[1] = _q(1);
attitude_setpoint.q_d[2] = _q(2);
attitude_setpoint.q_d[3] = _q(3);
attitude_setpoint.thrust_body[0] = 0.0f;
attitude_setpoint.thrust_body[1] = 0.0f;
attitude_setpoint.thrust_body[2] = map(_sticks.getThrottleZeroCentered(), -1.0f, 1.0f, 0.0f, 1.0f);
PX4_INFO("attitude_setpoint.thrust_body: %f, %f, %f", (double)attitude_setpoint.thrust_body[0], (double)attitude_setpoint.thrust_body[1], (double)attitude_setpoint.thrust_body[2]);
PX4_INFO("attitude_setpoint.q_d: %f, %f, %f, %f", (double)attitude_setpoint.q_d[0], (double)attitude_setpoint.q_d[1], (double)attitude_setpoint.q_d[2], (double)attitude_setpoint.q_d[3]);
return true;
}
/****************************************************************************
*
* Copyright (c) 2018-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.
*
****************************************************************************/
/**
* @file FlightTaskAprilTagFollow.hpp
*
* Flight task for AprilTag following.
*/
#pragma once
#include <lib/stick_yaw/StickYaw.hpp>
#include "FlightTask.hpp"
#include "../Utility/Sticks.hpp"
#include "../Utility/StickTiltXY.hpp"
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <matrix/math.hpp>
class FlightTaskAprilTagFollow : public FlightTask
{
public:
FlightTaskAprilTagFollow() = default;
virtual ~FlightTaskAprilTagFollow() = default;
bool activate(const trajectory_setpoint_s &last_setpoint) override;
bool updateInitialize() override;
bool update() override;
protected:
Sticks _sticks{this};
StickTiltXY _stick_tilt_xy{this};
StickYaw _stick_yaw{this};
bool _sticks_data_required = true; ///< let inherited task-class define if it depends on stick data
uORB::Publication<vehicle_attitude_setpoint_s> _attitude_setpoint_pub{ORB_ID(vehicle_attitude_setpoint)};
uORB::Subscription _attitude_sub{ORB_ID(vehicle_attitude)};
private:
float _euler_angles[3];
matrix::Quaternion<float> _q; ///< quaternion setpoint
};
i remove the publish becuse i understand this is the reson for the problem…