fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

replace heading_reset_counter

Browse Source
This commit is contained in:
Dennis Mannhart 2018-04-30 10:59:07 +02:00 committed by Lorenz Meier
parent 9ec47f2b74
commit afbeafebd8
2 changed files with 27 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.cpp
View File

@ -41,6 +41,19 @@
using namespace matrix;
bool FlightTaskManualStabilized::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!FlightTaskManual::initializeSubscriptions(subscription_array)) {
return false;
}
if (!subscription_array.get(ORB_ID(vehicle_attitude), _sub_attitude)) {
return false;
}
return true;
}
bool FlightTaskManualStabilized::activate()
{
bool ret = FlightTaskManual::activate();
@ -70,6 +83,13 @@ void FlightTaskManualStabilized::_updateHeadingSetpoints()
// hold the current heading when no more rotation commanded
if (!PX4_ISFINITE(_yaw_setpoint)) {
_yaw_setpoint = _yaw;
} else {
// check reset counter and update yaw setpoint if necessary
if (_sub_attitude->get().quat_reset_counter != _heading_reset_counter) {
_yaw_setpoint += matrix::Eulerf(matrix::Quatf(_sub_attitude->get().delta_q_reset)).psi();
_heading_reset_counter = _sub_attitude->get().quat_reset_counter;
}
}
}
}

9
src/lib/FlightTasks/tasks/FlightTaskManualStabilized.hpp
View File

@ -41,6 +41,7 @@
#pragma once
#include "FlightTaskManual.hpp"
#include <uORB/topics/vehicle_attitude.h>
class FlightTaskManualStabilized : public FlightTaskManual
{
@ -48,10 +49,10 @@ public:
FlightTaskManualStabilized() = default;
virtual ~FlightTaskManualStabilized() = default;
bool activate() override;
bool update() override;
bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
protected:
virtual void _updateSetpoints(); /**< updates all setpoints*/
@ -59,6 +60,10 @@ protected:
void _rotateIntoHeadingFrame(matrix::Vector2f &vec); /**< rotates vector into local frame */
private:
uint8_t _heading_reset_counter = 0; /**< estimator heading reset */
uORB::Subscription<vehicle_attitude_s> *_sub_attitude{nullptr};
void _updateHeadingSetpoints(); /**< sets yaw or yaw speed */
void _updateThrustSetpoints(); /**< sets thrust setpoint */