mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 08:43:34 +08:00
Mission topic: Use mission topic instead of global position for triplet
This commit is contained in:
@@ -117,7 +117,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
|
||||
_vCmd(this, "V_CMD"),
|
||||
_crMax(this, "CR_MAX"),
|
||||
_attPoll(),
|
||||
_lastPosCmd(),
|
||||
_lastMissionCmd(),
|
||||
_timeStamp(0)
|
||||
{
|
||||
_attPoll.fd = _att.getHandle();
|
||||
@@ -141,8 +141,8 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||
setDt(dt);
|
||||
|
||||
// store old position command before update if new command sent
|
||||
if (_posCmd.updated()) {
|
||||
_lastPosCmd = _posCmd.getData();
|
||||
if (_missionCmd.updated()) {
|
||||
_lastMissionCmd = _missionCmd.getData();
|
||||
}
|
||||
|
||||
// check for new updates
|
||||
@@ -159,7 +159,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||
if (_status.main_state == MAIN_STATE_AUTO) {
|
||||
// TODO use vehicle_control_mode here?
|
||||
// update guidance
|
||||
_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
|
||||
_guide.update(_pos, _att, _missionCmd.current, _lastMissionCmd.current);
|
||||
}
|
||||
|
||||
// XXX handle STABILIZED (loiter on spot) as well
|
||||
@@ -182,7 +182,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||
float vCmd = _vLimit.update(_vCmd.get());
|
||||
|
||||
// altitude hold
|
||||
float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
|
||||
float dThrottle = _h2Thr.update(_missionCmd.current.altitude - _pos.alt);
|
||||
|
||||
// heading hold
|
||||
float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
|
||||
|
||||
@@ -264,7 +264,7 @@ private:
|
||||
BlockParamFloat _crMax;
|
||||
|
||||
struct pollfd _attPoll;
|
||||
vehicle_global_position_set_triplet_s _lastPosCmd;
|
||||
mission_item_triplet_s _lastMissionCmd;
|
||||
enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
|
||||
uint64_t _timeStamp;
|
||||
public:
|
||||
|
||||
Reference in New Issue
Block a user