segway move to examples

cmake/configs/nuttx_px4fmu-v3_default.cmake

@@ -170,6 +170,11 @@ set(config_module_list
 	#
 	examples/rover_steering_control
 
+	#
+	# Segway
+	#
+	examples/segway
+
 	#
 	# Demo apps
 	#

cmake/configs/nuttx_px4fmu-v4pro_default.cmake

@@ -171,6 +171,11 @@ set(config_module_list
 	#
 	examples/rover_steering_control
 
+	#
+	# Segway
+	#
+	#examples/segway
+
 	#
 	# Demo apps
 	#

cmake/configs/nuttx_px4fmu-v5_default.cmake

@@ -169,6 +169,11 @@ set(config_module_list
 	#
 	#examples/rover_steering_control
 
+	#
+	# Segway
+	#
+	#examples/segway
+
 	#
 	# Demo apps
 	#

cmake/configs/posix_sitl_default.cmake

@@ -138,6 +138,11 @@ set(config_module_list
 	#
 	examples/rover_steering_control
 
+	#
+	# Segway
+	#
+	examples/segway
+
 	#
 	# Demo apps
 	#

src/examples/segway/BlockSegwayController.cpp

@@ -0,0 +1,66 @@
+#include "BlockSegwayController.hpp"
+
+using matrix::Eulerf;
+using matrix::Quatf;
+
+void BlockSegwayController::update()
+{
+	// wait for a sensor update, check for exit condition every 100 ms
+	if (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error
+
+	uint64_t newTimeStamp = hrt_absolute_time();
+	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
+	_timeStamp = newTimeStamp;
+
+	// check for sane values of dt
+	// to prevent large control responses
+	if (dt > 1.0f || dt < 0) { return; }
+
+	// set dt for all child blocks
+	setDt(dt);
+
+	// check for new updates
+	if (_param_update.updated()) { updateParams(); }
+
+	// get new information from subscriptions
+	updateSubscriptions();
+
+	actuator_controls_s &actuators = _actuators.get();
+
+	// default all output to zero unless handled by mode
+	for (unsigned i = 2; i < actuator_controls_s::NUM_ACTUATOR_CONTROLS; i++) {
+		actuators.control[i] = 0.0f;
+	}
+
+	const uint8_t &nav_state = _status.get().nav_state;
+
+	// only update guidance in auto mode
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) {
+		// update guidance
+	}
+
+	Eulerf euler = Eulerf(Quatf(_att.get().q));
+
+	// compute speed command
+	float spdCmd = -th2v.update(euler.theta()) - q2v.update(_att.get().pitchspeed);
+
+	// handle autopilot modes
+	if (nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION ||
+	    nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL ||
+	    nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL) {
+
+		actuators.control[0] = spdCmd;
+		actuators.control[1] = spdCmd;
+
+	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_STAB) {
+		actuators.control[0] = spdCmd;
+		actuators.control[1] = spdCmd;
+
+	} else if (nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL) {
+		actuators.control[CH_LEFT] = _manual.get().z;
+		actuators.control[CH_RIGHT] = -_manual.get().x;
+	}
+
+	// update all publications
+	updatePublications();
+}

src/examples/segway/BlockSegwayController.hpp

@@ -1,9 +1,11 @@
 #pragma once
 
 #include <px4_posix.h>
+#include <controllib/blocks.hpp>
 #include <controllib/uorb/blocks.hpp>
 
-using namespace control;
+using control::BlockPI;
+using control::BlockP;
 
 class BlockSegwayController : public control::BlockUorbEnabledAutopilot
 {
@@ -18,12 +20,15 @@ public:
 		_attPoll.fd = _att.getHandle();
 		_attPoll.events = POLLIN;
 	}
+
 	void update();
+
 private:
 	enum {CH_LEFT, CH_RIGHT};
+
 	BlockPI th2v;
 	BlockP q2v;
+
 	px4_pollfd_struct_t _attPoll;
 	uint64_t _timeStamp;
 };
-

src/examples/segway/CMakeLists.txt

@@ -31,8 +31,10 @@
 #
 ############################################################################
 px4_add_module(
-	MODULE modules__segway
+	MODULE examples__segway
 	MAIN segway
+	STACK_MAIN 1300
+	STACK_MAX 1300
 	SRCS
 		segway_main.cpp
 		BlockSegwayController.cpp
@@ -40,4 +42,4 @@ px4_add_module(
 	DEPENDS
 		platforms__common
 	)
-# vim: set noet ft=cmake fenc=utf-8 ff=unix : 
+# vim: set noet ft=cmake fenc=utf-8 ff=unix :

src/examples/segway/params.c

@@ -5,4 +5,3 @@ PARAM_DEFINE_FLOAT(SEG_TH2V_P, 10.0f); // pitch to voltage
 PARAM_DEFINE_FLOAT(SEG_TH2V_I, 0.0f); // pitch integral to voltage
 PARAM_DEFINE_FLOAT(SEG_TH2V_I_MAX, 0.0f); // integral limiter
 PARAM_DEFINE_FLOAT(SEG_Q2V, 1.0f); // pitch rate to voltage
-

src/lib/controllib/uorb/blocks.cpp

@@ -84,20 +84,19 @@ void BlockWaypointGuidance::update(
 BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name) :
 	SuperBlock(parent, name),
 	// subscriptions
+	_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
+	_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
+	_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
 	_att(ORB_ID(vehicle_attitude), 20, 0, &getSubscriptions()),
 	_attCmd(ORB_ID(vehicle_attitude_setpoint), 20, 0, &getSubscriptions()),
-	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
 	_pos(ORB_ID(vehicle_global_position), 20, 0, &getSubscriptions()),
-	_missionCmd(ORB_ID(position_setpoint_triplet), 20, 0, &getSubscriptions()),
-	_manual(ORB_ID(manual_control_setpoint), 20, 0, &getSubscriptions()),
+	_ratesCmd(ORB_ID(vehicle_rates_setpoint), 20, 0, &getSubscriptions()),
 	_status(ORB_ID(vehicle_status), 20, 0, &getSubscriptions()),
-	_param_update(ORB_ID(parameter_update), 1000, 0, &getSubscriptions()), // limit to 1 Hz
+
 	// publications
 	_actuators(ORB_ID(actuator_controls_0), -1, &getPublications())
 {
 }
 
-BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
-
-} // namespace control
+}
 

src/lib/controllib/uorb/blocks.hpp

@@ -39,27 +39,27 @@
 
 #pragma once
 
-#include <uORB/topics/vehicle_attitude_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
-#include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/position_setpoint_triplet.h>
-#include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/vehicle_status.h>
-#include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/parameter_update.h>
-
+#include <math.h>
+#include <poll.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <math.h>
 
+#include <controllib/blocks.hpp>
 #include <drivers/drv_hrt.h>
-#include <poll.h>
 
-#include "../blocks.hpp"
 #include <uORB/Subscription.hpp>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/position_setpoint_triplet.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_status.h>
+
 #include <uORB/Publication.hpp>
+#include <uORB/topics/actuator_controls.h>
 
 namespace control
 {
@@ -90,19 +90,20 @@ class __EXPORT BlockUorbEnabledAutopilot : public SuperBlock
 {
 protected:
 	// subscriptions
+	uORB::Subscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<parameter_update_s> _param_update;
+	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
 	uORB::Subscription<vehicle_attitude_s> _att;
 	uORB::Subscription<vehicle_attitude_setpoint_s> _attCmd;
-	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
 	uORB::Subscription<vehicle_global_position_s> _pos;
-	uORB::Subscription<position_setpoint_triplet_s> _missionCmd;
-	uORB::Subscription<manual_control_setpoint_s> _manual;
+	uORB::Subscription<vehicle_rates_setpoint_s> _ratesCmd;
 	uORB::Subscription<vehicle_status_s> _status;
-	uORB::Subscription<parameter_update_s> _param_update;
+
 	// publications
 	uORB::Publication<actuator_controls_s> _actuators;
 public:
 	BlockUorbEnabledAutopilot(SuperBlock *parent, const char *name);
-	virtual ~BlockUorbEnabledAutopilot();
+	virtual ~BlockUorbEnabledAutopilot() = default;
 };
 
 } // namespace control

src/modules/segway/BlockSegwayController.cpp

@@ -1,60 +0,0 @@
-#include "BlockSegwayController.hpp"
-
-void BlockSegwayController::update()
-{
-	// wait for a sensor update, check for exit condition every 100 ms
-	if (px4_poll(&_attPoll, 1, 100) < 0) { return; } // poll error
-
-	uint64_t newTimeStamp = hrt_absolute_time();
-	float dt = (newTimeStamp - _timeStamp) / 1.0e6f;
-	_timeStamp = newTimeStamp;
-
-	// check for sane values of dt
-	// to prevent large control responses
-	if (dt > 1.0f || dt < 0) { return; }
-
-	// set dt for all child blocks
-	setDt(dt);
-
-	// check for new updates
-	if (_param_update.updated()) { updateParams(); }
-
-	// get new information from subscriptions
-	updateSubscriptions();
-
-	// default all output to zero unless handled by mode
-	for (unsigned i = 2; i < NUM_ACTUATOR_CONTROLS; i++) {
-		_actuators.control[i] = 0.0f;
-	}
-
-	// only update guidance in auto mode
-	if (_status.main_state == MAIN_STATE_AUTO) {
-		// update guidance
-	}
-
-	// compute speed command
-	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
-
-	// handle autopilot modes
-	if (_status.main_state == MAIN_STATE_AUTO ||
-	    _status.main_state == MAIN_STATE_ALTCTL ||
-	    _status.main_state == MAIN_STATE_POSCTL) {
-		_actuators.control[0] = spdCmd;
-		_actuators.control[1] = spdCmd;
-
-	} else if (_status.main_state == MAIN_STATE_MANUAL) {
-		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
-			_actuators.control[CH_LEFT] = _manual.throttle;
-			_actuators.control[CH_RIGHT] = _manual.pitch;
-
-		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
-			_actuators.control[0] = spdCmd;
-			_actuators.control[1] = spdCmd;
-		}
-	}
-
-	// update all publications
-	updatePublications();
-
-}
-

src/modules/uORB/Publication.cpp

@@ -37,20 +37,20 @@
  */
 
 #include "Publication.hpp"
+#include "topics/actuator_controls.h"
+#include "topics/actuator_direct.h"
+#include "topics/actuator_outputs.h"
+#include "topics/debug_key_value.h"
+#include "topics/ekf2_innovations.h"
+#include "topics/filtered_bottom_flow.h"
+#include "topics/rc_channels.h"
+#include "topics/tecs_status.h"
 #include "topics/vehicle_attitude.h"
-#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_global_position.h"
-#include "topics/debug_key_value.h"
-#include "topics/actuator_controls.h"
 #include "topics/vehicle_global_velocity_setpoint.h"
-#include "topics/vehicle_attitude_setpoint.h"
+#include "topics/vehicle_local_position.h"
 #include "topics/vehicle_rates_setpoint.h"
-#include "topics/actuator_outputs.h"
-#include "topics/actuator_direct.h"
-#include "topics/tecs_status.h"
-#include "topics/rc_channels.h"
-#include "topics/filtered_bottom_flow.h"
-#include "topics/ekf2_innovations.h"
 
 #include <px4_defines.h>
 
@@ -108,19 +108,19 @@ PublicationNode::PublicationNode(const struct orb_metadata *meta,
 }
 
 // explicit template instantiation
+template class __EXPORT Publication<actuator_controls_s>;
+template class __EXPORT Publication<actuator_direct_s>;
+template class __EXPORT Publication<actuator_outputs_s>;
+template class __EXPORT Publication<debug_key_value_s>;
+template class __EXPORT Publication<ekf2_innovations_s>;
+template class __EXPORT Publication<filtered_bottom_flow_s>;
+template class __EXPORT Publication<rc_channels_s>;
+template class __EXPORT Publication<tecs_status_s>;
 template class __EXPORT Publication<vehicle_attitude_s>;
-template class __EXPORT Publication<vehicle_local_position_s>;
+template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
 template class __EXPORT Publication<vehicle_global_position_s>;
-template class __EXPORT Publication<debug_key_value_s>;
-template class __EXPORT Publication<actuator_controls_s>;
 template class __EXPORT Publication<vehicle_global_velocity_setpoint_s>;
-template class __EXPORT Publication<vehicle_attitude_setpoint_s>;
+template class __EXPORT Publication<vehicle_local_position_s>;
 template class __EXPORT Publication<vehicle_rates_setpoint_s>;
-template class __EXPORT Publication<actuator_outputs_s>;
-template class __EXPORT Publication<actuator_direct_s>;
-template class __EXPORT Publication<tecs_status_s>;
-template class __EXPORT Publication<rc_channels_s>;
-template class __EXPORT Publication<filtered_bottom_flow_s>;
-template class __EXPORT Publication<ekf2_innovations_s>;
 
 }

src/modules/uORB/Publication.hpp

@@ -117,9 +117,7 @@ public:
 	 * @param list A list interface for adding to
 	 * 	list during construction
 	 */
-	PublicationNode(const struct orb_metadata *meta,
-			int priority = -1,
-			List<PublicationNode *> *list = nullptr);
+	PublicationNode(const struct orb_metadata *meta, int priority = -1, List<PublicationNode *> *list = nullptr);
 
 	/**
 	 * This function is the callback for list traversal
@@ -145,9 +143,7 @@ public:
 	 * @param list A list interface for adding to
 	 * 	list during construction
 	 */
-	Publication(const struct orb_metadata *meta,
-		    int priority = -1,
-		    List<PublicationNode *> *list = nullptr)  :
+	Publication(const struct orb_metadata *meta, int priority = -1, List<PublicationNode *> *list = nullptr)  :
 		PublicationNode(meta, priority, list),
 		_data()
 	{

src/modules/uORB/Subscription.cpp

@@ -37,33 +37,33 @@
  */
 
 #include "Subscription.hpp"
-#include "topics/parameter_update.h"
+#include "topics/actuator_armed.h"
 #include "topics/actuator_controls.h"
-#include "topics/vehicle_gps_position.h"
-#include "topics/satellite_info.h"
-#include "topics/sensor_combined.h"
+#include "topics/att_pos_mocap.h"
+#include "topics/battery_status.h"
+#include "topics/control_state.h"
+#include "topics/distance_sensor.h"
 #include "topics/hil_sensor.h"
-#include "topics/vehicle_attitude.h"
-#include "topics/vehicle_global_position.h"
-#include "topics/position_setpoint_triplet.h"
-#include "topics/vehicle_status.h"
+#include "topics/home_position.h"
+#include "topics/log_message.h"
 #include "topics/manual_control_setpoint.h"
 #include "topics/mavlink_log.h"
-#include "topics/log_message.h"
-#include "topics/vehicle_local_position_setpoint.h"
-#include "topics/vehicle_local_position.h"
-#include "topics/vehicle_attitude_setpoint.h"
-#include "topics/vehicle_rates_setpoint.h"
-#include "topics/rc_channels.h"
-#include "topics/battery_status.h"
 #include "topics/optical_flow.h"
-#include "topics/distance_sensor.h"
-#include "topics/home_position.h"
+#include "topics/parameter_update.h"
+#include "topics/position_setpoint_triplet.h"
+#include "topics/rc_channels.h"
+#include "topics/satellite_info.h"
+#include "topics/sensor_combined.h"
+#include "topics/vehicle_attitude.h"
+#include "topics/vehicle_attitude_setpoint.h"
 #include "topics/vehicle_control_mode.h"
-#include "topics/actuator_armed.h"
-#include "topics/att_pos_mocap.h"
-#include "topics/control_state.h"
+#include "topics/vehicle_global_position.h"
+#include "topics/vehicle_gps_position.h"
 #include "topics/vehicle_land_detected.h"
+#include "topics/vehicle_local_position.h"
+#include "topics/vehicle_local_position_setpoint.h"
+#include "topics/vehicle_rates_setpoint.h"
+#include "topics/vehicle_status.h"
 
 #include <px4_defines.h>
 
@@ -154,32 +154,32 @@ bool Subscription<T>::check_updated()
 template <class T>
 const T &Subscription<T>::get() { return _data; }
 
-template class __EXPORT Subscription<parameter_update_s>;
+template class __EXPORT Subscription<actuator_armed_s>;
 template class __EXPORT Subscription<actuator_controls_s>;
-template class __EXPORT Subscription<vehicle_gps_position_s>;
-template class __EXPORT Subscription<satellite_info_s>;
-template class __EXPORT Subscription<sensor_combined_s>;
+template class __EXPORT Subscription<att_pos_mocap_s>;
+template class __EXPORT Subscription<battery_status_s>;
+template class __EXPORT Subscription<control_state_s>;
+template class __EXPORT Subscription<distance_sensor_s>;
 template class __EXPORT Subscription<hil_sensor_s>;
-template class __EXPORT Subscription<vehicle_attitude_s>;
-template class __EXPORT Subscription<vehicle_global_position_s>;
-template class __EXPORT Subscription<position_setpoint_triplet_s>;
-template class __EXPORT Subscription<vehicle_status_s>;
+template class __EXPORT Subscription<home_position_s>;
+template class __EXPORT Subscription<log_message_s>;
 template class __EXPORT Subscription<manual_control_setpoint_s>;
 template class __EXPORT Subscription<mavlink_log_s>;
-template class __EXPORT Subscription<log_message_s>;
-template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
-template class __EXPORT Subscription<vehicle_local_position_s>;
-template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
-template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<optical_flow_s>;
+template class __EXPORT Subscription<parameter_update_s>;
+template class __EXPORT Subscription<position_setpoint_triplet_s>;
 template class __EXPORT Subscription<rc_channels_s>;
+template class __EXPORT Subscription<satellite_info_s>;
+template class __EXPORT Subscription<sensor_combined_s>;
+template class __EXPORT Subscription<vehicle_attitude_s>;
+template class __EXPORT Subscription<vehicle_attitude_setpoint_s>;
 template class __EXPORT Subscription<vehicle_control_mode_s>;
-template class __EXPORT Subscription<actuator_armed_s>;
-template class __EXPORT Subscription<battery_status_s>;
-template class __EXPORT Subscription<home_position_s>;
-template class __EXPORT Subscription<optical_flow_s>;
-template class __EXPORT Subscription<distance_sensor_s>;
-template class __EXPORT Subscription<att_pos_mocap_s>;
-template class __EXPORT Subscription<control_state_s>;
+template class __EXPORT Subscription<vehicle_global_position_s>;
+template class __EXPORT Subscription<vehicle_gps_position_s>;
 template class __EXPORT Subscription<vehicle_land_detected_s>;
+template class __EXPORT Subscription<vehicle_local_position_s>;
+template class __EXPORT Subscription<vehicle_local_position_setpoint_s>;
+template class __EXPORT Subscription<vehicle_rates_setpoint_s>;
+template class __EXPORT Subscription<vehicle_status_s>;
 
 } // namespace uORB