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

PositionControl: add getter for output attitude setpoint

This commit is contained in:
Matthias Grob 2019-10-22 19:11:47 +02:00
parent 198605d258
commit f70d4d21a1
3 changed files with 19 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/modules/mc_pos_control/PositionControl/PositionControl.cpp
View File

@ -365,6 +365,12 @@ void PositionControl::getLocalPositionSetpoint(vehicle_local_position_setpoint_s
_thr_sp.copyTo(local_position_setpoint.thrust);
}
void PositionControl::getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint)
{
attitude_setpoint = ControlMath::thrustToAttitude(_thr_sp, _yaw_sp);
attitude_setpoint.yaw_sp_move_rate = _yawspeed_sp;
}
void PositionControl::updateParams()
{
ModuleParams::updateParams();

18
src/modules/mc_pos_control/PositionControl/PositionControl.hpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2018 PX4 Development Team. All rights reserved.
* Copyright (c) 2018 - 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
@ -39,6 +39,7 @@
#include <matrix/matrix/math.hpp>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_local_position_setpoint.h>
#include <uORB/topics/vehicle_constraints.h>
@ -128,13 +129,6 @@ public:
*/
void resetIntegralZ() { _thr_int(2) = 0.0f; }
/**
* Get the
* @see _yawspeed_sp
* @return The yawspeed set-point member.
*/
const float &getYawspeedSetpoint() { return _yawspeed_sp; }
/**
* Get the
* @see _vel_sp
@ -164,6 +158,14 @@ public:
*/
void getLocalPositionSetpoint(vehicle_local_position_setpoint_s &local_position_setpoint);
/**
* Get the controllers output attitude setpoint
* This attitude setpoint was generated from the resulting acceleration setpoint after position and velocity control.
* It needs to be executed by the attitude controller to achieve velocity and position tracking.
* @param attitude_setpoint reference to struct to fill up
*/
void getAttitudeSetpoint(vehicle_attitude_setpoint_s &attitude_setpoint);
protected:
void updateParams() override;

9
src/modules/mc_pos_control/mc_pos_control_main.cpp
View File

@ -68,7 +68,6 @@
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <PositionControl.hpp>
#include <ControlMath.hpp>
#include "Takeoff.hpp"
#include <float.h>
@ -689,12 +688,10 @@ MulticopterPositionControl::Run()
limit_thrust_during_landing(local_pos_sp);
}
// Fill attitude setpoint. Attitude is computed from yaw and thrust setpoint.
vehicle_attitude_setpoint_s attitude_setpoint{};
attitude_setpoint = ControlMath::thrustToAttitude(matrix::Vector3f(local_pos_sp.thrust), local_pos_sp.yaw);
attitude_setpoint.yaw_sp_move_rate = _control.getYawspeedSetpoint();
attitude_setpoint.fw_control_yaw = false;
attitude_setpoint.apply_flaps = false;
attitude_setpoint.timestamp = hrt_absolute_time();
_control.getAttitudeSetpoint(attitude_setpoint);
// publish attitude setpoint
// Note: this requires review. The reason for not sending