diff --git a/ArduCopter/AP_Arming.cpp b/ArduCopter/AP_Arming.cpp index be569ce1d5..38508c7866 100644 --- a/ArduCopter/AP_Arming.cpp +++ b/ArduCopter/AP_Arming.cpp @@ -847,6 +847,9 @@ bool AP_Arming_Copter::disarm(const AP_Arming::Method method, bool do_disarm_che bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure) { +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + return true; +#else uint32_t date; if (!copter.zr_app.check_deadline(date)) { @@ -857,4 +860,6 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure) }else{ return true; } + +#endif } \ No newline at end of file diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 378d7c4fb2..0eee5a24c1 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -607,12 +607,15 @@ void GCS_MAVLINK_Copter::send_banner() char frame_and_type_string[30]; copter.motors->get_frame_and_type_string(frame_and_type_string, ARRAY_SIZE(frame_and_type_string)); send_text(MAV_SEVERITY_INFO, "%s", frame_and_type_string); - + +#if CONFIG_HAL_BOARD != HAL_BOARD_SITL uint32_t deadline = 0; copter.zr_app.get_deadline_params(deadline); send_text(MAV_SEVERITY_INFO, "Date:%lu",deadline); send_text(MAV_SEVERITY_INFO, "ID:%lu",copter.zr_app.get_zr_sysid()); +#endif + } void GCS_MAVLINK_Copter::handle_command_ack(const mavlink_message_t &msg) diff --git a/Tools/ardupilotwaf/boards.py b/Tools/ardupilotwaf/boards.py index 5a20f3ce9d..715609205d 100644 --- a/Tools/ardupilotwaf/boards.py +++ b/Tools/ardupilotwaf/boards.py @@ -278,7 +278,7 @@ class Board: '-Werror=unused-value', '-Werror=unused-variable', '-Werror=delete-non-virtual-dtor', - '-Wfatal-errors', + # '-Wfatal-errors', '-Wno-trigraphs', '-Werror=parentheses', '-DARDUPILOT_BUILD', @@ -834,7 +834,7 @@ class chibios(Board): '-Wmissing-declarations', '-Wno-unused-parameter', '-Werror=array-bounds', - '-Wfatal-errors', + # '-Wfatal-errors', '-Werror=uninitialized', '-Werror=init-self', '-Werror=unused-but-set-variable', diff --git a/Tools/autotest/locations.txt b/Tools/autotest/locations.txt index 541a099634..d99405927e 100644 --- a/Tools/autotest/locations.txt +++ b/Tools/autotest/locations.txt @@ -1,4 +1,5 @@ #NAME=latitude,longitude,absolute-altitude,heading +CYDS=24.6187612,118.0383748,0,0 OSRF0=37.4003371,-122.0800351,0,353 OSRF0_PILOTSBOX=37.4003371,-122.0800351,2,270 CMAC=-35.363261,149.165230,584,353 diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh index 2fae275284..41e0437050 100755 --- a/Tools/environment_install/install-prereqs-ubuntu.sh +++ b/Tools/environment_install/install-prereqs-ubuntu.sh @@ -303,7 +303,8 @@ echo "Done!" CCACHE_PATH=$(which ccache) if [[ $DO_AP_STM_ENV -eq 1 ]]; then - install_arm_none_eabi_toolchain +# install_arm_none_eabi_toolchain + echo "*********** exit install_arm_none_eabi_toolchain **********!" fi heading "Check if we are inside docker environment..." diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp index 61945ca7c3..5d2b75de2c 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.cpp @@ -20,6 +20,14 @@ #include #include +#define BOAT_DEBUG 0 +#if BOAT_DEBUG +#include + # define Debug(fmt, args ...) gcs().send_text(MAV_SEVERITY_INFO, "Sonar: " fmt "\n", ## args) +#else + # define Debug(fmt, args ...) +#endif + extern const AP_HAL::HAL& hal; // return last value measured by sensor @@ -90,6 +98,7 @@ bool AP_RangeFinder_NMEA::decode(char c) return valid_sentence; } + case '@': // sentence begin case '$': // sentence begin _sentence_type = SONAR_UNKNOWN; _term_number = 0; @@ -119,6 +128,9 @@ bool AP_RangeFinder_NMEA::decode_latest_term() // handle the last term in a message if (_term_is_checksum) { _sentence_done = true; + if(_sentence_type == SONAR_EDEP){ // EDEP没有CRC + return true; + } uint8_t nibble_high = 0; uint8_t nibble_low = 0; if (!hex_to_uint8(_term[0], nibble_high) || !hex_to_uint8(_term[1], nibble_low)) { @@ -151,6 +163,7 @@ bool AP_RangeFinder_NMEA::decode_latest_term() return false; } const char *term_type = &_term[2]; + const char *term_sic = &_term[0]; if (strcmp(term_type, "DBT") == 0) { _sentence_type = SONAR_DBT; } else if (strcmp(term_type, "DPT") == 0) { @@ -159,6 +172,10 @@ bool AP_RangeFinder_NMEA::decode_latest_term() _sentence_type = SONAR_MTW; } else if (strcmp(term_type, "ED") == 0) { _sentence_type = SONAR_HDED; + }else if (strcmp(term_sic, "SIC") == 0) { + _sentence_type = SONAR_SIC; + }else if (strcmp(term_sic, "EDEP") == 0) { + _sentence_type = SONAR_EDEP; } else { _sentence_type = SONAR_UNKNOWN; } @@ -185,6 +202,30 @@ bool AP_RangeFinder_NMEA::decode_latest_term() if (_term_number == 4) { _distance_m = strtof(_term, NULL); } + }else if ((_sentence_type == SONAR_SIC )) { + + if (_term_number == 5) { + _distance_m = atof(_term); + Debug("dist:%.2f",_distance_m); + } + }else if (_sentence_type == SONAR_EDEP) { + // parse DBT messages + switch (_sentence_type + _term_number) { + case SONAR_EDEP + 1: + _distance_m = atof(_term); + // Debug("distance:%.2f",_distance_m); + Debug("%.2f,%.2f,%d",_distance_m,delt_dist,sound_speed); + break; + case SONAR_EDEP + 2: + delt_dist = atof(_term); + Debug("delt distance:%.2f",delt_dist); + break; + case SONAR_EDEP + 3: + sound_speed = atoi(_term); + Debug("sound speed:%d",sound_speed); + break; + } + } return false; diff --git a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h index 8f9cf0406a..85d510e210 100644 --- a/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h +++ b/libraries/AP_RangeFinder/AP_RangeFinder_NMEA.h @@ -52,6 +52,8 @@ private: SONAR_DPT, SONAR_MTW, // mean water temperature SONAR_HDED, // hondex custom sonar message + SONAR_SIC, + SONAR_EDEP }; // get a distance reading @@ -83,6 +85,9 @@ private: bool _term_is_checksum; // current term is the checksum sentence_types _sentence_type; // the sentence type currently being processed bool _sentence_done; // true if this sentence has already been decoded + + float delt_dist; + int sound_speed; }; #endif // AP_RANGEFINDER_NMEA_ENABLED