Fixes to fixed wing control example, fixes to the way the control lib publishes estimates

3 files changed, 14 insertions, 14 deletions

diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp
index a36f4429f..0a8ae2ff7 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/block/UOrbPublication.hpp
@@ -60,11 +60,15 @@ public:
 		List<UOrbPublicationBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
-		_handle() {
+		_handle(-1) {
 		if (list != NULL) list->add(this);
 	}
 	void update() {
-		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		if (_handle > 0) {
+			orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		} else {
+			setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+		}
 	}
 	virtual void *getDataVoidPtr() = 0;
 	virtual ~UOrbPublicationBase() {
@@ -99,10 +103,6 @@ public:
 		const struct orb_metadata *meta) :
 		T(), // initialize data structure to zero
 		UOrbPublicationBase(list, meta) {
-		// It is important that we call T()
-		// before we publish the data, so we
-		// call this here instead of the base class
-		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
 	}
 	virtual ~UOrbPublication() {}
 	/*
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
index 7be38015c..77b2ac806 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/controllib/fixedwing.cpp
@@ -130,7 +130,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
@@ -213,7 +213,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	// only update guidance in auto mode
 	if (_status.state_machine == SYSTEM_STATE_AUTO) {
 		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -225,7 +225,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
 
 		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
@@ -239,7 +239,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -328,7 +328,7 @@ void BlockMultiModeBacksideAutopilot::update()
 			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
 			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
 
-			// currenlty using manual throttle
+			// currently using manual throttle
 			// XXX if you enable this watch out, vz might be very noisy
 			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
 			_actuators.control[CH_THR] = _manual.throttle;
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
index 53d0cf893..e4028c40d 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/controllib/fixedwing.hpp
@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -280,7 +280,7 @@ protected:
 	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
 	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
 	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
+	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
 	UOrbSubscription<manual_control_setpoint_s> _manual;
 	UOrbSubscription<vehicle_status_s> _status;
 	UOrbSubscription<parameter_update_s> _param_update;
@@ -328,7 +328,7 @@ private:
 	BlockParam<float> _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_setpoint_s _lastPosCmd;
+	vehicle_global_position_set_triplet_s _lastPosCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public: