[modules] Added gimbal control for PX4

closes #1592

conf/airframes/TUDELFT/tudelft_conf.xml

@@ -117,7 +117,7 @@
    telemetry="telemetry/default_rotorcraft_slow.xml"
    flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml [settings/control/stabilization_att_int_quat.xml] settings/control/stabilization_indi.xml settings/estimation/body_to_imu.xml"
-   settings_modules="modules/geo_mag.xml modules/air_data.xml modules/gps_ubx_ucenter.xml"
+   settings_modules="modules/geo_mag.xml [modules/air_data.xml] modules/gps_ubx_ucenter.xml"
    gui_color="#ffffcccaccca"
   />
   <aircraft

conf/airframes/TUDELFT/tudelft_iris_indi.xml

@@ -40,6 +40,9 @@
       <configure name="INTERMCU_PORT" value="UART6" />
       <configure name="INTERMCU_BAUD" value="B1500000" /> <!-- This is only during first 10s start up, afterwards it is set to 230400-->
     </subsystem>
+    <subsystem name="actuators" type="pwm"> <!-- gimbal and buzzer--->
+      <define name="SERVO_HZ" value="400" />
+    </subsystem>
   </firmware>
   <firmware name="rotorcraft">
     <target name="fbw" board="px4io_2.4" />
@@ -80,6 +83,7 @@
   </firmware>
   <modules main_freq="512">
     <load name="px4_flash.xml" />
+    <load name="px4_gimbal.xml" />
     <load name="geo_mag.xml" />
     <load name="air_data.xml" />
     <load name="send_imu_mag_current.xml" />

conf/modules/px4_gimbal.xml

@@ -0,0 +1,19 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="px4_gimbal">
+  <doc>
+    <description>Control gimbal camera axis through px4 from rc</description>
+    <define name="PX4_GIMBAL_PWM_CHAN" value="0-5" description="PWM channel id gimbal output (default: 0)"/>
+    <define name="PX4_GIMBAL_RC_CHAN" value="0-7" description="Radio channel id gimbal input (default: RADIO_AUX2)"/>
+    <define name="PX4_GIMBAL_PWM_MIN" value="0-1024" description="Minimum pwm value (default: 900)"/>
+    <define name="PX4_GIMBAL_PWM_MAX" value="0-1024" description="Maximum pwm value (default: 1500)"/>
+  </doc>
+  <header>
+    <file name="px4_gimbal.h"/>
+  </header>  
+  <periodic fun="px4_set_gimbal_angle_periodic()" start="px4_gimbal_init()" freq="100" autorun="TRUE"/>
+  <makefile>
+    <file name="px4_gimbal.c"/>
+  </makefile>
+</module>
+

sw/airborne/modules/px4_gimbal/px4_gimbal.c

@@ -0,0 +1,60 @@
+/*
+ * Copyright (C) Kevin van Hecke
+ *
+ * This file is part of paparazzi
+ *
+ */
+/**
+ * @file "modules/px4_gimbal/px4_gimbal.c"
+ * @author Kevin van Hecke
+ * Control gimbal camera axis through px4 from rc
+ */
+
+#include "modules/px4_gimbal/px4_gimbal.h"
+#include "subsystems/radio_control.h"
+
+#include "generated/airframe.h" // AC_ID is required
+#include "subsystems/actuators.h"
+
+#ifndef PX4_GIMBAL_PWM_CHAN
+#define PX4_GIMBAL_PWM_CHAN 0
+#endif
+#ifndef PX4_GIMBAL_RC_CHAN
+#define PX4_GIMBAL_RC_CHAN RADIO_AUX2
+#endif
+
+#ifndef PX4_GIMBAL_PWM_MIN
+#define PX4_GIMBAL_PWM_MIN 900
+#endif
+
+#ifndef PX4_GIMBAL_PWM_MAX
+#define PX4_GIMBAL_PWM_MAX 1500
+#endif
+
+
+void px4_gimbal_init() {
+#ifdef INTER_MCU_AP
+   actuators_init();
+#endif
+}
+
+ void px4_set_gimbal_angle_periodic() {
+   //reroute gimbal rc input from fbw to pwm output on ap
+
+   float value =radio_control.values[PX4_GIMBAL_RC_CHAN];
+
+   float range = PX4_GIMBAL_PWM_MAX - PX4_GIMBAL_PWM_MIN;
+   value = ((MAX_PPRZ-value) / (float)MAX_PPRZ) * range + PX4_GIMBAL_PWM_MIN;
+   if (value < 1) {value=1;} // 0 does not seem to work
+
+   ActuatorPwmSet(PX4_GIMBAL_PWM_CHAN, (int32_t)value);
+
+#ifdef INTER_MCU_AP
+   AllActuatorsCommit();
+#endif
+
+
+ }
+
+
+

sw/airborne/modules/px4_gimbal/px4_gimbal.h

@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) Kevin van Hecke
+ *
+ * This file is part of paparazzi
+ *
+ */
+/**
+ * @file "modules/px4_gimbal/px4_gimbal.h"
+ * @author Kevin van Hecke
+ * Control gimbal camera axis through px4 from rc
+ */
+
+#ifndef PX4_GIMBAL_H
+#define PX4_GIMBAL_H
+
+ extern void px4_gimbal_init(void);
+ extern void px4_set_gimbal_angle_periodic(void);
+
+#endif
+