Mission Computer and payload computer support (#2053)

* Copilot module
* Added mission computer status
* modules_parse_datalink() now accepts arguments
* Update pprzlink
* Update cartography module to comply with the extended CAMERA_SNAPSHOT message

conf/airframes/AGGIEAIR/aggieair_control_panel.xml

@@ -82,7 +82,9 @@
        <arg flag="-s" constant="57600"/>
      </program>
      <program name="Server"/>
-     <program name="GCS"/>
+     <program name="GCS">
+      <arg flag="-layout" constant="AGGIEAIR/vertical.xml"/>
+     </program>
      <program name="Messages"/>
      <program name="Real-time Plotter"/>
    </session>
@@ -135,6 +137,23 @@
    </session>
 
 
+   <session name="NPS Flight Gear">
+     <program name="Data Link">
+       <arg flag="-udp"/>
+       <arg flag="-udp_broadcast"/>
+     </program>
+     <program name="Server">
+      <arg flag="-n"/>
+     </program>
+     <program name="GCS"/>
+     <program name="Simulator">
+      <arg flag="-a" constant="@AIRCRAFT"/>
+      <arg flag="-f" constant="127.0.0.1"/>
+      <arg flag="-b" constant="127.255.255.255"/>
+      <arg flag="--fg_fdm" constant=""/>
+     </program>
+   </session>
+
    <session name="NPS Flight Gear">
      <program name="Data Link">
        <arg flag="-udp"/>

conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml

@@ -57,6 +57,7 @@ RP3 Lisa MX
     <module name="nav" type="launcher"/>
     <module name="nav" type="skid_landing"/>
     <module name="sys_mon"/>
+    <module name="copilot"/>
     <module name="extra_dl">
       <!-- in order to use uart1 without chibios we need to remap the peripheral-->
       <define name="REMAP_UART1" value="TRUE"/>

conf/flight_plans/AGGIEAIR/BasicTuning_Launcher.xml

@@ -22,6 +22,7 @@
     <waypoint alt="1347.0" name="TD" x="1155.4" y="-354.1"/>
     <waypoint name="CLIMB" x="1142.0" y="-97.9"/>
     <waypoint name="_BASELEG" x="168.8" y="-13.8"/>
+    <waypoint name="PAYLOAD" x="100" y="50"/>
   </waypoints>
   <sectors>
     <sector name="Section1">

conf/gcs/AGGIEAIR/vertical.xml

@@ -0,0 +1,52 @@
+<layout width="1280" height="709">
+  <columns>
+    <rows SIZE="600">
+      <widget NAME="strips" SIZE="225"/>
+      <widget NAME="aircraft" SIZE="350"/>
+      <widget NAME="alarms"/>
+    </rows>
+    <rows>
+      <widget NAME="map2d">
+        <papget type="message_field" display="gauge" x="77" y="304">
+          <property name="field" value="CAMERA_PAYLOAD:used_memory"/>
+          <property name="scale" value="1.0"/>
+          <property name="min" value="0."/>
+          <property name="max" value="100."/>
+          <property name="size" value="50."/>
+          <property name="text" value="Used memory(%)"/>
+        </papget>
+        <papget type="message_field" display="gauge" x="74" y="198">
+          <property name="field" value="CAMERA_PAYLOAD:used_disk"/>
+          <property name="scale" value="1.0"/>
+          <property name="min" value="0."/>
+          <property name="max" value="100."/>
+          <property name="size" value="50."/>
+          <property name="text" value="Used disk(%)"/>
+        </papget>
+        <papget type="message_field" display="text" x="45" y="414">
+          <property name="field" value="CAMERA_PAYLOAD:error_code"/>
+          <property name="size" value="15."/>
+          <property name="format" value="Error code: %.0f"/>
+          <property name="color" value="green"/>
+        </papget>
+        <papget type="message_field" display="led" x="151" y="395">
+          <property name="scale" value="1"/>
+          <property name="field" value="CAMERA_PAYLOAD:error_code"/>
+          <property name="size" value="15."/>
+          <property name="text" value="Error:"/>
+          <property name="test_value" value="0"/> <!-- will be green only if value==0 -->
+          <property name="test_invert" value="true"/>
+        </papget>
+        <papget type="message_field" display="led" x="152" y="366">
+          <property name="scale" value="1"/>
+          <property name="field" value="CAMERA_PAYLOAD:door_status"/>
+          <property name="size" value="15."/>
+          <property name="text" value="Door status:"/>
+          <property name="test_value" value="2"/> <!-- will be green only if value==2 -->
+          <property name="test_invert" value="true"/>
+        </papget>
+
+      </widget>
+    </rows>
+  </columns>
+</layout>

conf/modules/copilot.xml

@@ -0,0 +1,41 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="copilot" dir="mission">
+  <doc>
+    <description>
+    Mission Computer module, interfacing the mission computer (also known as Copilot),
+    based losely on
+    ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module
+    Based on paper "A Payload Verification and Management Framework
+    for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans
+    and Chris Coffin. Link: http://ieeexplore.ieee.org/abstract/document/6309316/
+
+    More info can be found on http://wiki.paparazziuav.org/wiki/Mission_computer
+
+    Copilot is intended mainly for mapping applications.
+
+    This module processes messages from Copilot, and either forwards them to the GCS
+    (such as CAMERA_SNAPSHOT or CAMERA_PAYLOAD messages), or responds to them as necessary
+    (such as MOVE_WP).
+
+    The module assumes the source of the messages is trusted (i.e. not authentication besides 
+    AC_ID check is performed).
+    </description>
+  </doc>
+
+
+  <depends>extra_dl</depends>
+  <header>
+    <file name="copilot.h"/>
+  </header>
+  <init fun="copilot_init()"/>
+  <periodic fun="copilot_periodic()" freq="1." autorun="TRUE"/>
+  <datalink message="CAMERA_SNAPSHOT_DL" fun="copilot_parse_cam_snapshot_dl(buf)"/>
+  <datalink message="CAMERA_PAYLOAD_DL" fun="copilot_parse_cam_payload_dl(buf)"/>
+  <datalink message="COPILOT_STATUS_DL" fun="copilot_parse_copilot_status_dl(buf)"/>
+  <datalink message="MOVE_WP" fun="copilot_parse_move_wp_dl(buf)"/>
+  <makefile target="ap">
+    <file name="copilot.c"/>
+  </makefile>
+</module>
+

conf/telemetry/AGGIEAIR/aggieair_fixedwing.xml

@@ -5,7 +5,7 @@
     <mode name="default">
       <message name="AUTOPILOT_VERSION"   period="11.1"/>
       <message name="ALIVE"               period="5.1"/>
-      <message name="GPS"                 period="0.25"/>
+      <message name="GPS_LLA"             period="0.25"/>
       <message name="NAVIGATION"          period="1."/>
       <message name="ATTITUDE"            period="0.1"/>
       <message name="ESTIMATOR"           period="0.5"/>
@@ -21,6 +21,7 @@
       <message name="DL_VALUE"            period="1.5"/>
       <message name="IR_SENSORS"          period="1.2"/>
       <message name="SURVEY"              period="2.1"/>
+      <message name="COMMANDS"            period="1"/>
       <message name="FBW_STATUS"          period="2"/>
       <message name="VECTORNAV_INFO"      period="1.0"/>
     </mode>
@@ -106,7 +107,10 @@
   </process>
   <process name="Extra">
     <mode name="default">
-    <message name="COMMANDS"            period="0.01"/>
+      <message name="COMMANDS"            period="0.01"/>
+      <message name="GPS_LLA"             period="1.25"/>
+      <message name="ATTITUDE"            period="1.1"/>
+      <message name="VECTORNAV_INFO"      period="1.0"/>
     </mode>
   </process>
 </telemetry>

sw/airborne/modules/cartography/cartography.c

@@ -151,7 +151,16 @@ void init_carto(void)
 
 void periodic_downlink_carto(void)
 {
-  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice, &camera_snapshot_image_number);
+  static uint16_t dummy_id = 0;
+  static uint8_t dummy_state = 0;
+  static uint8_t snapshot_valid = 1;
+  static float dummy_temp = NAN;
+  DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
+      &dummy_d,
+      &dummy_state,
+      &camera_snapshot_image_number,
+      &snapshot_valid,
+      &dummy_temp);
 }
 
 void start_carto(void)

sw/airborne/modules/datalink/extra_pprz_dl.h

@@ -41,6 +41,8 @@
 /* PPRZ transport structure */
 extern struct pprz_transport extra_pprz_tp;
 
+extern uint8_t extra_dl_buffer[MSG_SIZE]  __attribute__((aligned));
+
 /** Datalink Event */
 void extra_pprz_dl_event(void);
 

sw/airborne/modules/mission/copilot.c

@@ -0,0 +1,247 @@
+/*
+ * Copyright (C) 2016  2017 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/**
+ * @file "modules/mission/copilot.c"
+ *
+ *  Mission Computer module, interfacing the mission computer (also known as Copilot),
+ *  based losely on
+ *  ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module
+ *  Based on paper "A Payload Verification and Management Framework
+ *  for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans
+ *  and Chris Coffin. Link: http://ieeexplore.ieee.org/abstract/document/6309316/
+ *
+ *  More info can be found on http://wiki.paparazziuav.org/wiki/Mission_computer
+ *
+ *  Copilot is intended mainly for mapping applications.
+ *
+ *   This module processes messages from Copilot, and either forwards them to the GCS
+ *  (such as CAMERA_SNAPSHOT or CAMERA_PAYLOAD messages), or responds to them as necessary
+ *  (such as MOVE_WP).
+ *
+ *  The module assumes the source of the messages is trusted (i.e. not authentication besides
+ *  AC_ID check is performed).
+ */
+
+#include "modules/mission/copilot.h"
+#include "subsystems/datalink/telemetry.h"
+#include <string.h>
+
+#include "pprz_mutex.h"
+
+// needed for WP_MOVED confirmation
+#include "firmwares/fixedwing/nav.h"
+#include "subsystems/navigation/common_nav.h"
+#include "math/pprz_geodetic_float.h"
+
+bool send_cam_snapshot;
+bool send_cam_payload;
+bool send_copilot_status;
+
+struct CameraPayload cam_payload;
+struct CameraSnapshot cam_snapshot;
+struct CopilotStatus copilot_status;
+
+PPRZ_MUTEX(copilot_cam_snapshot_mtx);
+PPRZ_MUTEX(copilot_cam_payload_mtx);
+PPRZ_MUTEX(copilot_status_mtx);
+
+/** Init function */
+void copilot_init(void)
+{
+  send_cam_snapshot = false;
+  send_cam_payload = false;
+  send_copilot_status = false;
+
+  memset(&cam_payload, 0, sizeof(cam_payload));
+  memset(&cam_snapshot, 0, sizeof(cam_snapshot));
+  memset(&copilot_status, 0, sizeof(copilot_status));
+
+  PPRZ_MUTEX_INIT(copilot_cam_snapshot_mtx);
+  PPRZ_MUTEX_INIT(copilot_cam_payload_mtx);
+  PPRZ_MUTEX_INIT(copilot_status_mtx);
+}
+
+/** Periodic function */
+void copilot_periodic(void)
+{
+  PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx);
+  if (send_cam_snapshot)
+  {
+    // send down to GCS
+    DOWNLINK_SEND_CAMERA_SNAPSHOT(DefaultChannel, DefaultDevice,
+        &cam_snapshot.cam_id,
+        &cam_snapshot.cam_state,
+        &cam_snapshot.snapshot_num,
+        &cam_snapshot.snapshot_valid,
+        &cam_snapshot.lens_temp,
+        &cam_snapshot.array_temp);
+
+    send_cam_snapshot = false;
+  }
+  PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx);
+
+
+  PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx);
+  if (send_cam_payload)
+  {
+    // NOTE: to send the message over the EXTRA_DL port
+    // use "DOWNLINK_SEND_CAMERA_PAYLOAD(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,"
+
+    // send down to GCS
+    DOWNLINK_SEND_CAMERA_PAYLOAD(DefaultChannel, DefaultDevice,
+        &cam_payload.timestamp,
+        &cam_payload.used_mem,
+        &cam_payload.used_disk,
+        &cam_payload.door_status,
+        &cam_payload.error_code);
+
+    send_cam_payload = false;
+  }
+  PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx);
+
+  PPRZ_MUTEX_LOCK(copilot_status_mtx);
+  // send down to GCS
+  if (send_copilot_status)
+  {
+    DOWNLINK_SEND_COPILOT_STATUS(DefaultChannel, DefaultDevice,
+        &copilot_status.timestamp,
+        &copilot_status.used_mem,
+        &copilot_status.used_disk,
+        &copilot_status.status,
+        &copilot_status.error_code);
+
+    send_copilot_status = false;
+  }
+  PPRZ_MUTEX_UNLOCK(copilot_status_mtx);
+
+}
+
+/**
+ *
+ * copy CAMERA_SNAPSHOT message and mark it to be sent
+ *
+ * In case of multiple cameras, it is up to the payload computer to send
+ * CAMERA_SNAPSHOT messages for each camera at proper interval, so the values
+ * don't get overwritten.
+ *
+ */
+void copilot_parse_cam_snapshot_dl(uint8_t *buf)
+{
+  PPRZ_MUTEX_LOCK(copilot_cam_snapshot_mtx);
+
+  // copy CAMERA_SNAPSHOT message and mark it to be sent
+  cam_snapshot.cam_id = DL_CAMERA_SNAPSHOT_DL_camera_id(buf);
+  cam_snapshot.cam_state = DL_CAMERA_SNAPSHOT_DL_camera_state(buf);
+  cam_snapshot.snapshot_num = DL_CAMERA_SNAPSHOT_DL_snapshot_image_number(buf);
+  cam_snapshot.snapshot_valid = DL_CAMERA_SNAPSHOT_DL_snapshot_valid(buf);
+  cam_snapshot.lens_temp = DL_CAMERA_SNAPSHOT_DL_lens_temp(buf);
+  cam_snapshot.array_temp = DL_CAMERA_SNAPSHOT_DL_array_temp(buf);
+
+  send_cam_snapshot = true;
+
+  PPRZ_MUTEX_UNLOCK(copilot_cam_snapshot_mtx);
+}
+
+/**
+ * copy CAMERA_PAYLOAD message and mark it to be sent
+ */
+void copilot_parse_cam_payload_dl(uint8_t *buf)
+{
+  PPRZ_MUTEX_LOCK(copilot_cam_payload_mtx);
+
+  cam_payload.timestamp = DL_CAMERA_PAYLOAD_DL_timestamp(buf);
+  cam_payload.used_mem = DL_CAMERA_PAYLOAD_DL_used_memory(buf);
+  cam_payload.used_disk = DL_CAMERA_PAYLOAD_DL_used_disk(buf);
+  cam_payload.door_status = DL_CAMERA_PAYLOAD_DL_door_status(buf);
+  cam_payload.error_code = DL_CAMERA_PAYLOAD_DL_error_code(buf);
+
+  send_cam_payload = true;
+
+  PPRZ_MUTEX_UNLOCK(copilot_cam_payload_mtx);
+}
+
+/**
+ * copy COPILOT_STATUS message and mark it to be sent
+ */
+void copilot_parse_copilot_status_dl(uint8_t *buf)
+{
+  PPRZ_MUTEX_LOCK(copilot_status_mtx);
+
+  copilot_status.timestamp = DL_COPILOT_STATUS_DL_timestamp(buf);
+  copilot_status.used_mem = DL_COPILOT_STATUS_DL_used_memory(buf);
+  copilot_status.used_disk = DL_COPILOT_STATUS_DL_used_disk(buf);
+  copilot_status.status = DL_COPILOT_STATUS_DL_status(buf);
+  copilot_status.error_code = DL_COPILOT_STATUS_DL_error_code(buf);
+
+  send_copilot_status = true;
+
+  PPRZ_MUTEX_UNLOCK(copilot_status_mtx);
+}
+
+/**
+ * If MOVE_WP from GCS
+ *  - processed in  firmware_parse_msg(dev, trans, buf); with regular buffer
+ *  - reponse over telemetry (regular buffer)
+ *  - here send WP_MOVED over extra_dl
+ *
+ *  If MOVE_WP from extra_dl
+ *  - processed in firmware_parse_msg(dev, trans, buf); with extra buffer
+ *  - response over extra_dl
+ *  - send an update to GCS
+ *
+ *  In both cases, the MOVE_WP message was already processed in firmware_parse
+ *  here we are taking care only about propagating the change
+ *
+ */
+void copilot_parse_move_wp_dl(uint8_t *buf)
+{
+  if (DL_MOVE_WP_ac_id(buf) == AC_ID) {
+    uint8_t wp_id = DL_MOVE_WP_wp_id(buf);
+
+    /* Computes from (lat, long) in the referenced UTM zone */
+    struct LlaCoor_f lla;
+    lla.lat = RadOfDeg((float)(DL_MOVE_WP_lat(buf) / 1e7));
+    lla.lon = RadOfDeg((float)(DL_MOVE_WP_lon(buf) / 1e7));
+    lla.alt = ((float)(DL_MOVE_WP_alt(buf)))/1000.;
+    struct UtmCoor_f utm;
+    utm.zone = nav_utm_zone0;
+    utm_of_lla_f(&utm, &lla);
+
+    // Waypoint range is limited. Computes the UTM pos back from the relative
+    // coordinates */
+    utm.east = waypoints[wp_id].x + nav_utm_east0;
+    utm.north = waypoints[wp_id].y + nav_utm_north0;
+
+    if (buf == extra_dl_buffer) {
+       // MOVE_WP came from extra_dl, respond over telemetry
+      DOWNLINK_SEND_WP_MOVED(DefaultChannel, DefaultDevice,
+                             &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
+    }
+
+    if (buf == dl_buffer) {
+      // MOVE_WP came over telemetry, respond over extra_dl
+      DOWNLINK_SEND_WP_MOVED(extra_pprz_tp, EXTRA_DOWNLINK_DEVICE,
+                             &wp_id, &utm.east, &utm.north, &utm.alt, &nav_utm_zone0);
+    }
+
+  }
+}

sw/airborne/modules/mission/copilot.h

@@ -0,0 +1,92 @@
+/*
+ * Copyright (C) 2016  2017 Michal Podhradsky <http://github.com/podhrmic>
+ *
+ * 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, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/**
+ * @file "modules/mission/copilot.h"
+ *
+ *  Mission Computer module, interfacing the mission computer (also known as Copilot),
+ *  based losely on
+ *  ISaAC: The Intelligent Safety and Airworthiness Co-Pilot module
+ *  Based on paper "A Payload Verification and Management Framework
+ *  for Small UAV-based Personal Remote Sensing Systems" by Cal Coopmans
+ *  and Chris Coffin. Link: http://ieeexplore.ieee.org/abstract/document/6309316/
+ *
+ *  More info can be found on http://wiki.paparazziuav.org/wiki/Mission_computer
+ *
+ *  Copilot is intended mainly for mapping applications.
+ *
+ *   This module processes messages from Copilot, and either forwards them to the GCS
+ *  (such as CAMERA_SNAPSHOT or CAMERA_PAYLOAD messages), or responds to them as necessary
+ *  (such as MOVE_WP).
+ *
+ *  The module assumes the source of the messages is trusted (i.e. not authentication besides
+ *  AC_ID check is performed).
+ */
+
+#ifndef COPILOT_H
+#define COPILOT_H
+
+#include "subsystems/datalink/datalink.h"
+#include "modules/datalink/extra_pprz_dl.h"
+
+struct CameraPayload {
+  float timestamp;
+  uint8_t used_mem;
+  uint8_t used_disk;
+  uint8_t door_status;
+  uint8_t error_code;
+};
+
+struct CameraSnapshot {
+  uint16_t cam_id;
+  uint8_t cam_state;
+  uint16_t snapshot_num;
+  uint8_t snapshot_valid;
+  float lens_temp;
+  float array_temp;
+};
+
+struct CopilotStatus {
+  float timestamp;
+  uint8_t used_mem;
+  uint8_t used_disk;
+  uint8_t status;
+  uint8_t error_code;
+};
+
+extern bool send_cam_snapshot;
+extern bool send_cam_payload;
+extern bool send_copilot_status;
+
+/** Init function */
+void copilot_init(void);
+
+/** Periodic function */
+void copilot_periodic(void);
+
+/** Message processing functions */
+void copilot_parse_cam_snapshot_dl(uint8_t *buf);
+void copilot_parse_cam_payload_dl(uint8_t *buf);
+void copilot_parse_copilot_status_dl(uint8_t *buf);
+void copilot_parse_move_wp_dl(uint8_t *buf);
+
+#endif /* COPILOT_H */
+

sw/airborne/subsystems/datalink/datalink.c

@@ -146,7 +146,7 @@ void dl_parse_msg(struct link_device *dev, struct transport_tx *trans, uint8_t *
   firmware_parse_msg(dev, trans, buf);
 
   /* Parse modules datalink */
-  modules_parse_datalink(msg_id);
+  modules_parse_datalink(msg_id, dev, trans, buf);
 }
 
 /* default empty WEAK implementation for firmwares without an extra firmware_parse_msg */

sw/tools/generators/gen_modules.ml

@@ -304,7 +304,10 @@ let print_event_functions = fun modules ->
 let print_datalink_functions = fun modules ->
   lprintf out_h "\n#include \"pprzlink/messages.h\"\n";
   lprintf out_h "#include \"generated/airframe.h\"\n";
-  lprintf out_h "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused))) {\n";
+  lprintf out_h "static inline void modules_parse_datalink(uint8_t msg_id __attribute__ ((unused)),
+                                          struct link_device *dev __attribute__((unused)),
+                                          struct transport_tx *trans __attribute__((unused)),
+                                          uint8_t *buf __attribute__((unused))) {\n";
   right ();
   let else_ = ref "" in
   List.iter (fun m ->