Merge branch 'release/v1.2.1'

metisvela · Jul 29, 2022 · a085898 · a085898
2 parents 3069aad + acc7952
commit a085898
Show file tree

Hide file tree

Showing 2 changed files with 41 additions and 36 deletions.
diff --git a/Automation_Custom_Script.sh b/Automation_Custom_Script.sh
@@ -23,8 +23,6 @@ G_EXEC /boot/dietpi/dietpi-services dietpi_controlled sailtrack-status
 G_EXEC /boot/dietpi/dietpi-services dietpi_controlled sailtrack-timesync
 G_EXEC /boot/dietpi/dietpi-services dietpi_controlled sailtrack-tileserver
 G_EXEC /boot/dietpi/dietpi-services dietpi_controlled sailtrack-filter
-G_EXEC /boot/dietpi/dietpi-services restart grafana-server
-G_EXEC sleep 5
 
 # Configure DietPi Banner
 G_EXEC touch /boot/dietpi/.dietpi-banner
@@ -33,12 +31,11 @@ for i in "${!SETTINGS[@]}"; do
   G_CONFIG_INJECT "aENABLED\[$i]=" "aENABLED[$i]=${SETTINGS[$i]}" /boot/dietpi/.dietpi-banner
 done
 
-# TODO: Investigate failure here
-
 # Configure passwords and keys
+G_EXEC /boot/dietpi/dietpi-services restart grafana-server
 GLOBAL_PASSWORD=$(openssl enc -d -a -md sha256 -aes-256-cbc -iter 10000 -salt -pass pass:'DietPiRocks!' -in /var/lib/dietpi/dietpi-software/.GLOBAL_PW.bin)
 GRAFANA_API_KEY=$(\
-  curl -s -X POST -H "Content-Type: application/json" -d '{"name":"sailtrack", "role": "Admin"}' "http://admin:$GLOBAL_PASSWORD@localhost:3001/api/auth/keys" | \
+  curl --retry 10 --retry-delay 5 --retry-connrefused -s -X POST -H "Content-Type: application/json" -d '{"name":"sailtrack", "role": "Admin"}' "http://admin:$GLOBAL_PASSWORD@localhost:3001/api/auth/keys" | \
   python3 -c "import sys, json; print(json.load(sys.stdin)['key'])" \
 )
 GCI_PASSWORD=1 G_CONFIG_INJECT "SAILTRACK_GLOBAL_PASSWORD=" "SAILTRACK_GLOBAL_PASSWORD=$GLOBAL_PASSWORD" /etc/default/sailtrack
@@ -50,7 +47,7 @@ telegraf --section-filter=agent config > /etc/telegraf/telegraf.conf
 G_CONFIG_INJECT "omit_hostname =" "  omit_hostname = true" /etc/telegraf/telegraf.conf
 
 # Configure Grafana
-G_EXEC curl -X PUT -H "Content-Type: application/json" -d '{"name":"SailTrack"}' "http://admin:$GLOBAL_PASSWORD@localhost:3001/api/org"
+G_EXEC curl --retry 10 --retry-delay 5 --retry-connrefused -s -X PUT -H "Content-Type: application/json" -d '{"name":"SailTrack"}' "http://admin:$GLOBAL_PASSWORD@localhost:3001/api/org"
 G_EXEC grafana-cli --pluginUrl=https://github.com/alexandrainst/alexandra-trackmap-panel/archive/master.zip plugins install alexandra-trackmap-panel
 G_CONFIG_INJECT ";allow_loading_unsigned_plugins =" "allow_loading_unsigned_plugins = alexandra-trackmap-panel" /etc/grafana/grafana.ini
 G_CONFIG_INJECT ";disable_sanitize_html =" "disable_sanitize_html = true" /etc/grafana/grafana.ini

diff --git a/sailtrack/sailtrack-filter b/sailtrack/sailtrack-filter
@@ -3,8 +3,8 @@
 import json
 import logging
 import math
-import os
 from datetime import timedelta
+import os
 
 import numpy as np
 from filterpy.kalman import KalmanFilter
@@ -17,6 +17,7 @@ KALMAN_SAMPLE_TIME = 0.01
 MQTT_PUBLISH_FREQ_HZ = 5
 
 MQTT_CLIENT_ID = "sailtrack-filter_boat"
+CONFIG_FILE_PATH = "/etc/sailtrack/sailtrack.conf"
 
 MQTT_JOB_INTERVAL_MS = 1000 / MQTT_PUBLISH_FREQ_HZ
 
@@ -29,21 +30,25 @@ LAT_FACTOR = 1
 gps_ref = {}
 gps_data = {}
 imu_data = {}
+wind_data = {}
 orientation = {
     "heading": 0,
     "pitch": 0,
     "roll": 0
 }
 wind_metrics = {}
-body2world_mtx = []
-body2sailor_mtx = np.matrix([[0, 1],
-                            [-1, 0]])
+world2body_mtx = np.matrix([[1, 0, 0],
+                            [0, 1, 0],
+                            [0, 0, 1]])
+ned2neu_mtx = np.matrix([[1, 0, 0],
+                         [0, 1, 0],
+                         [0, 0, -1]])
+
 def angle_wrap_180(angle):
     if angle > 180:
         angle -= 360
     return angle
 
-
 def on_message_callback_gps(client, userdata, message):
     global gps_ref
     global gps_data
@@ -72,7 +77,7 @@ def on_message_callback_gps(client, userdata, message):
 def on_message_callback_imu(client, userdata, message):
     global imu_data
     global orientation
-    global body2world_mtx
+    global world2body_mtx
     imu_data = json.loads(message.payload)
 
     orientation["heading"] = 360 - imu_data["euler"]["z"]
@@ -81,13 +86,13 @@ def on_message_callback_imu(client, userdata, message):
 
     # Rotate linear acceleration from Body Reference Frame (BRF) to World Reference Frame (WRT)
 
-    body2world_mtx = np.matrix([
+    world2body_mtx = np.matrix([
         [math.cos(math.radians(orientation["heading"])), math.sin(math.radians(orientation["heading"])), 0],
         [-math.sin(math.radians(orientation["heading"])), math.cos(math.radians(orientation["heading"])), 0],
         [0, 0, 1]
     ])
 
-    linear_accel_wrf = body2world_mtx @ np.array(list(imu_data["linearAccel"].values())).reshape(3, 1)
+    linear_accel_wrf = world2body_mtx @ np.array(list(imu_data["linearAccel"].values())).reshape(3, 1)
 
     imu_data["linearAccel"]["x"] = linear_accel_wrf[0]
     imu_data["linearAccel"]["y"] = linear_accel_wrf[1]
@@ -137,7 +142,7 @@ def wind_filter_init():
 
 # STATE SPACE MODEL FOR POSITION DYNAMICS
 def boat_filter_init(sample_time):
-    w_std = 0.01         # State noise standard deviation
+    w_std = 1         # State noise standard deviation
     transition_mtx = np.matrix([[1, 0, 0, sample_time, 0, 0],
                                 [0, 1, 0, 0, sample_time, 0],
                                 [0, 0, 1, 0, 0, sample_time],
@@ -167,7 +172,6 @@ def boat_filter_init(sample_time):
     kalman.P = noise_state_cov
     return kalman
 
-
 mqtt = Client(MQTT_CLIENT_ID)
 mqtt.username_pw_set("mosquitto", os.environ["SAILTRACK_GLOBAL_PASSWORD"])
 mqtt.message_callback_add("sensor/gps0", on_message_callback_gps)
@@ -184,18 +188,17 @@ tl = Timeloop()
 def boat_kalman_job():
 
     gps_rel_pos = [
-        (gps_data["lon"] - gps_ref["lon"]) * EARTH_CIRCUMFERENCE_METERS * LAT_FACTOR / 360,
         (gps_data["lat"] - gps_ref["lat"]) * EARTH_CIRCUMFERENCE_METERS / 360,
+        (gps_data["lon"] - gps_ref["lon"]) * EARTH_CIRCUMFERENCE_METERS * LAT_FACTOR / 360,
         gps_data["hMSL"] - gps_ref["hMSL"]
     ]
     gps_ned_velocity = [
         gps_data["velN"],
         gps_data["velE"],
-        gps_data["velD"],
+        -gps_data["velD"],
     ]
-    r_gain = 2500000
-    kalman_boat.R = r_gain*np.diag([gps_data["hAcc"], gps_data["hAcc"], gps_data["vAcc"],
-                                    gps_data["sAcc"], gps_data["sAcc"], gps_data["sAcc"]])
+    kalman_boat.R = np.diag([gps_data["hAcc"], gps_data["hAcc"], gps_data["vAcc"],
+                             gps_data["sAcc"], gps_data["sAcc"], gps_data["sAcc"]])
 
     measure = np.hstack((gps_rel_pos, gps_ned_velocity)).reshape(6, 1)
     kalman_boat.update(measure)
@@ -213,43 +216,48 @@ def wind_kalman_job():
     kalman_wind.predict()
 
 
+
 @tl.job(interval=timedelta(milliseconds=MQTT_JOB_INTERVAL_MS))
 def mqtt_job():
     pos_x = kalman_boat.x[0, 0]
     pos_y = kalman_boat.x[1, 0]
     pos_z = kalman_boat.x[2, 0]
-    speed_x = kalman_boat.x[3, 0]
-    speed_y = kalman_boat.x[4, 0]
-    speed_z = kalman_boat.x[5, 0]
-    sog = math.sqrt(speed_x ** 2 + speed_y ** 2) * MPS_TO_KNOTS_MULTIPLIER
-    cog = math.degrees(np.arctan2(pos_y, pos_x))
+    neu_speed = kalman_boat.x[3:6].reshape(3, 1)
+    sog = math.sqrt(neu_speed[0] ** 2 + neu_speed[1] ** 2) * MPS_TO_KNOTS_MULTIPLIER
+    cog = math.degrees(np.arctan2(neu_speed[1], neu_speed[0]))
     lon = pos_x * 360 / EARTH_CIRCUMFERENCE_METERS / LAT_FACTOR + gps_ref["lat"]
     lat = pos_y * 360 / EARTH_CIRCUMFERENCE_METERS + gps_ref["lon"]
     altitude = pos_z + gps_ref["hMSL"]
-    global_xy_speed = np.array([speed_x, speed_y, 0]).reshape(3, 1)
-    boat_xy_speed = body2world_mtx.T @ global_xy_speed
+    boat_xy_speed = world2body_mtx @ neu_speed
     twx = kalman_wind.x[0]-boat_xy_speed[0]
     twy = kalman_wind.x[1]-boat_xy_speed[1]
-    tw_xy = np.array([twx, twy]).reshape(2, 1)
-    tw_sailor = body2sailor_mtx @ tw_xy
-    tws = math.sqrt(twx**2+twy**2)
-    twa = math.degrees(math.atan2(tw_sailor[0], tw_sailor[1]))
+    tw_xy = np.array([twx.item(), twy.item(), 0],).reshape(3, 1)
+    twx = tw_xy[0]
+    twy = tw_xy[1]
+    tws = math.sqrt(twx**2+twy**2)*MPS_TO_KNOTS_MULTIPLIER
+    twa = math.degrees(math.atan2(twy, tws))
     twd = np.mod(orientation["heading"] + twa, 360)
+    twd = twd if twd > 0 else 360-twd
     mqtt.publish("boat", json.dumps({
         "lon": lon,
         "lat": lat,
-        "cog": angle_wrap_180(cog),                                  # toDo convert from 180/-180 to 0/360
+        "cog": cog,
         "sog": sog,
         "altitude": altitude,
-        "ascensionSpeed": speed_z,
+        "ascensionSpeed": neu_speed[2].item(),
         "heading": orientation["heading"],
         "pitch": angle_wrap_180(orientation["pitch"]),
-        "roll": angle_wrap_180(orientation["roll"])
+        "roll": angle_wrap_180(orientation["roll"]),
+        "drift": abs(angle_wrap_180(orientation["heading"])-angle_wrap_180(cog))
     }))
+    # UNCOMMENT TO SEND WIND DATA
+    """
     mqtt.publish("wind", json.dumps({
         "tws": tws,
-        "twd": angle_wrap_180(twd),
+        "twd": twd,
+        "twa": angle_wrap_180(twa)
     }))
+    """
 
 
 formatter = logging.Formatter("[%(levelname)s] %(message)s")