From 95ec62992364e118005e0ffdfe7c0e8dab89b68e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 28 Jan 2020 17:28:36 +0900 Subject: [PATCH] AP_HAL: add RCOutput::get_output_mode_banner --- libraries/AP_HAL/RCOutput.cpp | 52 +++++++++++++++++++ libraries/AP_HAL/RCOutput.h | 11 ++++ libraries/AP_HAL_ChibiOS/RCOutput.cpp | 73 +++++++++++++++++++++++++-- libraries/AP_HAL_ChibiOS/RCOutput.h | 5 +- 4 files changed, 135 insertions(+), 6 deletions(-) create mode 100644 libraries/AP_HAL/RCOutput.cpp diff --git a/libraries/AP_HAL/RCOutput.cpp b/libraries/AP_HAL/RCOutput.cpp new file mode 100644 index 0000000000..d42a3b0f06 --- /dev/null +++ b/libraries/AP_HAL/RCOutput.cpp @@ -0,0 +1,52 @@ +#include "AP_HAL.h" + +extern const AP_HAL::HAL &hal; + +// helper function for implementation of get_output_mode_banner +const char* AP_HAL::RCOutput::get_output_mode_string(enum output_mode out_mode) const +{ + // convert mode to string + switch (out_mode) { + case MODE_PWM_NONE: + return "None"; + case MODE_PWM_NORMAL: + return "PWM"; + case MODE_PWM_ONESHOT: + return "OneS"; + case MODE_PWM_ONESHOT125: + return "OS125"; + case MODE_PWM_BRUSHED: + return "Brush"; + case MODE_PWM_DSHOT150: + return "DS150"; + case MODE_PWM_DSHOT300: + return "DS300"; + case MODE_PWM_DSHOT600: + return "DS600"; + case MODE_PWM_DSHOT1200: + return "DS1200"; + case MODE_NEOPIXEL: + return "NeoP"; + } + + // we should never reach here but just in case + return "Unknown"; +} + +// convert output mode to string. helper function for implementation of get_output_mode_banner +void AP_HAL::RCOutput::append_to_banner(char banner_msg[], uint8_t banner_msg_len, output_mode out_mode, uint8_t low_ch, uint8_t high_ch) const +{ + const char* mode_str = get_output_mode_string(out_mode); + + // make copy of banner_msg + char banner_msg_temp[banner_msg_len]; + memcpy(banner_msg_temp, banner_msg, banner_msg_len); + + if (low_ch == high_ch) { + // handle single channel case + hal.util->snprintf(banner_msg, banner_msg_len, "%s %s:%u", banner_msg_temp, mode_str, (unsigned)low_ch); + } else { + // the general case + hal.util->snprintf(banner_msg, banner_msg_len, "%s %s:%u-%u", banner_msg_temp, mode_str, (unsigned)low_ch, (unsigned)high_ch); + } +} diff --git a/libraries/AP_HAL/RCOutput.h b/libraries/AP_HAL/RCOutput.h index 9d349549f9..3289eec22f 100644 --- a/libraries/AP_HAL/RCOutput.h +++ b/libraries/AP_HAL/RCOutput.h @@ -182,6 +182,11 @@ public: }; virtual void set_output_mode(uint16_t mask, enum output_mode mode) {} + /* + * get output mode banner to inform user of how outputs are configured + */ + virtual bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const { return false; } + /* set default update rate */ @@ -209,4 +214,10 @@ public: trigger send of neopixel data */ virtual void neopixel_send(void) {} + +protected: + + // helper functions for implementation of get_output_mode_banner + void append_to_banner(char banner_msg[], uint8_t banner_msg_len, output_mode out_mode, uint8_t low_ch, uint8_t high_ch) const; + const char* get_output_mode_string(enum output_mode out_mode) const; }; diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp index d808fd91f7..5a5d95ad79 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp +++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp @@ -312,7 +312,7 @@ void RCOutput::write(uint8_t chan, uint16_t period_us) // handle IO MCU channels if (AP_BoardConfig::io_enabled()) { uint16_t io_period_us = period_us; - if (iomcu_oneshot125 && ((1U< 0) && (iomcu_mode != MODE_PWM_NONE); + } +#endif + + // fill in ch_mode array for FMU channels + for (uint8_t i = 0; i < NUM_GROUPS; i++ ) { + const pwm_group &group = pwm_group_list[i]; + if (group.current_mode != MODE_PWM_NONE) { + for (uint8_t j = 0; j < ARRAY_SIZE(group.chan); j++) { + if (group.chan[j] != CHAN_DISABLED) { + const uint8_t chan_num = group.chan[j] + chan_offset; + if (chan_num < ARRAY_SIZE(ch_mode)) { + ch_mode[chan_num] = group.current_mode; + have_nonzero_modes = true; + } + } + } + } + } + + // handle simple case + if (!have_nonzero_modes) { + hal.util->snprintf(banner_msg, banner_msg_len, "RCOut: None"); + return true; + } + + // write banner to banner_msg + hal.util->snprintf(banner_msg, banner_msg_len, "RCOut:"); + uint8_t curr_mode_lowest_ch = 0; + for (uint8_t k = 1; k < ARRAY_SIZE(ch_mode); k++) { + if (ch_mode[k-1] != ch_mode[k]) { + if (ch_mode[k-1] != MODE_PWM_NONE) { + append_to_banner(banner_msg, banner_msg_len, ch_mode[k-1], curr_mode_lowest_ch + 1, k); + } + curr_mode_lowest_ch = k; + } + } + + // add final few channel's mode to banner (won't have been done by above loop) + const uint8_t final_index = ARRAY_SIZE(ch_mode)-1; + if (ch_mode[final_index] != MODE_PWM_NONE) { + append_to_banner(banner_msg, banner_msg_len, ch_mode[final_index], curr_mode_lowest_ch + 1, final_index + 1); + } + + return true; +} + /* start corking output */ diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.h b/libraries/AP_HAL_ChibiOS/RCOutput.h index 41b1584272..8cfd089c4c 100644 --- a/libraries/AP_HAL_ChibiOS/RCOutput.h +++ b/libraries/AP_HAL_ChibiOS/RCOutput.h @@ -49,6 +49,7 @@ public: return true; } void set_output_mode(uint16_t mask, enum output_mode mode) override; + bool get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len) const override; float scale_esc_to_unity(uint16_t pwm) override { return 2.0 * ((float) pwm - _esc_pwm_min) / (_esc_pwm_max - _esc_pwm_min) - 1.0; @@ -294,8 +295,8 @@ private: // widest pulse for oneshot triggering uint16_t trigger_widest_pulse; - // are we using oneshot125 for the iomcu? - bool iomcu_oneshot125; + // iomcu output mode (pwm, oneshot or oneshot125) + enum output_mode iomcu_mode = MODE_PWM_NORMAL; // find a channel group given a channel number struct pwm_group *find_chan(uint8_t chan, uint8_t &group_idx);