Addition of NED frame

instlscrip.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+
+VER=15:10.3-2021.10-9
+URL=https://developer.arm.com/-/media/Files/downloads/gnu-rm/10.3-2021.10/gcc-arm-none-eabi-10.3-2021.10-x86_64-linux.tar.bz2
+echo "Creating gcc-arm-none-eabi debian package version $VER"
+
+echo "Entering temporary directory..."
+cd /tmp
+
+echo "Downloading..."
+curl -fSL -A "Mozilla/4.0" -o gcc-arm-none-eabi.tar "$URL"
+
+echo "Extracting..."
+tar -xf gcc-arm-none-eabi.tar
+rm gcc-arm-none-eabi.tar
+
+echo "Generating debian package..."
+mkdir gcc-arm-none-eabi
+mkdir gcc-arm-none-eabi/DEBIAN
+mkdir gcc-arm-none-eabi/usr
+echo "Package: gcc-arm-none-eabi"          >  gcc-arm-none-eabi/DEBIAN/control
+echo "Version: $VER"                       >> gcc-arm-none-eabi/DEBIAN/control
+echo "Architecture: amd64"                 >> gcc-arm-none-eabi/DEBIAN/control
+echo "Maintainer: maintainer"              >> gcc-arm-none-eabi/DEBIAN/control
+echo "Description: Arm Embedded toolchain" >> gcc-arm-none-eabi/DEBIAN/control
+mv gcc-arm-none-eabi-*/* gcc-arm-none-eabi/usr/
+dpkg-deb --build --root-owner-group gcc-arm-none-eabi
+
+echo "Installing..."
+sudo apt install ./gcc-arm-none-eabi.deb -y --allow-downgrades
+
+echo "Removing temporary files..."
+rm -r gcc-arm-none-eabi*
+
+echo "Done."

libraries/AC_PrecLand/AC_PrecLand.cpp

@@ -650,8 +650,20 @@ bool AC_PrecLand::construct_pos_meas_using_rangefinder(float rangefinder_alt_m,
         const struct inertial_data_frame_s *inertial_data_delayed = (*_inertial_history)[0];
 
         const bool target_vec_valid = target_vec_unit_body.projected(_approach_vector_body).dot(_approach_vector_body) > 0.0f;
-        const Vector3f target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body;
-        const Vector3f approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body;
+
+        // Check for FRD or NED frame of reference
+        Vector3f target_vec_unit_ned;
+        Vector3f approach_vector_NED;
+
+        if (_backend->get_frame() == MAV_FRAME_BODY_FRD) {
+            target_vec_unit_ned = inertial_data_delayed->Tbn * target_vec_unit_body;
+            approach_vector_NED = inertial_data_delayed->Tbn * _approach_vector_body;
+        }
+        else if (_backend->get_frame() == MAV_FRAME_LOCAL_NED) {
+            target_vec_unit_ned = target_vec_unit_body;
+            approach_vector_NED = _approach_vector_body;
+        }
+
         const bool alt_valid = (rangefinder_alt_valid && rangefinder_alt_m > 0.0f) || (_backend->distance_to_target() > 0.0f);
         if (target_vec_valid && alt_valid) {
             // distance to target and distance to target along approach vector

libraries/AC_PrecLand/AC_PrecLand_Backend.h

@@ -33,6 +33,9 @@ class AC_PrecLand_Backend
     // returns system time in milliseconds of last los measurement
     virtual uint32_t los_meas_time_ms() = 0;
 
+    // return mav frame, FRD by default
+    virtual uint8_t get_frame() { return MAV_FRAME_BODY_FRD; };
+
     // return true if there is a valid los measurement available
     virtual bool have_los_meas() = 0;
 

libraries/AC_PrecLand/AC_PrecLand_Companion.cpp

@@ -30,6 +30,12 @@ bool AC_PrecLand_Companion::get_los_body(Vector3f& ret) {
     return false;
 }
 
+// return true if there is a valid los measurement available
+uint8_t AC_PrecLand_Companion::get_frame()
+{
+    return _frame;
+}
+
 // returns system time in milliseconds of last los measurement
 uint32_t AC_PrecLand_Companion::los_meas_time_ms() {
     return _los_meas_time_ms;
@@ -52,7 +58,9 @@ void AC_PrecLand_Companion::handle_msg(const mavlink_landing_target_t &packet, u
     _distance_to_target = packet.distance;
 
     if (packet.position_valid == 1) {
-        if (packet.frame == MAV_FRAME_BODY_FRD) {
+        _frame = packet.frame;
+        if (    (_frame == MAV_FRAME_BODY_FRD)
+             || (_frame == MAV_FRAME_LOCAL_NED)) {
             if (_distance_to_target > 0) {
                 _los_meas_body = Vector3f(packet.x, packet.y, packet.z);
                 _los_meas_body /= _distance_to_target;

libraries/AC_PrecLand/AC_PrecLand_Companion.h

@@ -30,6 +30,9 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend
     //  returns same as have_los_meas()
     bool get_los_body(Vector3f& ret) override;
 
+    // returns mavlink frame
+    uint8_t get_frame() override;
+
     // returns system time in milliseconds of last los measurement
     uint32_t los_meas_time_ms() override;
 
@@ -42,13 +45,15 @@ class AC_PrecLand_Companion : public AC_PrecLand_Backend
     // parses a mavlink message from the companion computer
     void handle_msg(const mavlink_landing_target_t &packet, uint32_t timestamp_ms) override;
 
+
 private:
     float               _distance_to_target;    // distance from the camera to target in meters
 
     Vector3f            _los_meas_body;         // unit vector in body frame pointing towards target
     bool                _have_los_meas;         // true if there is a valid measurement from the camera
     uint32_t            _los_meas_time_ms;      // system time in milliseconds when los was measured
     bool                _wrong_frame_msg_sent;
+    uint8_t             _frame;             // frame parameter (FRD or NED)
 };