添加ppkpos到uavcan

libraries/AP_GPS/AP_GPS_NMEA.cpp

@@ -38,6 +38,13 @@
 
 #include <AP_Logger/AP_Logger.h>
 #include <GCS_MAVLink/GCS.h>
+
+#if HAL_WITH_UAVCAN
+#include <AP_BoardConfig/AP_BoardConfig_CAN.h>
+#include <AP_UAVCAN/AP_UAVCAN.h>
+#include <AP_Camera/AP_Camera.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 // optionally log all NMEA data for debug purposes
@@ -282,25 +289,49 @@ bool AP_GPS_NMEA::_term_complete()
                 switch (_sentence_type) {
                 case _GPS_SENTENCE_MARK:
                 case _GPS_SENTENCE_EVENT:
-                    if(get_new_mark){                        
-                        gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n",gps_mark.week,gps_mark.second,gps_mark.lat,gps_mark.lon,gps_mark.hgt);
+                    if (get_new_mark)
+                    {
+                        gcs().send_text(MAV_SEVERITY_INFO, "%d, %.4f, %f , %f , %f\n", gps_mark.week, gps_mark.second, gps_mark.lat, gps_mark.lon, gps_mark.hgt);
                         get_new_mark = false;
-                            AP_Logger *logger = AP_Logger::get_singleton();
 
-                            if ( logger!=nullptr )
+#if HAL_WITH_UAVCAN
+                        uint8_t can_num_drivers = AP::can().get_num_drivers();
+                        for (uint8_t i = 0; i < can_num_drivers; i++)
+                        {
+                            AP_UAVCAN *uavcan = AP_UAVCAN::get_uavcan(i);
+                            if (uavcan != nullptr)
                             {
-                                Vector3d  llh = Vector3d(gps_mark.lat,gps_mark.lon, gps_mark.hgt);
-                                Vector3f  sigma = Vector3f(gps_mark.lat_sigma,gps_mark.lon_sigma, gps_mark.hgt_sigma);
-                                    AP::logger().Write_RTK_Mark_Pos(
-                                    gps_mark.week,
-                                    gps_mark.second,
-                                    llh,
-                                    gps_mark.undulation,
-                                    sigma,
-                                    gps_mark.diff_age,
-                                    gps_mark.sol_age
-                                    );
+                                ppk_pos_data_t ppk_pos;
+                                memset(&ppk_pos,0,sizeof(ppk_pos_data_t));
+                                ppk_pos.week = gps_mark.week;
+                                ppk_pos.weekms = gps_mark.second;
+                                ppk_pos.latitude_deg_1e7 = (int32_t)(gps_mark.lat * (int32_t)10000000UL);
+                                ppk_pos.longitude_deg_1e7 = (int32_t)(gps_mark.lon * (int32_t)10000000UL);
+                                ppk_pos.height_abs_cm = gps_mark.hgt * 100;
+                                AP_Camera *camera = AP::camera();
+                                if (camera != nullptr)
+                                {
+                                   ppk_pos.index =camera->get_image_index();
+                                }
+                                uavcan->set_zr_ppk_pos(ppk_pos);
                             }
+                        }
+#endif
+                        AP_Logger *logger = AP_Logger::get_singleton();
+
+                        if (logger != nullptr)
+                        {
+                            Vector3d llh = Vector3d(gps_mark.lat, gps_mark.lon, gps_mark.hgt);
+                            Vector3f sigma = Vector3f(gps_mark.lat_sigma, gps_mark.lon_sigma, gps_mark.hgt_sigma);
+                            AP::logger().Write_RTK_Mark_Pos(
+                                gps_mark.week,
+                                gps_mark.second,
+                                llh,
+                                gps_mark.undulation,
+                                sigma,
+                                gps_mark.diff_age,
+                                gps_mark.sol_age);
+                        }
                     }
                 }
                 

libraries/AP_UAVCAN/AP_UAVCAN.cpp

@@ -58,6 +58,8 @@
 
 #include <zrzk/protocol/Message.hpp>
 #include <zrzk/protocol/Request.hpp>
+#include <zrzk/equipment/uav/PPKPOS.hpp>
+
 #include <uavcan/equipment/camera_gimbal/GEOPOICommand.hpp>
 #include <uavcan/equipment/camera_gimbal/AngularCommand.hpp>
 
@@ -144,7 +146,7 @@ static uavcan::Publisher<zrzk::equipment::uav::State>* zr_uav_state[MAX_NUMBER_O
 static uavcan::Publisher<zrzk::equipment::uav::TimeStamp>* zr_uav_timestamp[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<zrzk::equipment::uav::Euler>* zr_uav_euler[MAX_NUMBER_OF_CAN_DRIVERS];
 static uavcan::Publisher<zrzk::equipment::uav::Position> * zr_uav_pos[MAX_NUMBER_OF_CAN_DRIVERS];
-
+static uavcan::Publisher<zrzk::equipment::uav::PPKPOS>* zr_uav_ppk[MAX_NUMBER_OF_CAN_DRIVERS]; 
 // subscribers
 
 // handler SafteyButton
@@ -359,6 +361,10 @@ void AP_UAVCAN::init(uint8_t driver_index, bool enable_filters)
     zr_uav_pos[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(100));
     zr_uav_pos[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
 
+    zr_uav_ppk[driver_index] = new uavcan::Publisher<zrzk::equipment::uav::PPKPOS>(*_node);
+    zr_uav_ppk[driver_index]->setTxTimeout(uavcan::MonotonicDuration::fromMSec(100));
+    zr_uav_ppk[driver_index]->setPriority(uavcan::TransferPriority::OneHigherThanLowest);
+
     safety_button_listener[driver_index] = new uavcan::Subscriber<ardupilot::indication::Button, ButtonCb>(*_node);
     if (safety_button_listener[driver_index]) {
         safety_button_listener[driver_index]->start(ButtonCb(this, &handle_button));
@@ -487,6 +493,7 @@ void AP_UAVCAN::loop(void)
         zr_uav_version_send();
         zr_uav_pos_send();
         zr_uav_euler_send();
+        zr_uav_ppk_send();
         AP::uavcan_server().verify_nodes(this);
     }
 }
@@ -1235,4 +1242,29 @@ void AP_UAVCAN::handle_zr_camera_sum(AP_UAVCAN *ap_uavcan, uint8_t node_id, cons
     camera->update_zr_cam_sum(pkt);
 }
 
+void AP_UAVCAN::set_zr_ppk_pos(ppk_pos_data_t &data)
+{
+    WITH_SEMAPHORE(_zr_ppk_pos.sem);
+    memcpy(&_zr_ppk_pos.data, &data, sizeof(ppk_pos_data_t));
+    _zr_ppk_pos.need_send = true;
+}
+
+void AP_UAVCAN::zr_uav_ppk_send()
+{
+    if (!_zr_ppk_pos.need_send)
+        return;
+
+    zrzk::equipment::uav::PPKPOS msg;
+    WITH_SEMAPHORE(_zr_ppk_pos.sem);
+    msg.index = _zr_ppk_pos.data.index;
+    msg.week = _zr_ppk_pos.data.week;
+    msg.weekms = _zr_ppk_pos.data.weekms;
+    msg.latitude_deg_1e7 = _zr_ppk_pos.data.latitude_deg_1e7;
+    msg.longitude_deg_1e7 = _zr_ppk_pos.data.longitude_deg_1e7;
+    msg.height_abs_cm = _zr_ppk_pos.data.height_abs_cm;
+    msg.cam_number = _zr_ppk_pos.data.cam_number;
+    zr_uav_ppk[_driver_index]->broadcast(msg);
+    _zr_ppk_pos.need_send = false;
+}
+
 #endif // HAL_WITH_UAVCAN

libraries/AP_UAVCAN/AP_UAVCAN.h

@@ -65,6 +65,17 @@ typedef struct
     uint8_t wp_number;
     bool armd; // bit len 1
 } zr_uav_state_data_t;
+
+typedef struct
+{
+    uint32_t week;              // bit len 32
+    uint32_t weekms;            // bit len 32
+    uint32_t latitude_deg_1e7;  // bit len 32
+    uint32_t longitude_deg_1e7; // bit len 32
+    uint32_t height_abs_cm;     // bit len 24
+    uint16_t index;             // bit len 16
+    uint8_t cam_number;
+} ppk_pos_data_t;
 /*
     Frontend Backend-Registry Binder: Whenever a message of said DataType_ from new node is received,
     the Callback will invoke registery to register the node as separate backend.
@@ -94,6 +105,8 @@ public:
         uint8_t hardware_patch; // bit len 8
     } zr_uav_version_data_t;
 
+  
+
    typedef enum
     {
         ZR_UAVCAN_REQUEST_TIMESTAMP=1,
@@ -131,6 +144,7 @@ public:
     void set_zr_timestamp(uint8_t id,uint64_t timestamp);
     void set_zr_uav_state(zr_uav_state_data_t &data);
     void set_zr_uav_version(zr_uav_version_data_t &data);
+    void set_zr_ppk_pos(ppk_pos_data_t &data);
     template <typename DataType_>
     class RegistryBinder {
     protected:
@@ -215,6 +229,7 @@ private:
     void  zr_uav_version_send();
     void  zr_uav_pos_send();
     void  zr_uav_euler_send();
+    void  zr_uav_ppk_send();
     uavcan::PoolAllocator<UAVCAN_NODE_POOL_SIZE, UAVCAN_NODE_POOL_BLOCK_SIZE, AP_UAVCAN::RaiiSynchronizer> _node_allocator;
 
     // UAVCAN parameters
@@ -238,6 +253,7 @@ private:
     uint8_t _SRV_armed;
     uint32_t _SRV_last_send_us;
     uint32_t _pos_last_send_ms;
+    uint32_t _ppk_last_send_ms;
     HAL_Semaphore SRV_sem;
     HAL_Semaphore uav_pos_sem;
     ///// LED /////
@@ -345,9 +361,15 @@ private:
         uint8_t param2;  // bit len 8
         uint8_t param3;  // bit len 8
         uint8_t param4;  // bit len 8
-
     } zr_request_t;
 
+    struct
+    {
+        HAL_Semaphore sem;
+        ppk_pos_data_t data;
+        bool need_send;
+    } _zr_ppk_pos;
+
     // safety status send state
     uint32_t _last_safety_state_ms;
     uint32_t _last_uav_state_ms;

libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan

@@ -0,0 +1,7 @@
+uint8 cam_number
+uint16 index
+uint16 week
+uint32 weekms
+int32 latitude_deg_1e7
+int32 longitude_deg_1e7
+int24 height_abs_cm