From f0244d05188f721fcf3843a41c862e0630c7ef85 Mon Sep 17 00:00:00 2001 From: zbr Date: Fri, 15 Oct 2021 16:27:39 +0800 Subject: [PATCH] backup --- APMrover2/Rover.cpp | 30 ++++--- APMrover2/system.cpp | 2 +- APMrover2/version.h | 3 +- libraries/AP_Proximity/AP_Proximity.cpp | 27 +++++- libraries/AP_Proximity/AP_Proximity.h | 3 + .../AP_Proximity/AP_Proximity_UAVCAN.cpp | 84 ++++++++++++++++--- libraries/AP_Proximity/AP_Proximity_UAVCAN.h | 7 ++ zr-rover.sh | 2 +- 8 files changed, 134 insertions(+), 24 deletions(-) diff --git a/APMrover2/Rover.cpp b/APMrover2/Rover.cpp index 7348d42305..3026b19791 100644 --- a/APMrover2/Rover.cpp +++ b/APMrover2/Rover.cpp @@ -301,23 +301,33 @@ void Rover::one_second_loop(void) // need to set "likely flying" when armed to allow for compass // learning to run - ahrs.set_likely_flying(hal.util->get_soft_armed()); + ahrs.set_likely_flying(hal.util->get_soft_armed()); // send latest param values to wp_nav g2.wp_nav.set_turn_params(g.turn_max_g, g2.turn_radius, g2.motors.have_skid_steering()); // gcs().send_text(MAV_SEVERITY_CRITICAL, "test"); - + // rover.g2.proximity.re_init(); + static uint32_t prx_init_time = AP_HAL::millis(); + static bool init_prx = false; + if (init_prx == false ) { + uint32_t tnow = AP_HAL::millis(); + if(tnow - prx_init_time > 30000){ + init_prx = true; + init_proximity(); + gcs().send_text(MAV_SEVERITY_INFO, "--- init prx:%dms -> %dms ---",prx_init_time,tnow); + } + } #if DUAL_ANT_HEADING - static uint8_t prt_cnt; - prt_cnt++; - if(prt_cnt > 5 && gps.gps_heading_enable()){ - prt_cnt = 0; - float heading = gps.gps_heading()*0.01f; - float gps_heading_rad = ToRad(heading); - gcs().send_text(MAV_SEVERITY_INFO, "[GNSS]:heading:%3.2f , heading_rad:%3.2f ", heading, gps_heading_rad); - } + // static uint8_t prt_cnt; + // prt_cnt++; + // if(prt_cnt > 5 && gps.gps_heading_enable()){ + // prt_cnt = 0; + // float heading = gps.gps_heading()*0.01f; + // float gps_heading_rad = ToRad(heading); + // gcs().send_text(MAV_SEVERITY_INFO, "[GNSS]:heading:%3.2f , heading_rad:%3.2f ", heading, gps_heading_rad); + // } #endif #if DUAL_ANT_HEADING diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp index 4225a97f67..7450758420 100644 --- a/APMrover2/system.cpp +++ b/APMrover2/system.cpp @@ -99,7 +99,7 @@ void Rover::init_ardupilot() rangefinder.init(ROTATION_NONE); // init proximity sensor - init_proximity(); + // init_proximity(); // init beacons used for non-gps position estimation init_beacon(); diff --git a/APMrover2/version.h b/APMrover2/version.h index f1c321884f..0bf178f88a 100644 --- a/APMrover2/version.h +++ b/APMrover2/version.h @@ -6,7 +6,7 @@ #include "ap_version.h" -#define THISFIRMWARE "ZR_Boat V4.0.3" +#define THISFIRMWARE "ZR_Boat V4.0.3 RC2" // the following line is parsed by the autotest scripts #define FIRMWARE_VERSION 4,0,3,FIRMWARE_VERSION_TYPE_DEV @@ -20,5 +20,6 @@ * @brief 更新说明 * * 4.0.3 去除声呐串口读取，使用uavcan，增加8、9通道控制relay 1、2 + * 4.0.3 rc2 uavcan避障调整 * */ diff --git a/libraries/AP_Proximity/AP_Proximity.cpp b/libraries/AP_Proximity/AP_Proximity.cpp index cf57332dfd..04777c211c 100644 --- a/libraries/AP_Proximity/AP_Proximity.cpp +++ b/libraries/AP_Proximity/AP_Proximity.cpp @@ -215,13 +215,15 @@ void AP_Proximity::init(void) // update Proximity state for all instances. This should be called at a high rate by the main loop void AP_Proximity::update(void) { + uint8_t cnt = 0; for (uint8_t i=0; iupdate(); } - + prx_init_cnt = cnt; // work out primary instance - first sensor returning good data for (int8_t i=num_instances-1; i>=0; i--) { if (drivers[i] != nullptr && (state[i].status == Status::Good)) { @@ -230,6 +232,29 @@ void AP_Proximity::update(void) } } +#include +bool AP_Proximity::re_init() +{ + bool ret = (prx_init_cnt != 0); + static uint32_t last_5s; + uint32_t tnow = AP_HAL::millis(); + + if(!ret){ + if (tnow - last_5s > 5000) { + last_5s = tnow; + init(); + gcs().send_text(MAV_SEVERITY_NOTICE, "init [%d] prx: %ld",num_instances,tnow); + } + + } + if (tnow - last_5s > 5000) { + last_5s = tnow; + // init(); + gcs().send_text(MAV_SEVERITY_NOTICE, " prx[%d] : %ld, %d",num_instances,tnow,ret); + } + return ret; +} + // return sensor orientation uint8_t AP_Proximity::get_orientation(uint8_t instance) const { diff --git a/libraries/AP_Proximity/AP_Proximity.h b/libraries/AP_Proximity/AP_Proximity.h index 5c80bf75a3..d9c735bc69 100644 --- a/libraries/AP_Proximity/AP_Proximity.h +++ b/libraries/AP_Proximity/AP_Proximity.h @@ -136,6 +136,7 @@ public: bool sensor_enabled() const; bool sensor_failed() const; + bool re_init(); private: static AP_Proximity *_singleton; Proximity_State state[PROXIMITY_MAX_INSTANCES]; @@ -158,6 +159,8 @@ private: AP_Int8 _ignore_width_deg[PROXIMITY_MAX_IGNORE]; // width of beam (in degrees) that should be ignored void detect_instance(uint8_t instance); + // bool prx_init; + uint8_t prx_init_cnt; }; namespace AP { diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp index c58f22e428..cba79c0620 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -8,6 +8,13 @@ #include #include +#define PRX_DEBUG 1 +#if PRX_DEBUG +#include + # define Prx_Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "prx-%d: " fmt "", __LINE__, ## args) +#else + # define Prx_Debug(fmt, args ...) +#endif extern const AP_HAL::HAL &hal; @@ -16,18 +23,24 @@ extern const AP_HAL::HAL &hal; { \ if ((level_debug) <= AP::can().get_debug_level_driver(can_driver)) \ { \ - printf(fmt, ##args); \ + hal.console->printf(fmt, ##args); \ } \ } while (0) HAL_Semaphore AP_Proximity_UAVCAN::_sem_registry; UC_REGISTRY_BINDER(ProximityCb, zrzk::equipment::range_sensor::Proximity); + +uint8_t AP_Proximity_UAVCAN::_node_id = 0; +AP_Proximity_UAVCAN* AP_Proximity_UAVCAN::_driver = nullptr; +AP_UAVCAN* AP_Proximity_UAVCAN::_ap_uavcan = nullptr; + AP_Proximity_UAVCAN::DetectedModules AP_Proximity_UAVCAN::_detected_modules[] = {0}; AP_Proximity_UAVCAN::AP_Proximity_UAVCAN(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) : AP_Proximity_Backend(_frontend, _state) { + _driver = this; } void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) @@ -56,7 +69,16 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, WITH_SEMAPHORE(_sem_registry); AP_Proximity_UAVCAN *backend = nullptr; - + Prx_Debug("---------- AP_Proximity_UAVCAN ----------"); + backend = new AP_Proximity_UAVCAN(_frontend, _state); + if (backend == nullptr) + { + Prx_Debug("backend:%d",(int)backend); + }else{ + + Prx_Debug("faild,backend"); + } + /* for (uint8_t i = 0; i < PROXIMITY_MAX_INSTANCES; i++) { if (_detected_modules[i].driver == nullptr && _detected_modules[i].ap_uavcan != nullptr) @@ -64,6 +86,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, backend = new AP_Proximity_UAVCAN(_frontend, _state); if (backend == nullptr) { + Prx_Debug("------index:%d, Failed register Node %d on Bus %d\n", + _detected_modules[i].ap_uavcan->get_driver_index(), + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + debug_proximity_uavcan(2, _detected_modules[i].ap_uavcan->get_driver_index(), "Failed register UAVCAN Proximity Node %d on Bus %d\n", @@ -72,6 +99,11 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, } else { + Prx_Debug("-----index:%d, Registered Node %d on Bus %d\n", + _detected_modules[i].ap_uavcan->get_driver_index(), + _detected_modules[i].node_id, + _detected_modules[i].ap_uavcan->get_driver_index()); + _detected_modules[i].driver = backend; debug_proximity_uavcan(2, _detected_modules[i].ap_uavcan->get_driver_index(), @@ -82,6 +114,8 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, break; } } + */ + Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n"); return backend; } @@ -166,14 +200,37 @@ void AP_Proximity_UAVCAN::update(void) void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, uint8_t node_id, const ProximityCb &cb) { - WITH_SEMAPHORE(_sem_registry); - + + if (_driver == nullptr) { + return; + } + + if (_ap_uavcan == nullptr) { + _ap_uavcan = ap_uavcan; + _node_id = node_id; + Prx_Debug("---- uavcan: ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); + } + + if (_ap_uavcan == ap_uavcan && _node_id == node_id) { + WITH_SEMAPHORE(_sem_registry); + _driver->_interim_data.d1_cm = cb.msg->d0; + _driver->_interim_data.d2_cm = cb.msg->d45; + _driver->_interim_data.d3_cm = cb.msg->d90; + _driver->_interim_data.d4_cm = cb.msg->d135; + _driver->_interim_data.d5_cm = cb.msg->d180; + _driver->_interim_data.d6_cm = cb.msg->d225; + _driver->_interim_data.d7_cm = cb.msg->d270; + _driver->_interim_data.d8_cm = cb.msg->d315; + _driver->_last_distance_received_ms = AP_HAL::millis(); + } + /* AP_Proximity_UAVCAN *driver = get_uavcan_backend(ap_uavcan,node_id); if (driver == nullptr) { return; } driver->handle_proximity_data(cb); + */ } void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb) @@ -207,13 +264,20 @@ void AP_Proximity_UAVCAN::handle_proximity_data(const ProximityCb &cb) void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distance_cm) { + + const uint8_t sector = convert_angle_to_sector(angle_deg); - _angle[sector] = angle_deg; - _distance[sector] = ((float)distance_cm) / 1000; - _distance_valid[sector] = distance_cm != 0xffff; - //_last_distance_received_ms = AP_HAL::millis(); - // update boundary used for avoidance - update_boundary_for_sector(sector, true); + // if(convert_angle_to_sector(angle_deg,sector)){ + _angle[sector] = angle_deg; + _distance[sector] = ((float)distance_cm) / 1000; + // _distance_valid[sector] = distance_cm != 0xffff; + //_last_distance_received_ms = AP_HAL::millis(); + // update boundary used for avoidance + + _distance_valid[sector] = (_distance[sector] >= distance_min()) && (_distance[sector] <= distance_max()); + + update_boundary_for_sector(sector, true); + // } } #endif diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h index 8918eafbe5..d81f99b069 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.h +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.h @@ -30,6 +30,8 @@ public: private: static AP_Proximity_UAVCAN *get_uavcan_backend(AP_UAVCAN *ap_uavcan,uint8_t node_id); + + static AP_Proximity_UAVCAN* _driver; // inline void register_sensor() // { // // instance = frontend.register_sensor(); @@ -61,5 +63,10 @@ private: static HAL_Semaphore _sem_registry; HAL_Semaphore _sem_proximity; + + static AP_UAVCAN* _ap_uavcan; + static uint8_t _node_id; + + }; #endif //HAL_WITH_UAVCAN \ No newline at end of file diff --git a/zr-rover.sh b/zr-rover.sh index 139e847fdb..e7000e1c63 100755 --- a/zr-rover.sh +++ b/zr-rover.sh @@ -1,3 +1,3 @@ ./waf configure --board zr-v4 ./waf rover -cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrv4-rover.px4 +cp build/zr-v4/bin/ardurover.apj /mnt/f/_01-work/100=data/固件/zr-v4/wsl/zrrover-4.0.3-rc2.px4