From 7ed9a3adcca0c9ad68c34ad0961f7b8494151252 Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Tue, 12 Mar 2013 14:29:50 +0100
Subject: [PATCH] [sonar] some fix for sonar module

sonar_scale is now a float in m/adc
---
 sw/airborne/modules/sonar/sonar_adc.c | 28 +++++++++++++++++----------
 sw/airborne/modules/sonar/sonar_adc.h | 14 ++++++++++----
 2 files changed, 28 insertions(+), 14 deletions(-)

diff --git a/sw/airborne/modules/sonar/sonar_adc.c b/sw/airborne/modules/sonar/sonar_adc.c
index e828670c3d5..96672c573e9 100644
--- a/sw/airborne/modules/sonar/sonar_adc.c
+++ b/sw/airborne/modules/sonar/sonar_adc.c
@@ -1,5 +1,4 @@
 /*
- *
  * Copyright (C) 2010  Gautier Hattenberger, 2013 Tobias Münch
  *
  * This file is part of paparazzi.
@@ -22,28 +21,39 @@
  */
 
 #include "modules/sonar/sonar_adc.h"
+#include "generated/airframe.h"
 #include "mcu_periph/adc.h"
-#include "subsystems/datalink/downlink.h"
 #ifdef SITL
-#include "subsystems/gps.h"
+#include "state.h"
 #endif
 
+#include "mcu_periph/uart.h"
+#include "messages.h"
+#include "subsystems/datalink/downlink.h"
 #ifndef DOWNLINK_DEVICE
 #define DOWNLINK_DEVICE DOWNLINK_AP_DEVICE
 #endif
 
+/** Sonar offset.
+ *  Offset value in m (float)
+ *  equals to the height when the ADC gives 0
+ */
 #ifndef SONAR_OFFSET
-#define SONAR_OFFSET 0
+#define SONAR_OFFSET 0.
 #endif
+
+/** Sonar scale.
+ *  Sensor sensitivity in m/adc (float)
+ */
 #ifndef SONAR_SCALE
-#define SONAR_SCALE 166
+#define SONAR_SCALE 0.0166
 #endif
 
 uint16_t sonar_meas;
 bool_t sonar_data_available;
 float sonar_distance;
 float sonar_offset;
-uint16_t sonar_scale;
+float sonar_scale;
 
 #ifndef SITL
 static struct adc_buf sonar_adc;
@@ -67,12 +77,10 @@ void sonar_adc_read(void) {
 #ifndef SITL
   sonar_meas = sonar_adc.sum / sonar_adc.av_nb_sample;
   sonar_data_available = TRUE;
-//sonar_offset in cm, sonar_distance in m!
-  sonar_distance = ((float)sonar_meas * (float)sonar_scale) / 10000 + sonar_offset;
+  sonar_distance = ((float)sonar_meas * sonar_scale) + sonar_offset;
 
 #else // SITL
-  sonar_distance = (gps.hmsl / 1000.0) - ground_alt;
-  //sonar_meas = (sonar_distance - sonar_offset / (sonar_scale / 1000);
+  sonar_distance = stateGetPositionEnu_f()->z;
   Bound(sonar_distance, 0.1f, 7.0f);
 #endif // SITL
 
diff --git a/sw/airborne/modules/sonar/sonar_adc.h b/sw/airborne/modules/sonar/sonar_adc.h
index bf66d5e0e90..ec5a8c06cb8 100644
--- a/sw/airborne/modules/sonar/sonar_adc.h
+++ b/sw/airborne/modules/sonar/sonar_adc.h
@@ -1,5 +1,4 @@
 /*
- *
  * Copyright (C) 2010  Gautier Hattenberger
  *
  * This file is part of paparazzi.
@@ -21,9 +20,8 @@
  *
  */
 
-/** \file sonar_maxbotix.h
- *
- * simple driver to deal with one maxbotix sensor
+/** @file sonar_adc.h
+ *  @brief simple driver to deal with one sonar sensor on ADC
  */
 
 #ifndef SONAR_ADC_H
@@ -31,8 +29,16 @@
 
 #include "std.h"
 
+/** Raw ADC value.
+ */
 extern uint16_t sonar_meas;
+
+/** New data available.
+ */
 extern bool_t sonar_data_available;
+
+/** Sonar distance in m.
+ */
 extern float sonar_distance;
 
 extern void sonar_adc_init(void);