HAL_PX4: publish actuator values for UAVCAN ESCs

this publishes scaled actuator values so that the uavcan module can
drive ESCs

libraries/AP_HAL_PX4/RCOutput.cpp

@@ -11,6 +11,8 @@
 #include <unistd.h>
 
 #include <drivers/drv_pwm_output.h>
+#include <uORB/topics/actuator_direct.h>
+#include <drivers/drv_hrt.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -55,6 +57,12 @@ void PX4RCOutput::init(void* unused)
     for (int i=0; i < PX4_NUM_OUTPUT_CHANNELS; i++) {
         _period[i] = PWM_IGNORE_THIS_CHANNEL;
     }
+
+    // publish actuator vaules on demand
+    _actuator_direct_pub = -1;
+
+    // and armed state
+    _actuator_armed_pub = -1;
 }
 
 
@@ -256,12 +264,70 @@ void PX4RCOutput::read(uint16_t* period_us, uint8_t len)
     }
 }
 
+/*
+  update actuator armed state
+ */
+void PX4RCOutput::_arm_actuators(bool arm)
+{
+    if (_armed.armed == arm) {
+        // already armed;
+        return;
+    }
+
+	_armed.timestamp = hrt_absolute_time();
+    _armed.armed = arm;
+    _armed.ready_to_arm = arm;
+    _armed.lockdown = false;
+    _armed.force_failsafe = false;
+
+    if (_actuator_armed_pub == -1) {
+        _actuator_armed_pub = orb_advertise(ORB_ID(actuator_armed), &_armed);
+    } else {
+        orb_publish(ORB_ID(actuator_armed), _actuator_armed_pub, &_armed);
+    }
+}
+
+/*
+  publish new outputs to the actuator_direct topic
+ */
+void PX4RCOutput::_publish_actuators(void)
+{
+	struct actuator_direct_s actuators;
+
+	actuators.nvalues = _max_channel;
+    if (actuators.nvalues > NUM_ACTUATORS_DIRECT) {
+        actuators.nvalues = NUM_ACTUATORS_DIRECT;
+    }
+    // don't publish more than 8 actuators for now, as the uavcan ESC
+    // driver refuses to update any motors if you try to publish more
+    // than 8
+    if (actuators.nvalues > 8) {
+        actuators.nvalues = 8;
+    }
+	actuators.timestamp = hrt_absolute_time();
+    for (uint8_t i=0; i<actuators.nvalues; i++) {
+        actuators.values[i] = (_period[i] - _esc_pwm_min) / (float)(_esc_pwm_max - _esc_pwm_min);
+        // actuator values are from -1 to 1
+        actuators.values[i] = actuators.values[i]*2 - 1;
+    }
+
+    if (_actuator_direct_pub == -1) {
+        _actuator_direct_pub = orb_advertise(ORB_ID(actuator_direct), &actuators);
+    } else {
+        orb_publish(ORB_ID(actuator_direct), _actuator_direct_pub, &actuators);
+    }
+    if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) {
+        _arm_actuators(true);
+    }
+}
+
 void PX4RCOutput::_timer_tick(void)
 {
     uint32_t now = hal.scheduler->micros();
 
     if ((_enabled_channels & ((1U<<_servo_count)-1)) == 0) {
         // no channels enabled
+        _arm_actuators(false);
         goto update_pwm;
     }
 
@@ -287,6 +353,10 @@ void PX4RCOutput::_timer_tick(void)
                 ::write(_alt_fd, &_period[_servo_count], n*sizeof(_period[0]));
             }
         }
+
+        // also publish to actuator_direct
+        _publish_actuators();
+
         perf_end(_perf_rcout);
         _last_output = now;
     }

libraries/AP_HAL_PX4/RCOutput.h

@@ -5,6 +5,7 @@
 #include <AP_HAL_PX4.h>
 #include <systemlib/perf_counter.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
 
 #define PX4_NUM_OUTPUT_CHANNELS 16
 
@@ -24,6 +25,10 @@ public:
     void     set_failsafe_pwm(uint32_t chmask, uint16_t period_us);
     bool     force_safety_on(void);
     void     force_safety_off(void);
+    void     set_esc_scaling(uint16_t min_pwm, uint16_t max_pwm) {
+        _esc_pwm_min = min_pwm;
+        _esc_pwm_max = max_pwm;
+    }
 
     void _timer_tick(void);
 
@@ -42,8 +47,16 @@ private:
     uint16_t _enabled_channels;
     int _pwm_sub;
     actuator_outputs_s _outputs;
+    actuator_armed_s _armed;
+
+    int _actuator_direct_pub = -1;
+    int _actuator_armed_pub = -1;
+    uint16_t _esc_pwm_min = 1000;
+    uint16_t _esc_pwm_max = 2000;
 
     void _init_alt_channels(void);
+    void _publish_actuators(void);
+    void _arm_actuators(bool arm);
 };
 
 #endif // __AP_HAL_PX4_RCOUTPUT_H__