[joystick] convert old joystick input to module

The message field and types are extended to add yaw and to use int16 so
that pprz_t command can be used directly.
When a new JOYSTICK_RAW message is received, an ABI JOYSTICK message is
sent.
Flight plan for direct control of planes and rotorcraft are given as
examples.

conf/abi.xml

@@ -191,6 +191,13 @@
       <field name="extra"        type="int16_t">Extra field for options like detector source etc</field>
     </message>
 
+    <message name="JOYSTICK" id="28">
+      <field name="roll"      type="int16_t"/>
+      <field name="pitch"     type="int16_t"/>
+      <field name="yaw"       type="int16_t"/>
+      <field name="throttle"  type="int16_t"/>
+    </message>
+
   </msg_class>
 
 </protocol>

conf/flight_plans/joystick.xml

@@ -3,7 +3,18 @@
 <flight_plan alt="75" ground_alt="0" lat0="43.46223" lon0="1.27289" max_dist_from_home="1500" name="Basic" security_height="25">
   <header>
 #include "subsystems/datalink/datalink.h"
-#include "firmwares/fixedwing/joystick.h"
+
+#ifdef NAV_C
+#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
+#include "autopilot.h"
+void joystick_handler(void) {
+  if (And(autopilot_get_mode() == AP_MODE_AUTO2, nav_block == joystick_block)) {
+    h_ctl_roll_setpoint = AUTO1_MAX_ROLL * joystick.roll / MAX_PPRZ;
+    h_ctl_pitch_setpoint = AUTO1_MAX_PITCH * joystick.pitch / MAX_PPRZ;
+    v_ctl_throttle_setpoint = joystick.throttle;
+  }
+}
+#endif
 </header>
   <waypoints>
     <waypoint name="HOME" x="0" y="0"/>
@@ -18,6 +29,12 @@
     <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
     <waypoint name="CLIMB" x="-114.5" y="162.3"/>
   </waypoints>
+  <variables>
+    <variable var="joystick_block" type="uint8_t" init="255"/>
+  </variables>
+  <modules>
+    <module name="joystick"/>
+  </modules>
   <exceptions/>
   <blocks>
     <block name="Wait GPS">
@@ -77,7 +94,7 @@
       <deroute block="land"/>
     </block>
     <block name="land">
-      <call fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
+      <call_once fun="nav_compute_baseleg(WP_AF, WP_TD, WP__BASELEG, nav_radius)"/>
       <circle radius="nav_radius" until="NavCircleCount() > 0.5" wp="_BASELEG"/>
       <circle radius="nav_radius" until="NavQdrCloseTo(DegOfRad(baseleg_out_qdr)-(nav_radius/fabs(nav_radius))*10) && 10 > fabs(GetPosAlt() - WaypointAlt(WP__BASELEG))" wp="_BASELEG"/>
     </block>

conf/flight_plans/rotorcraft_joystick_enac.xml

@@ -0,0 +1,116 @@
+<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">
+
+<flight_plan alt="148" ground_alt="146" lat0="43.5640917" lon0="1.4829201" max_dist_from_home="20" name="Rotorcraft Optitrack (ENAC)" security_height="0.3">
+  <header>
+#ifdef NAV_C
+#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
+#include "autopilot.h"
+static inline void joystick_handler(uint8_t sender_id __attribute__((unused)), int16_t roll, int16_t pitch, int16_t yaw, int16_t throttle) {
+  if (And(autopilot_get_mode() == AP_MODE_NAV, nav_block == joystick_block)) {
+    nav_roll = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_PHI * roll / MAX_PPRZ);
+    nav_pitch = ANGLE_BFP_OF_REAL(STABILIZATION_ATTITUDE_SP_MAX_THETA * pitch / MAX_PPRZ);
+    nav_heading = ANGLE_BFP_OF_REAL(3.1416f * yaw / MAX_PPRZ);
+    nav_throttle = throttle;
+  }
+}
+#endif
+</header>
+  <waypoints>
+    <waypoint name="HOME" x="0.0" y="0.0"/>
+    <waypoint name="GOAL" x="2.0" y="2.0"/>
+    <waypoint name="STDBY" x="-0.7" y="-0.8"/>
+    <waypoint name="TD" x="0.8" y="-1.7"/>
+    <waypoint name="S1" x="3" y="4"/>
+    <waypoint name="S2" x="3" y="-4"/>
+    <waypoint name="S3" x="-3" y="-4"/>
+    <waypoint name="S4" x="-3" y="4"/>
+    <waypoint name="_N1" x="4.5" y="5.2"/>
+    <waypoint name="_N2" x="4.5" y="-5.2"/>
+    <waypoint name="_N3" x="-4.5" y="-5.2"/>
+    <waypoint name="_N4" x="-4.5" y="5.2"/>
+  </waypoints>
+  <sectors>
+    <sector name="Net" color="red">
+      <corner name="_N1"/>
+      <corner name="_N2"/>
+      <corner name="_N3"/>
+      <corner name="_N4"/>
+    </sector>
+    <sector name="Survey" color="green" type="dynamic">
+      <corner name="S1"/>
+      <corner name="S2"/>
+      <corner name="S3"/>
+      <corner name="S4"/>
+    </sector>
+  </sectors>
+  <variables>
+    <variable var="joystick_block" type="uint8_t" init="255"/>
+    <abi_binding name="JOYSTICK" handler="joystick_handler"/>
+  </variables>
+  <modules>
+    <module name="nav" type="survey_rectangle_rotorcraft"/>
+    <module name="joystick"/>
+  </modules>
+  <blocks>
+    <block name="Wait GPS">
+      <call_once fun="NavKillThrottle()"/>
+      <while cond="!GpsFixValid()"/>
+    </block>
+    <block name="Holding point">
+      <call_once fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+    <block name="Start Engine">
+      <call_once fun="NavResurrect()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+    <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
+      <exception cond="stateGetPositionEnu_f()->z > 1.0" deroute="Standby"/>
+      <call_once fun="NavSetWaypointHere(WP_STDBY)"/>
+      <stay climb="nav_climb_vspeed" vmode="climb" wp="STDBY"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+      <stay wp="STDBY"/>
+    </block>
+    <block name="START" strip_button="Go" strip_icon="lookfore.png">
+      <call_once fun="NavSetWaypointHere(WP_GOAL)"/>
+    </block>
+    <block name="StayGoal">
+      <stay wp="GOAL"/>
+    </block>
+    <block name="Joystick" strip_button="J">
+      <set var="horizontal_mode" value="HORIZONTAL_MODE_ATTITUDE"/>
+      <set var="vertical_mode" value="VERTICAL_MODE_MANUAL"/>
+      <set var="joystick_block" value="nav_block"/>
+      <while cond="TRUE"/>
+    </block>
+    <block group="extra_pattern" name="Survey S1-S2 Sweep NS SET" strip_button="SvySweep-NS" strip_icon="survey_rect_ns.png">
+      <call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, 1, NS)"/>
+      <deroute block="Survey RECTANGLE RUN"/>
+    </block>
+    <block group="extra_pattern" name="Survey S1-S2 Sweep WE SET" strip_button="SvySweep-WE" strip_icon="survey_rect_we.png">
+      <call_once fun="nav_survey_rectangle_rotorcraft_setup(WP_S1, WP_S3, 1, WE)"/>
+      <deroute block="Survey RECTANGLE RUN"/>
+    </block>
+    <block group="extra_pattern" name="Survey RECTANGLE RUN" strip_button="SvySweep CONT" strip_icon="survey_rect_run.png">
+      <exception cond="rectangle_survey_sweep_num == 1" deroute="Standby"/>
+      <call fun="nav_survey_rectangle_rotorcraft_run(WP_S1, WP_S3)"/>
+    </block>
+    <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
+      <call_once fun="NavSetWaypointHere(WP_TD)"/>
+    </block>
+    <block name="land">
+      <go wp="TD"/>
+    </block>
+    <block name="flare">
+      <exception cond="NavDetectGround()" deroute="Holding point"/>
+      <exception cond="!nav_is_in_flight()" deroute="landed"/>
+      <call_once fun="NavStartDetectGround()"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
+    </block>
+    <block name="landed">
+      <call_once fun="NavKillThrottle()"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>

conf/modules/joystick.xml

@@ -0,0 +1,22 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="joystick">
+  <doc>
+    <description>
+      Handle JOYSTICK_RAW messages
+
+      Each new message is parsed and stored into the joystick structure.
+      An ABI message JOYSTICK is sent and can be used for control or payload
+      depending on the application.
+    </description>
+  </doc>
+  <header>
+    <file name="joystick.h"/>
+  </header>
+  <init fun="joystick_init()"/>
+  <datalink message="JOYSTICK_RAW" fun="joystick_parse()"/>
+  <makefile>
+    <file name="joystick.c"/>
+  </makefile>
+</module>
+

sw/airborne/firmwares/fixedwing/fixedwing_datalink.c

@@ -47,19 +47,6 @@
 
 #define MOfMM(_x) (((float)(_x))/1000.)
 
-#if USE_JOYSTICK
-#include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
-#include "autopilot.h"
-uint8_t joystick_block;
-#define JoystickHandeDatalink(_roll_int8, _pitch_int8, _throttle_int8) { \
-    if (autopilot_get_mode() == AP_MODE_AUTO2 && nav_block == joystick_block) {  \
-      h_ctl_roll_setpoint = _roll_int8 * (AUTO1_MAX_ROLL / 0x7f);       \
-      h_ctl_pitch_setpoint = _pitch_int8 * (AUTO1_MAX_PITCH / 0x7f);    \
-      v_ctl_throttle_setpoint = (MAX_PPRZ/0x7f) * _throttle_int8;       \
-    }                                                                   \
-  }
-#endif
-
 void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct transport_tx *trans __attribute__((unused)), uint8_t *buf)
 {
   uint8_t msg_id = IdOfPprzMsg(buf);
@@ -152,16 +139,6 @@ void firmware_parse_msg(struct link_device *dev __attribute__((unused)), struct
     break;
 #endif /* HITL */
 
-#if USE_JOYSTICK
-    case DL_JOYSTICK_RAW: {
-      if (DL_JOYSTICK_RAW_ac_id(buf) == AC_ID) {
-        JoystickHandeDatalink(DL_JOYSTICK_RAW_roll(buf),
-                              DL_JOYSTICK_RAW_pitch(buf),
-                              DL_JOYSTICK_RAW_throttle(buf));
-      }
-    }
-    break;
-#endif // USE_JOYSTICK
     default:
       break;
   }

sw/airborne/firmwares/fixedwing/joystick.h

@@ -1,6 +0,0 @@
-#ifndef JOYSTICK_H
-#define JOYSTICK_H
-
-extern uint8_t joystick_block;
-
-#endif

sw/airborne/modules/joystick/joystick.c

@@ -0,0 +1,52 @@
+/*
+ * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/joystick/joystick.c"
+ * @author Gautier Hattenberger
+ * Handle JOYSTICK_RAW messages
+ */
+
+#include "modules/joystick/joystick.h"
+#include "subsystems/datalink/datalink.h"
+#include "subsystems/abi.h"
+#include "pprzlink/dl_protocol.h"
+#include "generated/airframe.h"
+
+struct Joystick joystick;
+
+void joystick_init(void)
+{
+  joystick.roll = 0;
+  joystick.pitch = 0;
+  joystick.yaw = 0;
+  joystick.throttle = 0;
+}
+
+void joystick_parse(void)
+{
+  if (DL_JOYSTICK_RAW_ac_id(dl_buffer) == AC_ID) {
+    joystick.roll = DL_JOYSTICK_RAW_roll(dl_buffer);
+    joystick.pitch = DL_JOYSTICK_RAW_pitch(dl_buffer);
+    joystick.yaw = DL_JOYSTICK_RAW_yaw(dl_buffer);
+    joystick.throttle = DL_JOYSTICK_RAW_throttle(dl_buffer);
+    AbiSendMsgJOYSTICK(JOYSTICK_ID, joystick.roll, joystick.pitch, joystick.yaw, joystick.throttle);
+  }
+}
+

sw/airborne/modules/joystick/joystick.h

@@ -0,0 +1,55 @@
+/*
+ * Copyright (C) 2019 Gautier Hattenberger <gautier.hattenberger@enac.fr>
+ *
+ * This file is part of paparazzi
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, see
+ * <http://www.gnu.org/licenses/>.
+ */
+/**
+ * @file "modules/joystick/joystick.h"
+ * @author Gautier Hattenberger
+ * Handle JOYSTICK_RAW messages
+ */
+
+#ifndef JOYSTICK_H
+#define JOYSTICK_H
+
+#include "std.h"
+
+/**
+ * Joystick structure
+ */
+struct Joystick {
+  int32_t roll;     ///< roll command
+  int32_t pitch;    ///< pitch command
+  int32_t yaw;      ///< yaw command
+  int32_t throttle; ///< throttle command
+};
+
+extern struct Joystick joystick;
+
+/**
+ * Init function
+ */
+extern void joystick_init(void);
+
+/**
+ * JOYSTICK_RAW message parser
+ * if valid, send a JOYSTICK ABI message
+ */
+extern void joystick_parse(void);
+
+#endif
+

sw/airborne/subsystems/abi_sender_ids.h

@@ -425,4 +425,11 @@
 #define DETECT_GATE_ABI_ID 33
 #endif
 
+/*
+ * JOYSTICK message (used for payload or control, but not as a RC)
+ */
+#ifndef JOYSTICK_ID
+#define JOYSTICK_ID 1
+#endif
+
 #endif /* ABI_SENDER_IDS_H */