diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 697f2e42f0..a7807e47c7 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -38,6 +38,13 @@ #include #include + +#if HAL_WITH_UAVCAN +#include +#include +#include +#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); + } } } diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.cpp b/libraries/AP_UAVCAN/AP_UAVCAN.cpp index 10b5ebd90c..7291f1aa56 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.cpp +++ b/libraries/AP_UAVCAN/AP_UAVCAN.cpp @@ -58,6 +58,8 @@ #include #include +#include + #include #include @@ -144,7 +146,7 @@ static uavcan::Publisher* zr_uav_state[MAX_NUMBER_O static uavcan::Publisher* zr_uav_timestamp[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher* zr_uav_euler[MAX_NUMBER_OF_CAN_DRIVERS]; static uavcan::Publisher * zr_uav_pos[MAX_NUMBER_OF_CAN_DRIVERS]; - +static uavcan::Publisher* 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(*_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(*_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 diff --git a/libraries/AP_UAVCAN/AP_UAVCAN.h b/libraries/AP_UAVCAN/AP_UAVCAN.h index aca872952b..cdff8c6f80 100644 --- a/libraries/AP_UAVCAN/AP_UAVCAN.h +++ b/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 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 _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; diff --git a/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan b/libraries/AP_UAVCAN/dsdl/zrzk/equipment/uav/26135.PPKPOS.uavcan new file mode 100644 index 0000000000..d781bd4f85 --- /dev/null +++ b/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 \ No newline at end of file