Skip to content

Commit

Permalink
add quad true_PWM outputs
Browse files Browse the repository at this point in the history
  • Loading branch information
@magicrub
magicrub committed Mar 19, 2024
1 parent cd19114 commit 3036cc4
Show file tree
Hide file tree
Showing 5 changed files with 257 additions and 64 deletions.
278 changes: 222 additions & 56 deletions Tools/AP_Periph/adc.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,34 @@ 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),

// @Param: PWM_MIN
// @DisplayName: PWM_MIN
// @Description: PWM_MIN duty cycle percent. Should be higher than PWM_MIN
// @Range: 0 100
// @Units: %
AP_GROUPINFO("PWM_MIN", 7, Periph_ADC, params.pwm_min_duty, 0),

// @Param: PWM_MAX
// @DisplayName: PWM_MAX
// @Description: PWM_MAX duty cycle percent. Should be lower than PWM_MAX
// @Range: 0 100
// @Units: %
AP_GROUPINFO("PWM_MAX", 8, Periph_ADC, params.pwm_max_duty, 98),

AP_GROUPEND
};
Expand All @@ -58,79 +86,217 @@ 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_min_0 = 1000 + params.pwm_min_duty.get() * 10;
const uint16_t pwm_max_1 = 2000 - params.pwm_max_duty.get() * 10;

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:
// TODO: use the PID output somehow
// pwms[0] = pwm_max_0 + output;
// pwms[1] = pwm_max_0 + output;
// pwms[2] = pwm_max_0 + output;
// pwms[3] = pwm_max_0 + output;
memset(pwms, 0, sizeof(pwms));
break;

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<ARRAY_SIZE(pwms); i++) {
periph.rcout_srv_PWM(i+1, pwms[i]);
}


// debug stuff
if (params.send_rate >= 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<pkt1.voltages.len; i++) {
pkt1.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer1[ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_MAX_SIZE] {};
uint16_t total_size1 = ardupilot_equipment_power_BatteryCells_encode(&pkt1, buffer1, !periph.canfdout());
periph.canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_SIGNATURE,
ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer1[0],
total_size1);



rb_ADC pkt2 {};
pkt2.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt2.voltages.data));
for (uint8_t i=0; i<pkt2.voltages.len; i++) {
pkt2.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer2[RB_ADC_MAX_SIZE] {};
uint16_t total_size2 = rb_ADC_encode(&pkt2, buffer2, !periph.canfdout());
periph.canard_broadcast(RB_ADC_SIGNATURE,
RB_ADC_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer2[0],
total_size2);

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) {
_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<pkt1.voltages.len; i++) {
pkt1.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer1[ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_MAX_SIZE] {};
uint16_t total_size1 = ardupilot_equipment_power_BatteryCells_encode(&pkt1, buffer1, !periph.canfdout());
periph.canard_broadcast(ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_SIGNATURE,
ARDUPILOT_EQUIPMENT_POWER_BATTERYCELLS_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer1[0],
total_size1);



rb_ADC pkt2 {};
pkt2.voltages.len = MIN(ADS1115_CHANNELS_COUNT,ARRAY_SIZE(pkt2.voltages.data));
for (uint8_t i=0; i<pkt2.voltages.len; i++) {
pkt2.voltages.data[i] = adc_lib.read_by_channel(i);
}

uint8_t buffer2[RB_ADC_MAX_SIZE] {};
uint16_t total_size2 = rb_ADC_encode(&pkt2, buffer2, !periph.canfdout());
periph.canard_broadcast(RB_ADC_SIGNATURE,
RB_ADC_ID,
CANARD_TRANSFER_PRIORITY_LOW,
&buffer2[0],
total_size2);
}
}

Expand Down
8 changes: 8 additions & 0 deletions Tools/AP_Periph/adc.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,14 @@ class Periph_ADC {

void init();
void update();
uint32_t get_true_pwm_mask() const { return (uint32_t)params.true_pwm_mask.get(); }

static const struct AP_Param::GroupInfo var_info[];

private:

void debug_print_stuff();

AC_PID pid {
// Initial Conditions
AC_PID::Defaults{
Expand All @@ -40,6 +43,11 @@ class Periph_ADC {
struct {
AP_Int32 send_rate;
AP_Int16 servo_index;
AP_Float debug;
AP_Float test_input;
AP_Int32 true_pwm_mask;
AP_Float pwm_min_duty;
AP_Float pwm_max_duty;
} params;

AP_ADC_ADS1115 adc_lib;
Expand Down
6 changes: 6 additions & 0 deletions Tools/AP_Periph/rc_out.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,12 @@ void AP_Periph_FW::rcout_init()
}
}

#ifdef HAL_PERIPH_ENABLE_ADC
if (adc.get_true_pwm_mask() != 0) {
esc_mask = adc.get_true_pwm_mask();
}
#endif

// run this once and at 1Hz to configure aux and esc ranges
rcout_init_1Hz();

Expand Down
Loading

0 comments on commit 3036cc4

Please sign in to comment.