diff --git a/plot.sh b/plot.sh new file mode 100755 index 0000000..c3f4baf --- /dev/null +++ b/plot.sh @@ -0,0 +1,24 @@ +#!/bin/bash + +function sample() { + sigrok-cli -d fx2lafw:conn=1.13 --config 'samplerate=1 MHz' -C D5,D7 --time 400ms -P servo:data=D5 -P servo:data=D7 +} + +samples=$(sample | head -n2 | sed -e 's/servo-[12]: //') + +cha=$(echo "$samples" | head -n1) +cha_c=$(( ( `echo $cha | tr -d '.'` - 1000 ) / 10 )) +chb=$(echo "$samples" | tail -n1) +chb_c=$(( ( `echo $chb | tr -d '.'` - 1000 ) / 10 )) + +echo -n " |" +printf "%0.s " $(seq 1 49) +echo -n "|" +printf "%0.s " $(seq 1 49) +echo "|" +echo -n "A: $cha " +printf "%0.s#" $(seq 1 $cha_c) +echo +echo -n "B: $chb " +printf "%0.s#" $(seq 1 $chb_c) +echo diff --git a/rx_esp32/servo/__init__.py b/rx_esp32/servo/__init__.py new file mode 100644 index 0000000..2b953df --- /dev/null +++ b/rx_esp32/servo/__init__.py @@ -0,0 +1,24 @@ +## +## This file is part of the libsigrokdecode project. +## +## Copyright (C) 2014 Torsten Duwe +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 2 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see . +## + +''' +Servo decoder +''' + +from .pd import Decoder diff --git a/rx_esp32/servo/pd.py b/rx_esp32/servo/pd.py new file mode 100644 index 0000000..bbbe9e2 --- /dev/null +++ b/rx_esp32/servo/pd.py @@ -0,0 +1,141 @@ + +## +## This file is base of the timeing decoder from the libsigrokdecode project made by +## Torsten Duwe and Sebastien Bourdelin +## and converted to a Servo decoder by FReenen +## +## This program is free software; you can redistribute it and/or modify +## it under the terms of the GNU General Public License as published by +## the Free Software Foundation; either version 2 of the License, or +## (at your option) any later version. +## +## This program is distributed in the hope that it will be useful, +## but WITHOUT ANY WARRANTY; without even the implied warranty of +## MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +## GNU General Public License for more details. +## +## You should have received a copy of the GNU General Public License +## along with this program; if not, see . +## + +import sigrokdecode as srd + +class SamplerateError(Exception): + pass + +class Decoder(srd.Decoder): + api_version = 3 + id = 'servo' + name = 'Servo' + longname = 'Servo PWM signal (1 to 2 ms pulses every 20 ms)' + desc = '1 to 2 ms pulses every 20 ms' + license = 'gplv2+' + inputs = ['logic'] + outputs = [] + tags = ['Encoding'] + channels = ( + {'id': 'data', 'name': 'Data', 'desc': 'Data line'}, + ) + options = ( + {'id': 'polarity', 'desc': 'Polarity', 'default': 'active-high', + 'values': ('active-low', 'active-high')}, + ) + annotations = ( + ('duty-cycle', 'Duty cycle'), + ('period', 'Period'), + ) + annotation_rows = ( + ('duty-cycle', 'Duty cycle', (0,)), + ('period', 'Period', (1,)), + ) + # binary = ( + # ('raw', 'RAW file'), + # ) + + def __init__(self): + self.reset() + + def reset(self): + self.samplerate = None + self.ss_block = self.es_block = None + + def metadata(self, key, value): + if key == srd.SRD_CONF_SAMPLERATE: + self.samplerate = value + + def start(self): + self.out_ann = self.register(srd.OUTPUT_ANN) + self.out_binary = self.register(srd.OUTPUT_BINARY) + self.out_average = \ + self.register(srd.OUTPUT_META, + meta=(float, 'Average', 'PWM base (cycle) frequency')) + + def putx(self, data): + self.put(self.ss_block, self.es_block, self.out_ann, data) + + def putp(self, period_t): + # Adjust granularity. + # if period_t == 0 or period_t >= 1: + # period_s = '%.1f s' % (period_t) + # elif period_t <= 1e-12: + # period_s = '%.1f fs' % (period_t * 1e15) + # elif period_t <= 1e-9: + # period_s = '%.1f ps' % (period_t * 1e12) + # elif period_t <= 1e-6: + # period_s = '%.1f ns' % (period_t * 1e9) + # elif period_t <= 1e-3: + # period_s = '%.1f μs' % (period_t * 1e6) + # else: + # period_s = '%.1f ms' % (period_t * 1e3) + + period_s = '%.3f' % (period_t * 1e3) + + self.put(self.ss_block, self.es_block, self.out_ann, [1, [period_s]]) + + def putb(self, data): + self.put(self.ss_block, self.es_block, self.out_binary, data) + + def decode(self): + if not self.samplerate: + raise SamplerateError('Cannot decode without samplerate.') + + num_cycles = 0 + average = 0 + + # Wait for an "active" edge (depends on config). This starts + # the first full period of the inspected signal waveform. + self.wait({0: 'f' if self.options['polarity'] == 'active-low' else 'r'}) + self.first_samplenum = self.samplenum + + # Keep getting samples for the period's middle and terminal edges. + # At the same time that last sample starts the next period. + while True: + + # Get the next two edges. Setup some variables that get + # referenced in the calculation and in put() routines. + start_samplenum = self.samplenum + self.wait({0: 'f'}) + end_samplenum = self.samplenum + self.wait({0: 'r'}) + self.ss_block = start_samplenum + self.es_block = self.samplenum + + pulse = float((end_samplenum - start_samplenum) / self.samplerate) + ratio = (pulse - 0.0015) / 0.0005 + + # Report the duty cycle in percent. + # percent = float(ratio * 100) + # self.putx([0, ['%f%%' % percent]]) + + # Report the duty cycle in the binary output. + # self.putb([0, bytes([int(ratio * 128)])]) + + # Report the period in units of time. + # print("servo pulse: " + str(pulse)) + self.putp(pulse) + + # Update and report the new duty cycle average. + # num_cycles += 1 + # average += percent + # self.put(self.first_samplenum, self.es_block, self.out_average, + # float(average / num_cycles)) diff --git a/rx_esp32/src/config.h b/rx_esp32/src/config.h index 38a4099..496bf44 100644 --- a/rx_esp32/src/config.h +++ b/rx_esp32/src/config.h @@ -12,13 +12,8 @@ static uint8_t BoatId = 1; #define HISTORY // enable cli history -#define SERVOS_CH0 {LEDC_CHANNEL_0, 7}, -#define SERVOS_CH1 {LEDC_CHANNEL_1, 21} -#define SERVOS_CH2 -#define SERVOS_CH3 -#define SERVOS_CH4 -#define SERVOS_CH5 -#define SERVOS_CH6 +#define SERVOS_CH0_PIN 7 +#define SERVOS_CH1_PIN 21 #define WS_RX_BUFFER_LEN 1024 #define WS_TX_BUFFER_LEN 128 diff --git a/rx_esp32/src/servos.c b/rx_esp32/src/servos.c index dce3aab..6155087 100644 --- a/rx_esp32/src/servos.c +++ b/rx_esp32/src/servos.c @@ -14,27 +14,33 @@ #define SERVO_DUTY_MIN ( 410) // 2**13 * (1000/20000) // 1000us of the 20ms #define SERVO_DUTY_MAX ( 819) // 2**13 * (2000/20000) // 2000us of the 20ms // #define SERVO_DUTY_MAX (1024) // 2**13 * (2500/20000) // 2500us of the 20ms -#define SERVO_DUTY_DEFUALT ( 614) // 2**13 * (1500/20000) // 2500us of the 20ms +#define SERVO_DUTY_MID ( 614) // 2**13 * (1500/20000) // 2500us of the 20ms #define SERVO_DUTY_DIFF (SERVO_DUTY_MAX - SERVO_DUTY_MIN) -typedef struct ServoCh_t { - ledc_channel_t channel; - int gpio; -} ServoCh_t; - -static ServoCh_t Server_chs[] = { - SERVOS_CH0 - SERVOS_CH1 - SERVOS_CH2 - SERVOS_CH3 - SERVOS_CH4 - SERVOS_CH5 - SERVOS_CH6 -}; +servo_config_t Servos[2]; +bool Servos_ch_swap = false; void servo_init(void) { + Servos[0].pin = SERVOS_CH0_PIN; + Servos[0].ledc_ch = LEDC_CHANNEL_0; + Servos[0].pulse_min = SERVO_DUTY_MIN; + Servos[0].pulse_mid = SERVO_DUTY_MID; + Servos[0].pulse_max = SERVO_DUTY_MAX - 100; + Servos[0].mid_delay = 10; + Servos[0].mid_delay_timer = 0; + Servos[0].reversed = false; + + Servos[1].pin = SERVOS_CH1_PIN; + Servos[1].ledc_ch = LEDC_CHANNEL_1; + Servos[1].pulse_min = SERVO_DUTY_MIN; + Servos[1].pulse_mid = SERVO_DUTY_MID; + Servos[1].pulse_max = SERVO_DUTY_MAX; + Servos[1].mid_delay = 0; + Servos[1].mid_delay_timer = 0; + Servos[1].reversed = false; + // Prepare and then apply the LEDC PWM timer configuration ledc_timer_config_t ledc_timer = { .speed_mode = SERVO_LEDC_MODE, @@ -45,35 +51,87 @@ void servo_init(void) }; ledc_timer_config(&ledc_timer); - for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i >= 0; i--) + for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--) { // Prepare and then apply the LEDC PWM channel configuration ledc_channel_config_t ledc_channel = { .speed_mode = SERVO_LEDC_MODE, - .channel = Server_chs[i].channel, + .channel = Servos[i].ledc_ch, .timer_sel = SERVO_LEDC_TIMER, .intr_type = LEDC_INTR_DISABLE, - .gpio_num = Server_chs[i].gpio, - .duty = SERVO_DUTY_DEFUALT, // Set duty to 0% + .gpio_num = Servos[i].pin, + .duty = Servos[i].pulse_mid, .hpoint = 0 }; ledc_channel_config(&ledc_channel); - LOG_D("servo_init: inited servo channel %d (ledc: %u, gpio: %d)", i, Server_chs[i].channel, Server_chs[i].gpio); + LOG_D("servo_init: inited servo channel %d (ledc: %u, gpio: %d)", i, Servos[i].ledc_ch, Servos[i].pin); } } void servo_deinit(void) { ledc_timer_pause(SERVO_LEDC_MODE, SERVO_LEDC_TIMER); - for (int i = sizeof(Server_chs)/sizeof(ServoCh_t)-1; i > 0; i--) + for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--) { - ledc_stop(SERVO_LEDC_MODE, Server_chs[i].channel, 0); + ledc_stop(SERVO_LEDC_MODE, Servos[i].ledc_ch, 0); } } void servo_set(uint8_t ch, uint8_t pos) { - uint32_t duty = (uint32_t) ((double)pos * (double)SERVO_DUTY_DIFF/(double)UINT8_MAX); - duty += (SERVO_DUTY_MIN + (SERVO_DUTY_DIFF/2)); + if (Servos_ch_swap) + { + if (ch == 0) + { + ch = 1; + } + else + { + ch = 0; + } + } + + if (Servos[ch].reversed) + { + pos = -((int16_t)pos) + 255; + } + + double pos_double = ((double) pos) - 128; + + if (pos_double < 0) + { + if (Servos[ch].mid_delay_timer > 0) + { + Servos[ch].mid_delay_timer--; + pos_double = 0.0; + } + else + { + Servos[ch].mid_delay_timer = -Servos[ch].mid_delay; + } + } + else if (pos_double > 0) + { + if (Servos[ch].mid_delay_timer < 0) + { + Servos[ch].mid_delay_timer++; + pos_double = 0.0; + } + else + { + Servos[ch].mid_delay_timer = Servos[ch].mid_delay; + } + } + + uint32_t duty; + if (pos_double > 0.0) + { + duty = (pos_double/128.0 * (Servos[ch].pulse_max - Servos[ch].pulse_mid)) + Servos[ch].pulse_mid; + } + else + { + duty = (pos_double/128.0 * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid; + } + if (duty < SERVO_DUTY_MIN) { duty = SERVO_DUTY_MIN; @@ -82,6 +140,6 @@ void servo_set(uint8_t ch, uint8_t pos) { duty = SERVO_DUTY_MAX; } - ledc_set_duty(SERVO_LEDC_MODE, Server_chs[ch].channel, duty); - ledc_update_duty(SERVO_LEDC_MODE, Server_chs[ch].channel); + ledc_set_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch, duty); + ledc_update_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch); } diff --git a/rx_esp32/src/servos.h b/rx_esp32/src/servos.h index 2e05cff..d8afd91 100644 --- a/rx_esp32/src/servos.h +++ b/rx_esp32/src/servos.h @@ -2,6 +2,23 @@ #define SERVOS_H #include +#include + +#include + +typedef struct { + int pin; + ledc_channel_t ledc_ch; + uint16_t pulse_min; + uint16_t pulse_mid; + uint16_t pulse_max; + uint8_t mid_delay; + int16_t mid_delay_timer; + bool reversed; +} servo_config_t; +extern servo_config_t Servos[2]; + +extern bool Servos_ch_swap; void servo_init(void); void servo_deinit(void); diff --git a/rx_esp32/src/ws.c b/rx_esp32/src/ws.c index 2f20905..76f4c3d 100644 --- a/rx_esp32/src/ws.c +++ b/rx_esp32/src/ws.c @@ -166,7 +166,7 @@ void ws_sendData(ws_client_t client) { if ((client->txBuffer_wp > 0) && esp_websocket_client_is_connected(client->handle)) { - LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]); + // LOG_D("ws_sendData: (%d b) '%.*s'", client->txBuffer_wp, client->txBuffer_wp, (char *)&client->txBuffer[0]); esp_websocket_client_send_text(client->handle, (char *)&client->txBuffer[0], client->txBuffer_wp, 1000 * portTICK_PERIOD_MS); client->txBuffer_wp = 0; }