From 819c220a726b22ae3765fd5625992cb307b27e91 Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Tue, 19 Mar 2024 10:41:52 -0700 Subject: [PATCH] add quad true_PWM outputs --- Tools/AP_Periph/adc.cpp | 257 +++++++++++++++++++++------ Tools/AP_Periph/adc.h | 6 + Tools/AP_Periph/rc_out.cpp | 6 + libraries/AP_DAC/AP_DAC_DACx0501.cpp | 28 ++- libraries/AP_DAC/AP_DAC_DACx0501.h | 1 + 5 files changed, 234 insertions(+), 64 deletions(-) diff --git a/Tools/AP_Periph/adc.cpp b/Tools/AP_Periph/adc.cpp index 3eddb4ced5d41b..c783ba6890c305 100644 --- a/Tools/AP_Periph/adc.cpp +++ b/Tools/AP_Periph/adc.cpp @@ -41,6 +41,20 @@ const AP_Param::GroupInfo Periph_ADC::var_info[] { // @Description: SRV_INDEX AP_GROUPINFO("SRV_INDEX", 3, Periph_ADC, params.servo_index, -1), + // @Param: DEBUG + // @DisplayName: Debug + // @Description: Debug + AP_GROUPINFO("DEBUG", 4, Periph_ADC, params.debug, -1), + + // @Param: TEST_INPUT + // @DisplayName: TEST_INPUT + // @Description: TEST_INPUT + AP_GROUPINFO("TEST_INPUT", 5, Periph_ADC, params.test_input, 0), + + // @Param: PWM_MASK + // @DisplayName: PWM_MASK + // @Description: PWM_MASK to set channels to be true PWM + AP_GROUPINFO("PWM_MASK", 6, Periph_ADC, params.true_pwm_mask, 0), AP_GROUPEND }; @@ -58,79 +72,210 @@ void Periph_ADC::update() _last_pid_update_ms = now_ms; - // get a new sample - const uint32_t last_sample_timestamp_ms = adc_lib.get_last_sample_timestamp_ms(); - const bool new_sample_available = (last_sample_timestamp_ms != 0) && (last_sample_timestamp_ms != _last_sample_timestamp_ms); - - // params.servo_index - float target_srv_in = 0; + float target_srv_in = params.test_input.get(); const SRV_Channel *c = SRV_Channels::srv_channel(params.servo_index); if (c != nullptr && c->valid_function()) { target_srv_in = linear_interpolate(0, 5.0, c->get_output_pwm(), c->get_output_min(), c->get_output_max()); } + // The Update the PID float measurement_adc_in = adc_lib.read_by_channel(2); - const float output_dac_out = pid.update_all(target_srv_in, measurement_adc_in, dt); - - periph.dac.set_output_voltage(output_dac_out); - - // TODO: push to 4 other servo outputs but with true PWM output at 20kHz + const float output = pid.update_all(target_srv_in, measurement_adc_in, dt); + (void)output; + + int16_t pwms[4] {}; // rcout_esc wants this to be signed for some siilly reason + const uint16_t pwm_range = 1000; + const uint16_t pwm_min_0 = 1000; + const uint16_t pwm_max_1 = pwm_min_0 + 0.98*pwm_range; + + switch ((int)params.debug.get()) { + case 0: + pwms[0] = pwm_min_0; + pwms[1] = pwm_min_0; + pwms[2] = pwm_min_0; + pwms[3] = pwm_min_0; + break; + + case 1: + pwms[0] = pwm_max_1; + pwms[1] = pwm_min_0; + pwms[2] = pwm_min_0; + pwms[3] = pwm_min_0; + break; + + case 2: + pwms[0] = pwm_min_0; + pwms[1] = pwm_max_1; + pwms[2] = pwm_min_0; + pwms[3] = pwm_min_0; + break; + + case 3: + pwms[0] = pwm_max_1; + pwms[1] = pwm_max_1; + pwms[2] = pwm_min_0; + pwms[3] = pwm_min_0; + break; + + case 4: + pwms[0] = pwm_min_0; + pwms[1] = pwm_min_0; + pwms[2] = pwm_max_1; + pwms[3] = pwm_min_0; + break; + + case 5: + pwms[0] = pwm_max_1; + pwms[1] = pwm_min_0; + pwms[2] = pwm_max_1; + pwms[3] = pwm_min_0; + break; + + case 6: + pwms[0] = pwm_min_0; + pwms[1] = pwm_max_1; + pwms[2] = pwm_max_1; + pwms[3] = pwm_min_0; + break; + + case 7: + pwms[0] = pwm_max_1; + pwms[1] = pwm_max_1; + pwms[2] = pwm_max_1; + pwms[3] = pwm_min_0; + break; + + case 8: + pwms[0] = pwm_min_0; + pwms[1] = pwm_min_0; + pwms[2] = pwm_min_0; + pwms[3] = pwm_max_1; + break; + + case 9: + pwms[0] = pwm_max_1; + pwms[1] = pwm_min_0; + pwms[2] = pwm_min_0; + pwms[3] = pwm_max_1; + break; + + case 10: + pwms[0] = pwm_min_0; + pwms[1] = pwm_max_1; + pwms[2] = pwm_min_0; + pwms[3] = pwm_max_1; + break; + + case 11: + pwms[0] = pwm_max_1; + pwms[1] = pwm_max_1; + pwms[2] = pwm_min_0; + pwms[3] = pwm_max_1; + break; + + case 12: + pwms[0] = pwm_min_0; + pwms[1] = pwm_min_0; + pwms[2] = pwm_max_1; + pwms[3] = pwm_max_1; + break; + + case 13: + pwms[0] = pwm_max_1; + pwms[1] = pwm_min_0; + pwms[2] = pwm_max_1; + pwms[3] = pwm_max_1; + break; + + case 14: + pwms[0] = pwm_min_0; + pwms[1] = pwm_max_1; + pwms[2] = pwm_max_1; + pwms[3] = pwm_max_1; + break; + + case 15: + pwms[0] = pwm_max_1; + pwms[1] = pwm_max_1; + pwms[2] = pwm_max_1; + pwms[3] = pwm_max_1; + break; + + case -1: + default: + memset(pwms, 0, sizeof(pwms)); + break; + } + // send to SERVO_Functions motor1..4 + periph.rcout_esc(pwms, ARRAY_SIZE(pwms)); + // send to SERVO_Functions RCin1..4 + for (uint8_t i=0; i= 1 || params.send_rate == -1) { + debug_print_stuff(); + } +} +void Periph_ADC::debug_print_stuff() +{ + const uint32_t now_ms = AP_HAL::millis(); + // get a new sample + const uint32_t last_sample_timestamp_ms = adc_lib.get_last_sample_timestamp_ms(); + const bool new_sample_available = (last_sample_timestamp_ms != 0) && (last_sample_timestamp_ms != _last_sample_timestamp_ms); - // debug stuff - if (params.send_rate >= 1 || params.send_rate == -1) { if (params.send_rate == -1) { - if (now_ms - _last_debug_ms >= 1000) { - _last_debug_ms = now_ms; - _last_sample_timestamp_ms = last_sample_timestamp_ms; - - can_printf("ADC%s AN0SE:%.2f, AN1SE:%.2f, AN23DIF:%.2f", - new_sample_available ? "" : " STALE", - adc_lib.read_by_channel(0), - adc_lib.read_by_channel(1), - adc_lib.read_by_channel(2)); - } - - } else if (new_sample_available && now_ms - _last_send_ms >= params.send_rate) { + if (now_ms - _last_debug_ms >= 1000) { + _last_debug_ms = now_ms; _last_sample_timestamp_ms = last_sample_timestamp_ms; - _last_send_ms = now_ms; - - ardupilot_equipment_power_BatteryCells pkt1 {}; - pkt1.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt1.voltages.data)); - for (uint8_t i=0; i= params.send_rate) { + _last_sample_timestamp_ms = last_sample_timestamp_ms; + _last_send_ms = now_ms; + + ardupilot_equipment_power_BatteryCells pkt1 {}; + pkt1.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt1.voltages.data)); + for (uint8_t i=0; ivalid_function()) { + value_float = linear_interpolate(0, max_output, c->get_output_pwm(), c->get_output_min(), c->get_output_max()); + } else { + value_float = linear_interpolate(0, max_output, target_voltage, get_voltage_min(), get_voltage_max()); + } - uint16_t value = linear_interpolate(0, max_output, target_voltage, get_voltage_min(), get_voltage_max()); - value *= MAX(0.01f, params.gain.get()); + const uint16_t output_value = constrain_float(value_float * params.gain.get(), 0, UINT16_MAX); uint8_t buf[3]; buf[0] = DACx0501_REG_DAC_DATA; - buf[1] = HIGHBYTE(value); - buf[2] = LOWBYTE(value); + buf[1] = HIGHBYTE(output_value); + buf[2] = LOWBYTE(output_value); UNUSED_RESULT(_dev->transfer(buf, sizeof(buf), nullptr, 0)); } diff --git a/libraries/AP_DAC/AP_DAC_DACx0501.h b/libraries/AP_DAC/AP_DAC_DACx0501.h index 5bedc4a4d7e4b5..5d07fb5460c9ab 100644 --- a/libraries/AP_DAC/AP_DAC_DACx0501.h +++ b/libraries/AP_DAC/AP_DAC_DACx0501.h @@ -33,6 +33,7 @@ class AP_DAC_DACx0501 struct { AP_Int8 i2c_address; AP_Int8 i2c_bus; + AP_Int16 servo_index; AP_Int16 bit_resolution; AP_Float gain; AP_Float initial_voltage;