[lidar] Cleanup and added lidar for NPS targets

conf/airframes/AGGIEAIR/aggieair_conf.xml

@@ -29,7 +29,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_fixedwing.xml"
    flight_plan="flight_plans/AGGIEAIR/BasicTuning_Launcher.xml"
    settings="settings/fixedwing_basic.xml settings/nps.xml settings/control/ctl_basic.xml"
-   settings_modules="modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml"
+   settings_modules="modules/lidar_lite.xml modules/nav_skid_landing.xml modules/nav_survey_poly_osam.xml modules/gps.xml"
    gui_color="#00009e93ffff"
   />
 </conf>

conf/airframes/AGGIEAIR/aggieair_iris_indi.xml

@@ -19,11 +19,6 @@
     <define name="BAT_CHECKER_DELAY" value="80" /><!-- in seconds-->
     <define name="CATASTROPHIC_BATTERY_KILL_DELAY" value="80" /><!-- in seconds-->
 
-    <target name="nps" board="pc">
-      <module name="fdm" type="jsbsim" />
-      <module name="udp" />
-    </target>
-
     <module name="telemetry" type="transparent">
       <configure name="MODEM_PORT" value="UART2"/>
       <configure name="MODEM_BAUD" value="B115200"/>

conf/airframes/AGGIEAIR/aggieair_rp3_lia.xml

@@ -60,6 +60,7 @@ RP3 Lisa MX
       <configure name="EXTRA_DL_PORT" value="UART1"/>
       <configure name="EXTRA_DL_BAUD" value="B921600"/>
     </module>
+    <module name="lidar_lite"/>
   </modules>
 
   <!-- commands section -->

conf/modules/lidar_lite.xml

@@ -42,6 +42,9 @@
     <define name="LIDAR_LITE_COMPENSATE_ROTATION" value="$(LIDAR_LITE_COMPENSATE_ROTATION)"/>
     <file name="lidar_lite.c"/>
   </makefile>
+  <makefile target="nps">
+    <define name="USE_SONAR" value="1"/><!-- in NPS use a virtual sonar to simulate lidar measurements -->
+  </makefile>
 </module>
 
 

conf/modules/lidar_sf11.xml

@@ -1,6 +1,6 @@
 <!DOCTYPE module SYSTEM "module.dtd">
 
-<module name="lidar_lite" dir="lidar">
+<module name="lidar_sf11" dir="lidar">
   <doc>
     <description>
       Parallax SF11-A/B/C Laser Rangefinder connected over i2c bus.
@@ -43,6 +43,9 @@
     <define name="LIDAR_SF11_COMPENSATE_ROTATION" value="$(LIDAR_SF11_COMPENSATE_ROTATION)"/>
     <file name="lidar_sf11.c"/>
   </makefile>
+  <makefile target="nps">
+    <define name="USE_SONAR" value="1"/><!-- in NPS use a virtual sonar to simulate lidar measurements -->
+  </makefile>
 </module>
 
 

sw/airborne/modules/lidar/lidar_sf11.c

@@ -45,7 +45,7 @@ void lidar_sf11_init(void)
 {
   lidar_sf11.trans.status = I2CTransDone;
   lidar_sf11.addr = LIDAR_SF11_I2C_ADDR;
-  lidar_sf11.status = LIDAR_LITE_REQ_READ;
+  lidar_sf11.status = LIDAR_SF11_REQ_READ;
   lidar_sf11.update_agl = USE_LIDAR_SF11_AGL;
   lidar_sf11.compensate_rotation = LIDAR_SF11_COMPENSATE_ROTATION;
 

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -55,6 +55,11 @@
 // for datalink_time hack
 #include "subsystems/datalink/datalink.h"
 
+#if USE_SONAR
+// for sonar/lidar agl
+#include "subsystems/datalink/downlink.h"
+#endif
+
 struct NpsAutopilot autopilot;
 bool nps_bypass_ahrs;
 bool nps_bypass_ins;
@@ -147,6 +152,19 @@ void nps_autopilot_run_step(double time)
     Ap(event_task);
   }
 
+#if USE_SONAR
+  if (nps_sensors_sonar_available()) {
+    float dist = (float) sensors.sonar.value;
+    AbiSendMsgAGL(AGL_SONAR_NPS_ID, dist);
+
+    uint16_t foo = 0;
+    DOWNLINK_SEND_SONAR(DefaultChannel, DefaultDevice, &foo, &dist);
+
+    Fbw(event_task);
+    Ap(event_task);
+  }
+#endif
+
   if (nps_bypass_ahrs) {
     sim_overwrite_ahrs();
   }