From 69609df1df2bbd365bde0b69cbe7bbaf35fe7da5 Mon Sep 17 00:00:00 2001
From: Laurent Lalanne <f5soh@free.fr>
Date: Sun, 24 Mar 2019 22:51:15 +0100
Subject: [PATCH] LP-609 Add G force stats and alternate G force / speed/dist
 max status display

---
 .../UAVOHottBridge/inc/uavohottbridge.h       | 22 +++++++++++--------
 .../modules/UAVOHottBridge/uavohottbridge.c   | 22 +++++++++++++++++++
 2 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/flight/modules/UAVOHottBridge/inc/uavohottbridge.h b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h
index 1e378d180..c6c7310dd 100644
--- a/flight/modules/UAVOHottBridge/inc/uavohottbridge.h
+++ b/flight/modules/UAVOHottBridge/inc/uavohottbridge.h
@@ -180,18 +180,19 @@ typedef struct {
 // Private structures
 struct telemetrydata {
     HoTTBridgeSettingsData Settings;
-    AttitudeStateData Attitude;
-    BaroSensorData Baro;
+    AttitudeStateData      Attitude;
+    AccelStateData         Accel;
+    BaroSensorData         Baro;
     FlightBatteryStateData Battery;
-    FlightStatusData FlightStatus;
+    FlightStatusData       FlightStatus;
     GPSPositionSensorData  GPS;
-    AirspeedStateData Airspeed;
+    AirspeedStateData      Airspeed;
     GPSTimeData GPStime;
-    GyroSensorData    Gyro;
-    HomeLocationData  Home;
-    PositionStateData Position;
-    SystemAlarmsData  SysAlarms;
-    VelocityStateData Velocity;
+    GyroSensorData         Gyro;
+    HomeLocationData       Home;
+    PositionStateData      Position;
+    SystemAlarmsData       SysAlarms;
+    VelocityStateData      Velocity;
     int16_t climbratebuffer[climbratesize];
     uint8_t climbrate_pointer;
     float   altitude;
@@ -205,6 +206,9 @@ struct telemetrydata {
     float   homecourse;
     float   max_distance;
     float   max_speed;
+    float   current_G;  // G force
+    float   max_G;
+    float   min_G;
     uint8_t last_armed;
     char    statusline[statussize];
 };
diff --git a/flight/modules/UAVOHottBridge/uavohottbridge.c b/flight/modules/UAVOHottBridge/uavohottbridge.c
index 24f8e1f9f..7551b0033 100644
--- a/flight/modules/UAVOHottBridge/uavohottbridge.c
+++ b/flight/modules/UAVOHottBridge/uavohottbridge.c
@@ -39,6 +39,7 @@
 #include "callbackinfo.h"
 #include "hottbridgesettings.h"
 #include "attitudestate.h"
+#include "accelstate.h"
 #include "barosensor.h"
 #include "flightbatterystate.h"
 #include "flightstatus.h"
@@ -1289,6 +1290,9 @@ void update_telemetrydata()
     if (AttitudeStateHandle() != NULL) {
         AttitudeStateGet(&telestate->Attitude);
     }
+    if (AccelStateHandle() != NULL) {
+        AccelStateGet(&telestate->Accel);
+    }
     if (BaroSensorHandle() != NULL) {
         BaroSensorGet(&telestate->Baro);
     }
@@ -1325,14 +1329,19 @@ void update_telemetrydata()
 
     // send actual climbrate value to ring buffer as mm per 0.2s values
     uint8_t n = telestate->climbrate_pointer;
+    // DEBUG_PRINTF(3, "Current pointer: %d\r\n", n);
     telestate->climbratebuffer[telestate->climbrate_pointer++] = -telestate->Velocity.Down * 200;
     telestate->climbrate_pointer %= climbratesize;
+    // DEBUG_PRINTF(3, "Next pointer: %d\r\n", telestate->climbrate_pointer);
+
+    bool display_G = (n > 25);
 
     // calculate avarage climbrates in meters per 1, 3 and 10 second(s) based on 200ms interval
     telestate->climbrate1s  = 0;
     telestate->climbrate3s  = 0;
     telestate->climbrate10s = 0;
     for (uint8_t i = 0; i < climbratesize; i++) {
+        // DEBUG_PRINTF(3, "%d ", n);
         telestate->climbrate1s  += (i < 5) ? telestate->climbratebuffer[n] : 0;
         telestate->climbrate3s  += (i < 15) ? telestate->climbratebuffer[n] : 0;
         telestate->climbrate10s += (i < 50) ? telestate->climbratebuffer[n] : 0;
@@ -1364,6 +1373,10 @@ void update_telemetrydata()
     // calculate current 3D speed
     float speed_3d = sqrtf((telestate->Velocity.North * telestate->Velocity.North) + (telestate->Velocity.East * telestate->Velocity.East) + (telestate->Velocity.Down * telestate->Velocity.Down)) * MS_TO_KMH;
 
+    // Normal Acceleration (G unit)
+    float nz_alpha = 0.7f;
+    telestate->current_G = ((1 - nz_alpha) * (-telestate->Accel.z / 9.81f)) + (telestate->current_G * nz_alpha);
+
     // check and set min/max values when armed
     // and without receiver input for standalone board used as sensor
     if ((telestate->FlightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) || ((telestate->SysAlarms.Alarm.Attitude == SYSTEMALARMS_ALARM_OK) && (telestate->SysAlarms.Alarm.Receiver != SYSTEMALARMS_ALARM_OK))) {
@@ -1379,6 +1392,12 @@ void update_telemetrydata()
         if (telestate->max_speed < speed_3d) {
             telestate->max_speed = speed_3d;
         }
+        if (telestate->max_G < telestate->current_G) {
+            telestate->max_G = telestate->current_G;
+        }
+        if (telestate->min_G > telestate->current_G) {
+            telestate->min_G = telestate->current_G;
+        }
     }
 
     // statusline
@@ -1488,6 +1507,9 @@ void update_telemetrydata()
     snprintf(telestate->statusline, sizeof(telestate->statusline), "Max %3dkmh Dst %4dm", (uint16_t)telestate->max_speed, (uint16_t)telestate->max_distance);
     // "Max  200kmh"
     // "Dst 1000m "
+    if (display_G) {
+        snprintf(telestate->statusline, sizeof(telestate->statusline), "MaxG %2d.%d  MinG %2d.%d", (int8_t)(telestate->max_G), (int8_t)(telestate->max_G * 10) % 10, (int8_t)(telestate->min_G), (int8_t)abs(telestate->min_G * 10) % 10);
+    }
 }
 
 /**