diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index b887e7df27..dbb8eaa39b 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -584,7 +584,7 @@ void GCS_MAVLINK_Copter::send_banner() send_text(MAV_SEVERITY_INFO, "%s", frame_and_type_string); static char buf[50]; - snprintf(buf, sizeof(buf), "Version: zr-v%u.%u.%u ,ID: %lu",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,copter.zr_app.get_zr_sysid()); + snprintf(buf, sizeof(buf), "Version: zr-v%u.%u.%u ,ID: %08lu",AP::fwversion().major,AP::fwversion().minor,AP::fwversion().patch,copter.zr_app.get_zr_sysid()); uint32_t deadline = 0; copter.zr_app.get_deadline_params(deadline); diff --git a/ArduCopter/UserCode.cpp b/ArduCopter/UserCode.cpp index 23e07c6c39..b1c83fb367 100644 --- a/ArduCopter/UserCode.cpp +++ b/ArduCopter/UserCode.cpp @@ -42,6 +42,11 @@ void Copter::userhook_SlowLoop() void Copter::userhook_SuperSlowLoop() { // put your 1Hz code here + if(motors->armed()){ // 电子开关锁 + relay.on(3); + }else{ + relay.off(3); + } } #endif diff --git a/ArduCopter/version.h b/ArduCopter/version.h index 7811cbb82a..bb8831ac43 100644 --- a/ArduCopter/version.h +++ b/ArduCopter/version.h @@ -6,14 +6,16 @@ #include "ap_version.h" -#define THISFIRMWARE "ArduCopter V4.1.5" - +#define THISFIRMWARE "ZRUAV V5.0.0-dev" +#define FIRMWARE_TYPE 1 // the following line is parsed by the autotest scripts -#define FIRMWARE_VERSION 4,1,5,FIRMWARE_VERSION_TYPE_OFFICIAL +// #define FIRMWARE_VERSION 4,1,5,FIRMWARE_VERSION_TYPE_OFFICIAL +#define FIRMWARE_VERSION 5,0,0,FIRMWARE_TYPE -#define FW_MAJOR 4 -#define FW_MINOR 1 -#define FW_PATCH 5 -#define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL +#define FW_MAJOR 5 +#define FW_MINOR 0 +#define FW_PATCH 0 +// #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL +#define FW_TYPE FIRMWARE_TYPE #include \ No newline at end of file diff --git a/ArduCopter/zr_app.cpp b/ArduCopter/zr_app.cpp index fe8fc10633..438d9b7fd8 100644 --- a/ArduCopter/zr_app.cpp +++ b/ArduCopter/zr_app.cpp @@ -25,7 +25,7 @@ void Copter::zr_app_50hz(){ gcs().send_text(MAV_SEVERITY_INFO, "data_trans init"); } } - + if(zr_serial_api.data_trans_init){ Vector3l pos_neu_cm; @@ -139,4 +139,6 @@ void Copter::zr_app_50hz(){ } } -} \ No newline at end of file +} + + diff --git a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp index 33ffdbf1ef..ea9f6979f5 100644 --- a/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp +++ b/libraries/AP_Proximity/AP_Proximity_UAVCAN.cpp @@ -50,8 +50,7 @@ void AP_Proximity_UAVCAN::subscribe_msgs(AP_UAVCAN *ap_uavcan) Prx_Debug("ap_uavcan:nullptr" ); return; } - - Prx_Debug("ap_uavcan:subscribe_msgs"); + Prx_Debug("ap_uavcan:subscribe_msgs"); auto *node = ap_uavcan->get_node(); uavcan::Subscriber *proximity_listener; @@ -77,10 +76,8 @@ AP_Proximity_Backend *AP_Proximity_UAVCAN::probe(AP_Proximity &_frontend, { Prx_Debug("backend:%d",(int)backend); }else{ - Prx_Debug("faild,backend"); } - Prx_Debug("---------- AP_Proximity_UAVCAN ----------\n"); return backend; } @@ -137,33 +134,23 @@ float AP_Proximity_UAVCAN::distance_max() const float AP_Proximity_UAVCAN::distance_min() const { - return 0.20f; + return 1.00f; } void AP_Proximity_UAVCAN::update(void) { WITH_SEMAPHORE(_sem_proximity); - // update_sector_data(0, _interim_data.d1_cm); // d1 - // update_sector_data(45, _interim_data.d2_cm); // d8 - // update_sector_data(90, _interim_data.d3_cm); // d7 - // update_sector_data(135, _interim_data.d4_cm); // d6 - // update_sector_data(180, _interim_data.d5_cm); // d5 - // update_sector_data(225, _interim_data.d6_cm); // d4 - // update_sector_data(270, _interim_data.d7_cm); // d3 - // update_sector_data(315, _interim_data.d8_cm); // d2 - update_sector_data(0, _interim_data.d1_cm); // d1 update_sector_data(45, _interim_data.d2_cm); // d8 update_sector_data(315, _interim_data.d3_cm); // d7 - if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS)) - { + if ((_last_distance_received_ms == 0) || (AP_HAL::millis() - _last_distance_received_ms > PROXIMITY_UAVCAN_TIMEOUT_MS)){ set_status(AP_Proximity::Status::NoData); // gcs().send_text(MAV_SEVERITY_INFO, "NoData\r\n"); - } - else - { + }else if( _interim_data.d1_cm > 4000 || _interim_data.d1_cm < 100){ + set_status(AP_Proximity::Status::NoData); + }else{ set_status(AP_Proximity::Status::Good); } } @@ -173,10 +160,8 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, if (_driver == nullptr) { Prx_Debug("----nullptr : ap:%d,id:%d ----",(int)_ap_uavcan,_node_id); - return; } - if (_ap_uavcan == nullptr) { _ap_uavcan = ap_uavcan; _node_id = node_id; @@ -195,14 +180,6 @@ void AP_Proximity_UAVCAN::handle_proximity_data_trampoline(AP_UAVCAN *ap_uavcan, _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) @@ -235,17 +212,6 @@ void AP_Proximity_UAVCAN::update_sector_data(int16_t angle_deg, uint16_t distanc boundary.reset_face(face); } _last_distance_received_ms = AP_HAL::millis(); - - - /* - uint8_t sector; - if(convert_angle_to_sector(angle_deg,sector)){ - _angle[sector] = angle_deg; - _distance[sector] = ((float)distance_cm) / 100; - _distance_valid[sector] = (_distance[sector] > distance_min() && _distance[sector] < distance_max()); - update_boundary_for_sector(sector, true); - } - */ } #endif diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 1baf1f6592..7aec0f6432 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -281,7 +281,7 @@ void AP_Vehicle::scheduler_delay_callback() if (AP_BoardConfig::in_config_error()) { gcs().send_text(MAV_SEVERITY_CRITICAL, "Config Error: fix problem then reboot"); } else { - gcs().send_text(MAV_SEVERITY_INFO, "Initialising ArduPilot"); + gcs().send_text(MAV_SEVERITY_INFO, "Initialising ZRUAV"); } } diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp index 6a354e40e4..737e1c1fb9 100644 --- a/libraries/GCS_MAVLink/GCS_Common.cpp +++ b/libraries/GCS_MAVLink/GCS_Common.cpp @@ -3612,8 +3612,8 @@ void GCS_MAVLINK::send_banner() // mark the firmware version in the tlog const AP_FWVersion &fwver = AP::fwversion(); - send_text(MAV_SEVERITY_INFO, "{%d}%s",__LINE__, fwver.fw_string); - + send_text(MAV_SEVERITY_INFO, "%s", fwver.fw_string); +#if 0 if (fwver.middleware_name && fwver.os_name) { send_text(MAV_SEVERITY_INFO, "{%d}%s: %s %s: %s",__LINE__, fwver.middleware_name, fwver.middleware_hash_str, @@ -3641,6 +3641,7 @@ void GCS_MAVLINK::send_banner() send_text(MAV_SEVERITY_INFO, "{%d}%s",__LINE__, banner_msg); } } +#endif }