Integrate DF BebopBus into the wrapper

src/platforms/posix/drivers/df_bebop_bus_wrapper/df_bebop_bus_wrapper.cpp

@@ -50,10 +50,11 @@
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/battery_status.h>
 
 #include <systemlib/mixer/mixer.h>
-#include <systemlib/mixer/mixer_multirotor.generated.h>
 #include <systemlib/pwm_limit/pwm_limit.h>
+#include <systemlib/battery.h>
 
 #include <bebop_bus/BebopBus.hpp>
 #include <DevMgr.hpp>
@@ -65,18 +66,34 @@ using namespace DriverFramework;
 class DfBebopBusWrapper : public BebopBus
 {
 public:
-  DfBebopBusWrapper();
-  ~DfBebopBusWrapper() = default;
+	DfBebopBusWrapper();
+	~DfBebopBusWrapper() = default;
 
-  int start();
-  int stop();
-  int print_info();
+	int start();
+	int stop();
+	int print_info();
+	int start_motors();
+	int stop_motors();
+	int clear_errors();
+	int set_esc_speeds(const float pwm[4]);
+
+	void set_last_throttle(float throttle) {_last_throttle = throttle;};
 
 private:
+	orb_advert_t _battery_topic;
+
+	Battery _battery;
+	bool _armed;
+	float _last_throttle;
+
+	int _battery_orb_class_instance;
+
+	int _publish(struct bebop_state_data &data);
 };
 
 DfBebopBusWrapper::DfBebopBusWrapper() :
-  BebopBus(BEBOP_BUS_DEVICE_PATH)
+	BebopBus(BEBOP_BUS_DEVICE_PATH), _battery_topic(nullptr), _battery(), _armed(false), _last_throttle(0.0f),
+	_battery_orb_class_instance(-1)
 {}
 
 int DfBebopBusWrapper::start()
@@ -96,10 +113,14 @@ int DfBebopBusWrapper::start()
 		return ret;
 	}
 
+	// Signal bus start on Bebop
+	BebopBus::_play_sound(BebopBus::BOOT);
+
 	return 0;
 }
 
-int DfBebopBusWrapper::stop() {
+int DfBebopBusWrapper::stop()
+{
 
 	int ret = BebopBus::stop();
 
@@ -113,13 +134,12 @@ int DfBebopBusWrapper::stop() {
 
 int DfBebopBusWrapper::print_info()
 {
-  bebop_bus_info info;
+	bebop_bus_info info;
 	int ret = _get_info(&info);
 
-	if (ret < 0)
-  {
-    return -1;
-  }
+	if (ret < 0) {
+		return -1;
+	}
 
 	PX4_INFO("Bebop Controller Info");
 	PX4_INFO("  Software Version: %d.%d", info.version_major, info.version_minor);
@@ -130,19 +150,67 @@ int DfBebopBusWrapper::print_info()
 	PX4_INFO("  Total flight time: %d", info.total_flight_time);
 	PX4_INFO("  Last Error: %d\n", info.last_error);
 
-  return 0;
+	return 0;
+}
+
+int DfBebopBusWrapper::start_motors()
+{
+	_armed = true;
+	return BebopBus::_start_motors();
+}
+
+int DfBebopBusWrapper::stop_motors()
+{
+	_armed = false;
+	return BebopBus::_stop_motors();
+}
+
+int DfBebopBusWrapper::clear_errors()
+{
+	return BebopBus::_clear_errors();
+}
+
+int DfBebopBusWrapper::set_esc_speeds(const float pwm[4])
+{
+	return BebopBus::_set_esc_speed(pwm);
+}
+
+int DfBebopBusWrapper::_publish(struct bebop_state_data &data)
+{
+
+	battery_status_s battery_report;
+	const hrt_abstime timestamp = hrt_absolute_time();
+
+	// TODO Check if this is the right way for the Bebop
+	_battery.updateBatteryStatus(timestamp, data.battery_voltage_v, 0.0, _last_throttle, _armed, &battery_report);
+
+	// TODO: when is this ever blocked?
+	if (!(m_pub_blocked)) {
+
+		if (_battery_topic == nullptr) {
+			_battery_topic = orb_advertise_multi(ORB_ID(battery_status), &battery_report,
+							     &_battery_orb_class_instance, ORB_PRIO_LOW);
+
+		} else {
+			orb_publish(ORB_ID(battery_status), _battery_topic, &battery_report);
+		}
+
+	}
+
+	return 0;
 }
 
 namespace df_bebop_bus_wrapper
 {
-  
-DfBebopBusWrapper *g_dev = nullptr;
-volatile bool _task_should_exit = false; // flag indicating if snapdragon_rc_pwm task should exit
-static bool _is_running = false;         // flag indicating if snapdragon_rc_pwm app is running
+
+DfBebopBusWrapper *g_dev = nullptr;      // interface to the Bebop's I2C device
+volatile bool _task_should_exit = false; // flag indicating if bebop esc control task should exit
+static bool _is_running = false;         // flag indicating if bebop esc  app is running
+static bool _motors_running = false;     // flag indicating if the motors are running
 static px4_task_t _task_handle = -1;     // handle to the task main thread
 
 static const int FREQUENCY_PWM = 400;
-static const char *MIXER_FILENAME = "";
+static const char *MIXER_FILENAME = "/home/root/quad_x.main.mix";
 
 // subscriptions
 int     _controls_sub;
@@ -153,34 +221,26 @@ orb_advert_t    _outputs_pub = nullptr;
 orb_advert_t    _rc_pub = nullptr;
 
 // topic structures
-actuator_controls_s _controls;
+actuator_controls_s _controls[1];
 actuator_outputs_s  _outputs;
 actuator_armed_s    _armed;
 
-pwm_limit_t     _pwm_limit;
-
-// esc parameters
-int32_t _pwm_disarmed;
-int32_t _pwm_min;
-int32_t _pwm_max;
-
-MultirotorMixer *_mixer = nullptr;
+MixerGroup *_mixers = nullptr;
 
 int start();
 int stop();
 int info();
 void usage();
-void send_outputs_pwm(const uint16_t *pwm);
 void task_main(int argc, char *argv[]);
 
-/* mixer initialization */
-int initialize_mixer(const char *mixer_filename);
-int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
+/* mixers initialization */
+int initialize_mixers(const char *mixers_filename);
+int mixers_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
 
-int mixer_control_callback(uintptr_t handle,
-			   uint8_t control_group,
-			   uint8_t control_index,
-			   float &input)
+int mixers_control_callback(uintptr_t handle,
+			    uint8_t control_group,
+			    uint8_t control_index,
+			    float &input)
 {
 	const actuator_controls_s *controls = (actuator_controls_s *)handle;
 
@@ -189,70 +249,44 @@ int mixer_control_callback(uintptr_t handle,
 	return 0;
 }
 
-int initialize_mixer(const char *mixer_filename)
+int initialize_mixers(const char *mixers_filename)
 {
-	char buf[2048];
+	char buf[2048] = {0};
 	size_t buflen = sizeof(buf);
 
-	PX4_INFO("Trying to initialize mixer from config file %s", mixer_filename);
-
-	int fd_load = ::open(mixer_filename, O_RDONLY);
-
-	if (fd_load != -1) {
-		int nRead = ::read(fd_load, buf, buflen);
-		close(fd_load);
-
-		if (nRead > 0) {
-			_mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
+	PX4_INFO("Trying to initialize mixers from config file %s", mixers_filename);
 
-			if (_mixer != nullptr) {
-				PX4_INFO("Successfully initialized mixer from config file");
-				return 0;
+	if (load_mixer_file(mixers_filename, &buf[0], sizeof(buf)) < 0) {
+		warnx("can't load mixer: %s", mixers_filename);
+		return -1;
+	}
 
-			} else {
-				PX4_ERR("Unable to parse from mixer config file");
-				return -1;
-			}
+	if (_mixers == nullptr) {
+		_mixers = new MixerGroup(mixers_control_callback, (uintptr_t)_controls);
+	}
 
-		} else {
-			PX4_WARN("Unable to read from mixer config file");
-			return -2;
-		}
+	if (_mixers == nullptr) {
+		PX4_ERR("No mixers available");
+		return -1;
 
 	} else {
-		PX4_WARN("No mixer config file found, using default mixer.");
+		int ret = _mixers->load_from_buf(buf, buflen);
 
-		/* Mixer file loading failed, fall back to default mixer configuration for
-		* QUAD_X airframe. */
-		float roll_scale = 1;
-		float pitch_scale = 1;
-		float yaw_scale = 1;
-		float deadband = 0;
-
-		_mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
-					     MultirotorGeometry::QUAD_X,
-					     roll_scale, pitch_scale, yaw_scale, deadband);
-
-		// TODO: temporary hack to make this compile
-		(void)_config_index[0];
-
-		if (_mixer == nullptr) {
-			PX4_ERR("Mixer initialization failed");
-			return -1;
+		if (ret != 0) {
+			PX4_ERR("Unable to parse mixers file");
+			delete _mixers;
+			_mixers = nullptr;
+			ret = -1;
 		}
 
 		return 0;
 	}
 }
 
-void send_outputs_pwm(const uint16_t *pwm)
-{
-  PX4_INFO("%d %d %d %d", pwm[0], pwm[1], pwm[2], pwm[3]);
-}
-
 void task_main(int argc, char *argv[])
 {
 	_is_running = true;
+	_motors_running = false;
 
 	// Subscribe for orb topics
 	_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0));
@@ -269,13 +303,14 @@ void task_main(int argc, char *argv[])
 	/* Don't limit poll intervall for now, 250 Hz should be fine. */
 	//orb_set_interval(_controls_sub, 10);
 
-	// Set up mixer
-	if (initialize_mixer(MIXER_FILENAME) < 0) {
+	// Set up mixers
+	if (initialize_mixers(MIXER_FILENAME) < 0) {
 		PX4_ERR("Mixer initialization failed.");
 		return;
 	}
 
-	pwm_limit_init(&_pwm_limit);
+	// TODO check if we have to adjust the frequency
+	//orb_set_interval(_controls_sub, 1000);
 
 	// Main loop
 	while (!_task_should_exit) {
@@ -296,49 +331,54 @@ void task_main(int argc, char *argv[])
 		}
 
 		if (fds[0].revents & POLLIN) {
-			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
+			orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls[0]);
 
-			_outputs.timestamp = _controls.timestamp;
+			_outputs.timestamp = _controls[0].timestamp;
 
-			/* do mixing */
-			_outputs.noutputs = _mixer->mix(_outputs.output,
-							0 /* not used */,
-							NULL);
 
+			if (_mixers != nullptr) {
+				/* do mixing */
+				_outputs.noutputs = _mixers->mix(_outputs.output,
+								 4,
+								 NULL);
+			}
+
+			// Set last throttle for battery calculations
+			g_dev->set_last_throttle(_controls[0].control[3]);
 
 			/* disable unused ports by setting their output to NaN */
-			for (size_t i = _outputs.noutputs;
-			     i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
+			for (size_t i = _outputs.noutputs; i < sizeof(_outputs.output) / sizeof(_outputs.output[0]);
 			     i++) {
 				_outputs.output[i] = NAN;
 			}
 
-			const uint16_t reverse_mask = 0;
-			uint16_t disarmed_pwm[4];
-			uint16_t min_pwm[4];
-			uint16_t max_pwm[4];
-
-			for (unsigned int i = 0; i < 4; i++) {
-				disarmed_pwm[i] = _pwm_disarmed;
-				min_pwm[i] = _pwm_min;
-				max_pwm[i] = _pwm_max;
+			float motor_out[4];
+
+			for (size_t i = 0; i < 4; ++i) {
+				if (i < _outputs.noutputs &&
+				    PX4_ISFINITE(_outputs.output[i]) &&
+				    _outputs.output[i] >= -1.0f &&
+				    _outputs.output[i] <= 1.0f) {
+					/* scale for Bebop output 0.0 - 1.0 */
+					_outputs.output[i] = (_outputs.output[i] + 1.0f) / 2.0f;
+
+				} else {
+					/*
+					 * Value is NaN, INF or out of band - set to the minimum value.
+					 * This will be clearly visible on the servo status and will limit the risk of accidentally
+					 * spinning motors. It would be deadly in flight.
+					 */
+					_outputs.output[i] = 0.0;
+				}
 			}
 
-			uint16_t pwm[4];
-
-			// TODO FIXME: pre-armed seems broken
-			pwm_limit_calc(_armed.armed,
-				       false/*_armed.prearmed*/,
-				       _outputs.noutputs,
-				       reverse_mask,
-				       disarmed_pwm,
-				       min_pwm,
-				       max_pwm,
-				       _outputs.output,
-				       pwm,
-				       &_pwm_limit);
+			// Adjust order of BLDCs from PX4 to Bebop
+			motor_out[0] = _outputs.output[2];
+			motor_out[1] = _outputs.output[0];
+			motor_out[2] = _outputs.output[3];
+			motor_out[3] = _outputs.output[1];
 
-			send_outputs_pwm(pwm);
+			g_dev->set_esc_speeds(motor_out);
 
 			if (_outputs_pub != nullptr) {
 				orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
@@ -354,6 +394,17 @@ void task_main(int argc, char *argv[])
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 		}
+
+		if (_armed.armed && !_motors_running) {
+			g_dev->start_motors();
+			_motors_running = true;
+		}
+
+		if (!_armed.armed && _motors_running) {
+			g_dev->stop_motors();
+			g_dev->clear_errors();
+			_motors_running = false;
+		}
 	}
 
 	orb_unsubscribe(_controls_sub);
@@ -379,7 +430,7 @@ int start()
 		return ret;
 	}
 
-	// Open the MAG sensor
+	// Open the Bebop dirver
 	DevHandle h;
 	DevMgr::getHandle(BEBOP_BUS_DEVICE_PATH, h);
 
@@ -391,7 +442,7 @@ int start()
 
 	DevMgr::releaseHandle(h);
 
-  // Start the task to forward the motor control commands
+	// Start the task to forward the motor control commands
 	ASSERT(_task_handle == -1);
 
 	/* start the task */
@@ -413,7 +464,7 @@ int start()
 
 int stop()
 {
-  // Stop bebop motor control task
+	// Stop bebop motor control task
 	_task_should_exit = true;
 
 	while (_is_running) {
@@ -428,7 +479,7 @@ int stop()
 		return 1;
 	}
 
-  // Stop DF device
+	// Stop DF device
 	int ret = g_dev->stop();
 
 	if (ret != 0) {
@@ -455,9 +506,10 @@ info()
 	PX4_DEBUG("state @ %p", g_dev);
 
 	int ret = g_dev->print_info();
+
 	if (ret != 0) {
-	  PX4_ERR("Unable to print info");
-	  return ret;
+		PX4_ERR("Unable to print info");
+		return ret;
 	}
 
 	return 0;
@@ -469,7 +521,7 @@ usage()
 	PX4_INFO("Usage: df_bebop_bus_wrapper 'start', 'info', 'stop'");
 }
 
-} /* df_bebop_bus_wrapper */ 
+} /* df_bebop_bus_wrapper */
 
 
 int