diff --git a/.gitignore b/.gitignore index 2769d31790..38884c4846 100644 --- a/.gitignore +++ b/.gitignore @@ -93,6 +93,7 @@ mav.log mav.log.raw mav.parm mission.stg +output/ /defaults.parm /ArduCopter/defaults.parm /ArduPlane/defaults.parm diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp index b945032ef4..a4f89765b1 100644 --- a/Tools/AP_Periph/can.cpp +++ b/Tools/AP_Periph/can.cpp @@ -1705,12 +1705,13 @@ void AP_Periph_FW::esc_telem_update() #endif pkt.error_count = 0; - uint16_t error_count, count; + uint32_t error_count; + uint16_t count; if (esc_telem.get_count(i, count)) { - pkt.error_count = count; + pkt.error_count = count & 0xFF; } if (esc_telem.get_error_count(i, error_count)) { - pkt.error_count += (error_count & 0xFFFF) << 16; + pkt.error_count += (error_count & 0xFFFFFF) << 8; } uint8_t buffer[UAVCAN_EQUIPMENT_ESC_STATUS_MAX_SIZE] {}; diff --git a/Tools/Carbonix_scripts/carbonix_waf_build.sh b/Tools/Carbonix_scripts/carbonix_waf_build.sh index 82f7a0211e..102eef7469 100755 --- a/Tools/Carbonix_scripts/carbonix_waf_build.sh +++ b/Tools/Carbonix_scripts/carbonix_waf_build.sh @@ -6,18 +6,18 @@ set -e echo "Running distclean..." ./waf distclean -main_boards=("CubeOrange" "CubeOrangePlus" "CubeOrange-Volanti" "CubeOrangePlus-Volanti" "CubeOrange-Ottano" "CubeOrangePlus-Ottano") -for board in "${main_boards[@]}"; do - echo "Compiling ArduPlane for $board..." - ./waf configure --board "$board" -g - ./waf plane -done +# main_boards=("CubeOrange" "CubeOrangePlus" "CubeOrange-Volanti" "CubeOrangePlus-Volanti" "CubeOrange-Ottano" "CubeOrangePlus-Ottano") +# for board in "${main_boards[@]}"; do +# echo "Compiling ArduPlane for $board..." +# ./waf configure --board "$board" -g +# ./waf plane +# done -periph_boards=("CarbonixF405" "CarbonixF405-no-crystal") +periph_boards=("CarbonixF405") # "CarbonixF405-no-crystal") # Build all periph board with custom parameters for board in "${periph_boards[@]}"; do - for file in $(find libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/cpn_params/ -name "*.parm"); do + for file in $(find libraries/AP_HAL_ChibiOS/hwdef/CarbonixCommon/cpn_params/Volanti/*-M* -name "*.parm"); do # Extract the filename without the extension filename=$(basename -- "$file") filename="${filename%.*}" @@ -45,10 +45,10 @@ for board in "${periph_boards[@]}"; do done # Build all Default periph board -for board in "${periph_boards[@]}"; do - echo "Compiling AP_Periph for $board..." - ./waf configure --board "$board" -g - ./waf AP_Periph -done +# for board in "${periph_boards[@]}"; do +# echo "Compiling AP_Periph for $board..." +# ./waf configure --board "$board" -g +# ./waf AP_Periph +# done echo "Script completed successfully." diff --git a/libraries/AP_BLHeli/AP_BLHeli.cpp b/libraries/AP_BLHeli/AP_BLHeli.cpp index b6537fc9e4..64d6b09932 100644 --- a/libraries/AP_BLHeli/AP_BLHeli.cpp +++ b/libraries/AP_BLHeli/AP_BLHeli.cpp @@ -1457,7 +1457,7 @@ void AP_BLHeli::read_telemetry_packet(void) const uint8_t motor_idx = motor_map[last_telem_esc]; if (telem_uart->read(buf, telem_packet_size) < telem_packet_size) { // short read, we should have all bytes ready when this function is called - increment_error_count(motor_idx - chan_offset); + // THIS LINE IS NOT ACTUALLY REACHABLE, SO WE DON'T LOG THIS (the caller already checks) return; } @@ -1470,7 +1470,7 @@ void AP_BLHeli::read_telemetry_packet(void) if (buf[telem_packet_size-1] != crc) { // bad crc debug("Bad CRC on %u", last_telem_esc); - increment_error_count(motor_idx - chan_offset); + increment_error_count(motor_idx - chan_offset, 1 << 16); return; } // record the previous rpm so that we can slew to the new one @@ -1602,7 +1602,7 @@ void AP_BLHeli::update_telemetry(void) // if we have more than 10 bytes then we don't know which ESC // they are from. Throw them all away telem_uart->discard_input(); - increment_error_count(motor_map[last_telem_esc] - chan_offset); + increment_error_count(motor_map[last_telem_esc] - chan_offset, 1); return; } if (nbytes > 0 && @@ -1618,7 +1618,7 @@ void AP_BLHeli::update_telemetry(void) if (nbytes > 0 && nbytes < telem_packet_size) { // we've waited long enough, discard bytes if we don't have 10 yet telem_uart->discard_input(); - increment_error_count(motor_map[last_telem_esc] - chan_offset); + increment_error_count(motor_map[last_telem_esc] - chan_offset, 1 << 8); return; } if (nbytes == telem_packet_size) { diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp index d393934aa6..76dca0d13e 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.cpp @@ -337,7 +337,7 @@ bool AP_ESC_Telem::get_count(uint8_t esc_index, uint16_t& count) const } // get an individual ESC's error count if available, returns true on success -bool AP_ESC_Telem::get_error_count(uint8_t esc_index, uint16_t& error_count) const +bool AP_ESC_Telem::get_error_count(uint8_t esc_index, uint32_t& error_count) const { if (esc_index >= ESC_TELEM_MAX_ESCS) { return false; @@ -607,12 +607,12 @@ void AP_ESC_Telem::update_telem_data(const uint8_t esc_index, const AP_ESC_Telem } // callback to increment the error count in the frontend, should be called by the driver when an error occurs -void AP_ESC_Telem::increment_error_count(const uint8_t esc_index) +void AP_ESC_Telem::increment_error_count(const uint8_t esc_index, const uint32_t amount) { if (esc_index >= ESC_TELEM_MAX_ESCS) { return; } - _telem_data[esc_index].error_count++; + _telem_data[esc_index].error_count += amount; } // record an update to the RPM together with timestamp, this allows the notch values to be slewed diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem.h b/libraries/AP_ESC_Telem/AP_ESC_Telem.h index 129a3570fa..ed125c5bb9 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem.h @@ -71,7 +71,7 @@ class AP_ESC_Telem { bool get_count(uint8_t esc_index, uint16_t& count) const; // get an individual ESC's error count if available, returns true on success - bool get_error_count(uint8_t esc_index, uint16_t& error_count) const; + bool get_error_count(uint8_t esc_index, uint32_t& error_count) const; #if AP_EXTENDED_ESC_TELEM_ENABLED // get an individual ESC's input duty cycle if available, returns true on success @@ -129,7 +129,7 @@ class AP_ESC_Telem { void update_telem_data(const uint8_t esc_index, const AP_ESC_Telem_Backend::TelemetryData& new_data, const uint16_t data_mask); // callback to increment the error count in the frontend, should be called by the driver when an error occurs - void increment_error_count(const uint8_t esc_index); + void increment_error_count(const uint8_t esc_index, const uint32_t amount); #if AP_SCRIPTING_ENABLED /* diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp index 5d9f1c66cc..8733389ae6 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.cpp @@ -46,8 +46,8 @@ void AP_ESC_Telem_Backend::update_telem_data(const uint8_t esc_index, const Tele } // callback to increment the error count in the frontend, should be called by the driver when an error occurs -void AP_ESC_Telem_Backend::increment_error_count(const uint8_t esc_index) { - _frontend->increment_error_count(esc_index); +void AP_ESC_Telem_Backend::increment_error_count(const uint8_t esc_index, const uint32_t amount) { + _frontend->increment_error_count(esc_index, amount); } /* diff --git a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h index 0aa511ba6b..992afd3c77 100644 --- a/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h +++ b/libraries/AP_ESC_Telem/AP_ESC_Telem_Backend.h @@ -75,7 +75,7 @@ class AP_ESC_Telem_Backend { void update_telem_data(const uint8_t esc_index, const TelemetryData& new_data, const uint16_t data_present_mask); // callback to increment the error count in the frontend, should be called by the driver when an error occurs - void increment_error_count(const uint8_t esc_index); + void increment_error_count(const uint8_t esc_index, const uint32_t amount); private: AP_ESC_Telem* _frontend;