diff --git a/docs/man/man9/hostmot2.9 b/docs/man/man9/hostmot2.9 index d91a305..bd89e61 100644 --- a/docs/man/man9/hostmot2.9 +++ b/docs/man/man9/hostmot2.9 @@ -134,6 +134,11 @@ Only enable the first N pwmgens. If N is -1, all pwmgens are enabled. If N is 0, no pwmgens are enabled. If N is greater than the number of pwmgens available in the firmware, the board will fail to register. .TP +\fBnum_3pwmgens\fR [optional, default: -1] +Only enable the first N Three-phase pwmgens. If N is -1, all 3pwmgens +are enabled. If N is 0, no pwmgens are enabled. If N is greater than the +number of pwmgens available in the firmware, the board will fail to register. +.TP \fBnum_stepgens\fR [optional, default: -1] Only enable the first N stepgens. If N is -1, all stepgens are enabled. If N is 0, no stepgens are enabled. If N is greater than the number of @@ -287,6 +292,71 @@ is determined by the ClockHigh frequency of the Anything IO board; the frequency. Other boards may have different clocks, resulting in different max PDM frequencies. If the user attempts to set the frequency too high, it will be clipped to the max supported frequency of the board. + +.SH 3ppwmgen +Three-Phase PWM generators (3pwmgens) are intended for controlling the high-side +and low-side gates in a 3-phase motor driver. The function is included to +support the Mesa motor controller daughter-cards but can be used to control +an IGBT or similar driver directly. +3pwmgens have names like "hm2_..3pwmgen." +where is a 2-digit number. There will be num_3pwmgens instances, +starting at 00. +Each instance allocates 7 output and one input pins on the Mesa card connectors. +Outputs are: PWM A, PWM B, PWM C, /PWM A, /PWM B, /PWM C, Enable. The first three +pins are the high side drivers, the second three are their complementary low-side +drivers. The enable bit is intended to control the servo amplifier. +The input bit is a fault bit, typically wired to over-current detection. When set +the PWM generator is disabled. +The three phase duty-cycles are individually controllable from -Scale to +Scale. +Note that 0 corresponds to a 50% duty cycle and this is the inialization value. + +Pins: + +(float input) A-value, B-value, C-value: The PWM command value for each phase, +limited to +/- "scale". Defaults to zero which is 50% duty cycle on high-side and +low-sidepins (but see the "deadtime" parameter) + +(bit input) enable: When high the PWM is enabled as long as the fault bit is not +set by the external fault input pin. When low the PWM is disabled, with both high- +side and low-side drivers low. This is not the same as 0 output (50% duty cycle on +both sets of pins) or negative full scale (where the low side drivers are "on" +100% of the time) + +(bit output) fault: Indicates the status of the fault bit. This output latches high +once set by the physical fault pin until the "enable" pin is set to high. + +Parameters: + +(u32 rw) deadtime: Sets the dead-time between the high-side driver turning off and +the low-side driver turning on and vice-versa. Deadtime is subtracted from on time +and added to off time symmetrically. For example with 20 KHz PWM (50 uSec period), +50% duty cycle and zero dead time, the PWM and NPWM outputs would be square +waves (NPWM being inverted from PWM) with high times of 25 uS. With the same +settings but 1 uS of deadtime, the PWM and NPWM outputs would both have high +times of 23 uS (25 - (2X 1 uS), 1 uS per edge). +The value is specified in nS and defaults to a rather conservative 5000nS. Setting +this parameter to too low a value could be both expensive and dangerous as if both +gates are open at the same time there is effectively a short circuit accross the +supply. + +(float rw) scale: Sets the half-scale of the specified 3-phase PWM generator. +PWM values from -scale to +scale are valid. Default is +/- 1.0 + +(bit rw) fault-invert: Sets the polarity of the fault input pin. A value of 1 means +that a fault is triggered with the pin high, and 0 means that a fault it triggered +when the pin is pulled low. Default 0, fault = low so that the PWM works with the +fault pin unconnected. + +(u32 rw) sample-time: Sets the time during the cycle when an ADC pulse is generated. +0 = start of PWM cycle and 1 = end. Not currently useful to EMC2. Default 0.5. + +In addition the per-instance parameters above there is the following parameter +that affects all instances + +(u32 rw) frequency: Sets the master PWM frequency. Maximum is approx 48kHz, minimum +is 1kHz. Defaults to 20kHz. + + .SH stepgen stepgens have names like "hm2_..stepgen.". diff --git a/src/Makefile b/src/Makefile index e7e9810..711ad8d 100644 --- a/src/Makefile +++ b/src/Makefile @@ -605,6 +605,8 @@ obj-$(CONFIG_STEPGEN) += stepgen.o stepgen-objs := hal/components/stepgen.o $(MATHSTUB) obj-$(CONFIG_FREQGEN) += freqgen.o freqgen-objs := hal/components/freqgen.o $(MATHSTUB) +obj-$(CONFIG_TPPWMGEN) += tpwmgen.o +tp_pwmgen-objs := hal/components/tp_pwmgen.o $(MATHSTUB) obj-$(CONFIG_PWMGEN) += pwmgen.o pwmgen-objs := hal/components/pwmgen.o $(MATHSTUB) obj-$(CONFIG_SIGGEN) += siggen.o @@ -664,6 +666,7 @@ hostmot2-objs := \ hal/drivers/mesa-hostmot2/ioport.o \ hal/drivers/mesa-hostmot2/encoder.o \ hal/drivers/mesa-hostmot2/pwmgen.o \ + hal/drivers/mesa-hostmot2/tp_pwmgen.o \ hal/drivers/mesa-hostmot2/stepgen.o \ hal/drivers/mesa-hostmot2/watchdog.o \ hal/drivers/mesa-hostmot2/pins.o \ @@ -814,6 +817,7 @@ endif ../rtlib/stepgen$(MODULE_EXT): $(addprefix objects/rt,$(stepgen-objs)) ../rtlib/freqgen$(MODULE_EXT): $(addprefix objects/rt,$(freqgen-objs)) ../rtlib/pwmgen$(MODULE_EXT): $(addprefix objects/rt,$(pwmgen-objs)) +../rtlib/tp_pwmgen$(MODULE_EXT): $(addprefix objects/rt,$(tp_pwmgen-objs)) ../rtlib/siggen$(MODULE_EXT): $(addprefix objects/rt,$(siggen-objs)) ../rtlib/at_pid$(MODULE_EXT): $(addprefix objects/rt,$(at_pid-objs)) ../rtlib/pid$(MODULE_EXT): $(addprefix objects/rt,$(pid-objs)) diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.c b/src/hal/drivers/mesa-hostmot2/hostmot2.c index 18d7281..a47a605 100644 --- a/src/hal/drivers/mesa-hostmot2/hostmot2.c +++ b/src/hal/drivers/mesa-hostmot2/hostmot2.c @@ -83,6 +83,7 @@ static void hm2_read(void *void_hm2, long period) { if ((*hm2->llio->io_error) != 0) return; hm2_ioport_gpio_process_tram_read(hm2); + hm2_tp_pwmgen_read(hm2); // check the status of the fault bit hm2_encoder_process_tram_read(hm2, period); hm2_stepgen_process_tram_read(hm2, period); @@ -101,6 +102,7 @@ static void hm2_write(void *void_hm2, long period) { hm2_ioport_gpio_prepare_tram_write(hm2); hm2_pwmgen_prepare_tram_write(hm2); + hm2_tp_pwmgen_prepare_tram_write(hm2); hm2_stepgen_prepare_tram_write(hm2, period); hm2_tram_write(hm2); @@ -109,9 +111,9 @@ static void hm2_write(void *void_hm2, long period) { hm2_ioport_write(hm2); // handles gpio.is_output but not gpio.out (that's done in tram_write() above) hm2_watchdog_write(hm2); // in case the user has written to the watchdog.timeout_ns param hm2_pwmgen_write(hm2); // update pwmgen registers if needed + hm2_tp_pwmgen_write(hm2); // update Three Phase PWM registers if needed hm2_stepgen_write(hm2); // update stepgen registers if needed hm2_encoder_write(hm2); // update ctrl register if needed - hm2_raw_write(hm2); } @@ -171,6 +173,7 @@ const char *hm2_get_general_function_name(int gtag) { case HM2_GTAG_STEPGEN: return "StepGen"; case HM2_GTAG_PWMGEN: return "PWMGen"; case HM2_GTAG_TRANSLATIONRAM: return "TranslationRAM"; + case HM2_GTAG_TPPWM: return "ThreePhasePWM"; default: { static char unknown[100]; rtapi_snprintf(unknown, 100, "(unknown-gtag-%d)", gtag); @@ -188,6 +191,7 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { // default is to enable everything in the firmware hm2->config.num_encoders = -1; hm2->config.num_pwmgens = -1; + hm2->config.num_tp_pwmgens = -1; hm2->config.num_stepgens = -1; hm2->config.enable_raw = 0; hm2->config.firmware = NULL; @@ -215,6 +219,10 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { token += 12; hm2->config.num_pwmgens = simple_strtol(token, NULL, 0); + } else if (strncmp(token, "num_3pwmgens=", 13) == 0) { + token += 13; + hm2->config.num_tp_pwmgens = simple_strtol(token, NULL, 0); + } else if (strncmp(token, "num_stepgens=", 13) == 0) { token += 13; hm2->config.num_stepgens = simple_strtol(token, NULL, 0); @@ -238,6 +246,7 @@ static int hm2_parse_config_string(hostmot2_t *hm2, char *config_string) { HM2_DBG("final config:\n"); HM2_DBG(" num_encoders=%d\n", hm2->config.num_encoders); HM2_DBG(" num_pwmgens=%d\n", hm2->config.num_pwmgens); + HM2_DBG(" num_3pwmgens=%d\n", hm2->config.num_tp_pwmgens); HM2_DBG(" num_stepgens=%d\n", hm2->config.num_stepgens); HM2_DBG(" enable_raw=%d\n", hm2->config.enable_raw); HM2_DBG(" firmware=%s\n", hm2->config.firmware ? hm2->config.firmware : "(NULL)"); @@ -621,6 +630,10 @@ static int hm2_parse_module_descriptors(hostmot2_t *hm2) { md_accepted = hm2_watchdog_parse_md(hm2, md_index); break; + case HM2_GTAG_TPPWM: + md_accepted = hm2_tp_pwmgen_parse_md(hm2, md_index); + break; + default: HM2_WARN( "MD %d: %dx %s v%d: ignored\n", @@ -673,7 +686,7 @@ static void hm2_cleanup(hostmot2_t *hm2) { hm2_encoder_cleanup(hm2); hm2_watchdog_cleanup(hm2); hm2_pwmgen_cleanup(hm2); - + hm2_tp_pwmgen_cleanup(hm2); // free all the tram entries hm2_tram_cleanup(hm2); } @@ -684,6 +697,7 @@ static void hm2_cleanup(hostmot2_t *hm2) { void hm2_print_modules(hostmot2_t *hm2) { hm2_encoder_print_module(hm2); hm2_pwmgen_print_module(hm2); + hm2_tp_pwmgen_print_module(hm2); hm2_stepgen_print_module(hm2); hm2_ioport_print_module(hm2); hm2_watchdog_print_module(hm2); @@ -1280,8 +1294,6 @@ void rtapi_app_exit(void) { } - - // this pushes our idea of what things are like into the FPGA's poor little mind void hm2_force_write(hostmot2_t *hm2) { hm2_watchdog_force_write(hm2); @@ -1289,5 +1301,6 @@ void hm2_force_write(hostmot2_t *hm2) { hm2_encoder_force_write(hm2); hm2_pwmgen_force_write(hm2); hm2_stepgen_force_write(hm2); + hm2_tp_pwmgen_force_write(hm2); } diff --git a/src/hal/drivers/mesa-hostmot2/hostmot2.h b/src/hal/drivers/mesa-hostmot2/hostmot2.h index 1a1ac18..a70d7df 100644 --- a/src/hal/drivers/mesa-hostmot2/hostmot2.h +++ b/src/hal/drivers/mesa-hostmot2/hostmot2.h @@ -100,7 +100,7 @@ char **argv_split(gfp_t gfp, const char *str, int *argcp); #define HM2_GTAG_STEPGEN (5) #define HM2_GTAG_PWMGEN (6) #define HM2_GTAG_TRANSLATIONRAM (11) - +#define HM2_GTAG_TPPWM (19) @@ -390,6 +390,73 @@ typedef struct { } hm2_pwmgen_t; +// +// 3-Phase pwmgen +// + + +typedef struct { + + struct { + + struct { + hal_float_t *Avalue; + hal_float_t *Bvalue; + hal_float_t *Cvalue; + hal_bit_t *fault; + hal_bit_t *enable; + } pin; + + struct { + hal_float_t scale; + hal_float_t deadzone; + hal_bit_t faultpolarity; + hal_float_t sampletime; + } param; + + } hal; + + // these keep track of the written values of each register so we know if an update-write is needed + // enable is a little more complicated and is based on the read-back of the fault/enable register + float written_deadzone; + bool written_faultpolarity; + float written_sampletime; + +} hm2_tp_pwmgen_instance_t; + +typedef struct { + struct { + hal_u32_t pwm_frequency; // One PWM rate for all instances + } param; +} hm2_tp_pwmgen_global_hal_t; + +typedef struct { + int num_instances; + + hm2_tp_pwmgen_instance_t *instance; + + hm2_tp_pwmgen_global_hal_t *hal; + + u32 clock_frequency; + u8 version; + + // these keep track of the most recent hal->param.p{d,w}m_frequency + // that we've told the FPGA about, so we know if we need to update it + u32 written_pwm_frequency; + + u32 pwm_value_addr; // All three phases share a register (10 bits each) + u32 *pwm_value_reg; // Pointer to a memory block that holds the set. + + u32 setup_addr; // holds dead-time, fault polarity and ADC sample time + u32 *setup_reg; + + u32 enable_addr; // a 32-bit enable register for each tp_pwmgen. Seems excessive + u32 *enable_reg; + + u32 pwmgen_master_rate_dds_addr; + u32 pwmgen_master_rate_dds_reg; // one register for the whole Function + +} hm2_tp_pwmgen_t; // @@ -568,7 +635,6 @@ typedef struct { - // // raw peek/poke access // @@ -615,6 +681,7 @@ typedef struct { struct { int num_encoders; int num_pwmgens; + int num_tp_pwmgens; int num_stepgens; int enable_raw; char *firmware; @@ -643,10 +710,10 @@ typedef struct { // the hostmot2 "Functions" hm2_encoder_t encoder; hm2_pwmgen_t pwmgen; + hm2_tp_pwmgen_t tp_pwmgen; hm2_stepgen_t stepgen; hm2_ioport_t ioport; hm2_watchdog_t watchdog; - hm2_raw_t *raw; struct list_head list; @@ -765,6 +832,17 @@ void hm2_pwmgen_force_write(hostmot2_t *hm2); void hm2_pwmgen_prepare_tram_write(hostmot2_t *hm2); +// +// Three Phase pwmgen functions +// + +int hm2_tp_pwmgen_parse_md(hostmot2_t *hm2, int md_index); +void hm2_tp_pwmgen_print_module(hostmot2_t *hm2); +void hm2_tp_pwmgen_cleanup(hostmot2_t *hm2); +void hm2_tp_pwmgen_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_force_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_prepare_tram_write(hostmot2_t *hm2); +void hm2_tp_pwmgen_read(hostmot2_t *hm2); // @@ -793,9 +871,6 @@ void hm2_watchdog_cleanup(hostmot2_t *hm2); void hm2_watchdog_write(hostmot2_t *hm2); void hm2_watchdog_force_write(hostmot2_t *hm2); - - - // // the raw interface lets you peek and poke the hostmot2 instance from HAL // diff --git a/src/hal/drivers/mesa-hostmot2/pins.c b/src/hal/drivers/mesa-hostmot2/pins.c index 6bff378..4bcfef1 100644 --- a/src/hal/drivers/mesa-hostmot2/pins.c +++ b/src/hal/drivers/mesa-hostmot2/pins.c @@ -64,6 +64,19 @@ static const char* hm2_get_pin_secondary_name(hm2_pin_t *pin) { } break; + case HM2_GTAG_TPPWM: + switch (sec_pin) { + case 1: return "PWM A"; + case 2: return "PWM B"; + case 3: return "PWM C"; + case 4: return "PWM /A"; + case 5: return "PWM /B"; + case 6: return "PWM /C"; + case 7: return "Enable"; + case 8: return "Fault"; + } + break; + case HM2_GTAG_STEPGEN: // FIXME: these depend on the stepgen mode switch (sec_pin) { @@ -335,6 +348,7 @@ void hm2_configure_pins(hostmot2_t *hm2) { // encoder and pwmgen just get all their enabled instances' pins hm2_pins_allocate_all(hm2, HM2_GTAG_ENCODER, hm2->encoder.num_instances); hm2_pins_allocate_all(hm2, HM2_GTAG_PWMGEN, hm2->pwmgen.num_instances); + hm2_pins_allocate_all(hm2, HM2_GTAG_TPPWM, hm2->tp_pwmgen.num_instances); } diff --git a/src/hal/drivers/mesa-hostmot2/pwmgen.c b/src/hal/drivers/mesa-hostmot2/pwmgen.c index 338697b..bc295f2 100644 diff --git a/src/hal/drivers/mesa-hostmot2/tp_pwmgen.c b/src/hal/drivers/mesa-hostmot2/tp_pwmgen.c new file mode 100644 index 0000000..817e64b --- /dev/null +++ b/src/hal/drivers/mesa-hostmot2/tp_pwmgen.c @@ -0,0 +1,491 @@ +// +// Copyright (C) 2010 Andy Pugh +// Heavily based on pwmgen.c (C) Sebastian Kuzminsky + +// 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, write to the Free Software +// Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA +// + +#include + +#include "rtapi.h" +#include "rtapi_app.h" +#include "rtapi_string.h" +#include "rtapi_math.h" + +#include "hal.h" + +#include "hal/drivers/mesa-hostmot2/hostmot2.h" + +const float FLOAT_EPS = 3E-7; + +void hm2_tp_pwmgen_handle_pwm_frequency(hostmot2_t *hm2) { + u32 dds; + int deadtime; + int i; + + if (hm2->tp_pwmgen.hal->param.pwm_frequency < 1) { + HM2_ERR("3pwmgen.pwm_frequency %d is too low, setting to 1\n", hm2->tp_pwmgen.hal->param.pwm_frequency); + hm2->tp_pwmgen.hal->param.pwm_frequency = 1; + } + + + // + // pwm_frequency is the user's desired PWM frequency in Hz + // + // PWMClock is controlled by the 16-bit PWM Master Rate DDS Register: + // PWMClock = ClockHigh * DDS / 65536 + // + // Counter is fixed at 11 bits wide for 3-Phase PWM. + + // The key equation is: + // PWMFreq = PWMClock / 2048 + + // Solve for DDS: + // PWMFreq * (65536 * 2048) = ClockHigh * DDS + // DDS = (PWMFreq * 65536 * 2048) / ClockHigh + // + + // check that the frequency is achievable + dds = ((double)hm2->tp_pwmgen.hal->param.pwm_frequency * 65536.0 * 2048.0) / (double)hm2->tp_pwmgen.clock_frequency; + if (dds < 65536) { + hm2->tp_pwmgen.pwmgen_master_rate_dds_reg = dds; + } + else{ + // not possible so try the fastest we can + // PWMFreq = (ClockHigh * DDS) / (65536 * 2048) + dds = 65535; + hm2->tp_pwmgen.hal->param.pwm_frequency = ((double)hm2->tp_pwmgen.clock_frequency * 65535.0) / (65536.0 * 2048.0); + HM2_ERR("max PWM frequency is %d Hz\n", hm2->tp_pwmgen.hal->param.pwm_frequency); + hm2->tp_pwmgen.pwmgen_master_rate_dds_reg = dds; + } + + // Now recalculate the deadtime. + // This is specified in nS in the pin but 2/TPPWMCLK in the register. + // = (2*65536*1e9 / ClockHigh*dds)nS + + for (i =0 ; i < hm2->tp_pwmgen.num_instances ;i++) { + if (hm2->tp_pwmgen.instance[i].hal.param.sampletime > 1){ + HM2_ERR("Max sampletime is 1 (end of PWM cycle"); + hm2->tp_pwmgen.instance[i].hal.param.sampletime = 1; + } + else if (hm2->tp_pwmgen.instance[i].hal.param.sampletime < 0){ + HM2_ERR("Min sampletime is 0 (beginning of PWM cycle"); + hm2->tp_pwmgen.instance[i].hal.param.sampletime = 0; + } + + deadtime = (hm2->tp_pwmgen.instance[i].hal.param.deadzone * hm2->tp_pwmgen.clock_frequency * dds) / (2 * 65536 * 1e9); + + if (deadtime > 511){ + deadtime = 511; + hm2->tp_pwmgen.instance[i].hal.param.deadzone = ((float)deadtime * 2.0 * 65536.0 * 1.0e9)/((float)hm2->tp_pwmgen.clock_frequency * dds); + HM2_ERR("At this PWM frequency the maximum deadtime is %dnS\n", (int)hm2->tp_pwmgen.instance[i].hal.param.deadzone); + } + else if (deadtime < 0) { + HM2_ERR("Deadtime must be positive"); + deadtime = 0; + hm2->tp_pwmgen.instance[i].hal.param.deadzone = 0; + } + + // Now setup the control register + + hm2->tp_pwmgen.setup_reg[i] = ( + ((int)(hm2->tp_pwmgen.instance[i].hal.param.sampletime*1023) << 16) + +(hm2->tp_pwmgen.instance[i].hal.param.faultpolarity << 15) + +(deadtime) + ); + } +} + + +void hm2_tp_pwmgen_force_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances == 0) return; + + hm2_tp_pwmgen_handle_pwm_frequency(hm2); // This sets the pwmgen_dds register and also the deadtime/sample time register + + // update enable and setup registers + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + if (*hm2->tp_pwmgen.instance[i].hal.pin.enable) { + hm2->tp_pwmgen.enable_reg[i] = 1; + } + else hm2->tp_pwmgen.enable_reg[i] = 0; + } + //write out the values to their registers + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.setup_addr, hm2->tp_pwmgen.setup_reg, (hm2->tp_pwmgen.num_instances * sizeof(u32))); + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.enable_addr, hm2->tp_pwmgen.enable_reg, (hm2->tp_pwmgen.num_instances * sizeof(u32))); + hm2->llio->write(hm2->llio, hm2->tp_pwmgen.pwmgen_master_rate_dds_addr, &hm2->tp_pwmgen.pwmgen_master_rate_dds_reg, sizeof(u32)); + + if ((*hm2->llio->io_error) != 0) return; + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + hm2->tp_pwmgen.instance[i].written_faultpolarity = hm2->tp_pwmgen.instance[i].hal.param.faultpolarity; + hm2->tp_pwmgen.instance[i].written_deadzone = hm2->tp_pwmgen.instance[i].hal.param.deadzone; + hm2->tp_pwmgen.instance[i].written_sampletime = hm2->tp_pwmgen.instance[i].hal.param.sampletime; + } + + hm2->tp_pwmgen.written_pwm_frequency = hm2->tp_pwmgen.hal->param.pwm_frequency; +} + + +// +// Update the Registers of all Three Phase pwmgen instances that need it +// + +void hm2_tp_pwmgen_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances == 0) return; + + // check pwm frequency + if (hm2->tp_pwmgen.hal->param.pwm_frequency != hm2->tp_pwmgen.written_pwm_frequency) goto force_write; + + // update enable register? + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + if ( + (hm2->tp_pwmgen.instance[i].hal.param.deadzone != hm2->tp_pwmgen.instance[i].written_deadzone) + || (hm2->tp_pwmgen.instance[i].hal.param.sampletime != hm2->tp_pwmgen.instance[i].written_sampletime) + || (hm2->tp_pwmgen.instance[i].hal.param.faultpolarity != hm2->tp_pwmgen.instance[i].written_faultpolarity) + ) + { + goto force_write; + } + + // The enable register is a little odd. Writing a 1 to bit0 of the register is a request to enable, + // however the state of the bit, and the physical amp-enable pin, depends on the fault status. + // So rather than check with a "written_enable" variable we attempt to enable if + // there is a mismatch between the hal-pin and the enable bit. + + if ( + (*hm2->tp_pwmgen.instance[i].hal.pin.enable != (hm2->tp_pwmgen.enable_reg[i] & 1)) + ) + { + goto force_write; + } + } + + return; + +force_write: + hm2_tp_pwmgen_force_write(hm2); +} + +// +// Read the fault bit for each instance +// +void hm2_tp_pwmgen_read(hostmot2_t *hm2) { + int i; + hm2->llio->read(hm2->llio, hm2->tp_pwmgen.enable_addr, hm2->tp_pwmgen.enable_reg, (hm2->tp_pwmgen.num_instances * sizeof(u32))); + for (i = 0 ; i < hm2->tp_pwmgen.num_instances ; i++) { + *hm2->tp_pwmgen.instance[i].hal.pin.fault = (hm2->tp_pwmgen.enable_reg[i] & 2); + } +} + + +int hm2_tp_pwmgen_parse_md(hostmot2_t *hm2, int md_index) { + hm2_module_descriptor_t *md = &hm2->md[md_index]; + int r; + + + // + // some standard sanity checks + // + + if (!hm2_md_is_consistent_or_complain(hm2, md_index, 0, 4, 4, 0x0003)) { + HM2_ERR("inconsistent Module Descriptor!\n"); + return -EINVAL; + } + + if (hm2->tp_pwmgen.num_instances != 0) { + HM2_ERR( + "found duplicate Module Descriptor for %s (inconsistent firmware), not loading driver\n", + hm2_get_general_function_name(md->gtag) + ); + return -EINVAL; + } + + if (hm2->config.num_tp_pwmgens > md->instances) { + HM2_ERR( + "config.num_3pwmgens=%d, but only %d are available, not loading driver\n", + hm2->config.num_tp_pwmgens, + md->instances + ); + return -EINVAL; + } + + if (hm2->config.num_tp_pwmgens == 0) { + return 0; + } + + // + // looks good, start initializing + // + + + if (hm2->config.num_tp_pwmgens == -1) { + hm2->tp_pwmgen.num_instances = md->instances; + } else { + hm2->tp_pwmgen.num_instances = hm2->config.num_tp_pwmgens; + } + + + // allocate the module-global HAL shared memory + + hm2->tp_pwmgen.instance = (hm2_tp_pwmgen_instance_t *)hal_malloc(hm2->tp_pwmgen.num_instances * sizeof(hm2_tp_pwmgen_instance_t)); + if (hm2->tp_pwmgen.instance == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + + hm2->tp_pwmgen.hal = (hm2_tp_pwmgen_global_hal_t *)hal_malloc(sizeof(hm2_tp_pwmgen_global_hal_t)); + if (hm2->tp_pwmgen.instance == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail0; + } + + + hm2->tp_pwmgen.clock_frequency = md->clock_freq; + hm2->tp_pwmgen.version = md->version; + + hm2->tp_pwmgen.pwm_value_addr = md->base_address + (0 * md->register_stride); + hm2->tp_pwmgen.pwmgen_master_rate_dds_addr = md->base_address + (3 * md->register_stride); + hm2->tp_pwmgen.enable_addr = md->base_address + (1 * md->register_stride); + hm2->tp_pwmgen.setup_addr = md->base_address + (2 * md->register_stride); + + //Allocate some memory for the parameter registers. The value equivalent is handled in the tram section + hm2->tp_pwmgen.setup_reg = (u32 *)kmalloc(hm2->tp_pwmgen.num_instances * sizeof(u32), GFP_KERNEL); + if (hm2->tp_pwmgen.setup_reg == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail1; + } + hm2->tp_pwmgen.enable_reg = (u32 *)kmalloc(hm2->tp_pwmgen.num_instances * sizeof(u32), GFP_KERNEL); + if (hm2->tp_pwmgen.enable_reg == NULL) { + HM2_ERR("out of memory!\n"); + r = -ENOMEM; + goto fail2; + } + + // Register the PWM values with the TRAM + r = hm2_register_tram_write_region(hm2, hm2->tp_pwmgen.pwm_value_addr, (hm2->tp_pwmgen.num_instances * sizeof(u32)), &hm2->tp_pwmgen.pwm_value_reg); + if (r < 0) { + HM2_ERR("error registering tram write region for 3PWM Value register (%d)\n", r); + goto fail2; + } + + // export to HAL + // FIXME: r hides the r in enclosing function, and it returns the wrong thing + { + int i; + int r; + char name[HAL_NAME_LEN + 2]; + + // this hal parameter affects all the 3-Phase pwmgen instances + r = hal_param_u32_newf( + HAL_RW, + &(hm2->tp_pwmgen.hal->param.pwm_frequency), + hm2->llio->comp_id, + "%s.3pwmgen.frequency", + hm2->llio->name + ); + if (r < 0) { + HM2_ERR("error adding pin %s.3pwmgen.frequency param, aborting\n", hm2->llio->name); + goto fail2; + } + + hm2->tp_pwmgen.hal->param.pwm_frequency = 20000; + hm2->tp_pwmgen.written_pwm_frequency = 0; // force a write + + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + // pins + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.A-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Avalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.B-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Bvalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.C-value", hm2->llio->name, i); + r = hal_pin_float_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.Cvalue), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.enable", hm2->llio->name, i); + r = hal_pin_bit_new(name, HAL_IN, &(hm2->tp_pwmgen.instance[i].hal.pin.enable), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.fault", hm2->llio->name, i); + r = hal_pin_bit_new(name, HAL_OUT, &(hm2->tp_pwmgen.instance[i].hal.pin.fault), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding pin '%s', aborting\n", name); + goto fail2; + } + + // parameters + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.scale", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.scale), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.deadtime", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.deadzone), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.fault-invert", hm2->llio->name, i); + r = hal_param_bit_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.faultpolarity), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail2; + } + + rtapi_snprintf(name, HAL_NAME_LEN, "%s.3pwmgen.%02d.sample-time", hm2->llio->name, i); + r = hal_param_float_new(name, HAL_RW, &(hm2->tp_pwmgen.instance[i].hal.param.sampletime), hm2->llio->comp_id); + if (r < 0) { + HM2_ERR("error adding param '%s', aborting\n", name); + goto fail2; + } + + + // init hal objects + *(hm2->tp_pwmgen.instance[i].hal.pin.enable) = 0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Avalue) = 0.0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Bvalue) = 0.0; + *(hm2->tp_pwmgen.instance[i].hal.pin.Cvalue) = 0.0; + + hm2->tp_pwmgen.instance[i].hal.param.scale = 1.0; + hm2->tp_pwmgen.instance[i].hal.param.sampletime = 0.5; + hm2->tp_pwmgen.instance[i].hal.param.faultpolarity = 0.0; //active low + hm2->tp_pwmgen.instance[i].hal.param.deadzone = 5000; // 5000nS conservative deadzone for safety + + hm2->tp_pwmgen.instance[i].written_sampletime = -666; // force an update at the start + } + } + + return hm2->tp_pwmgen.num_instances; + +fail2: + kfree(hm2->tp_pwmgen.enable_reg); + +fail1: + kfree(hm2->tp_pwmgen.setup_reg); + +fail0: + hm2->tp_pwmgen.num_instances = 0; + return r; + +} + +void hm2_tp_pwmgen_print_module(hostmot2_t *hm2) { + int i; + HM2_PRINT("3-phase PWMGen: %d\n", hm2->tp_pwmgen.num_instances); + if (hm2->tp_pwmgen.num_instances <= 0) return; + HM2_PRINT(" clock_frequency: %d Hz (%s MHz)\n", hm2->tp_pwmgen.clock_frequency, hm2_hz_to_mhz(hm2->tp_pwmgen.clock_frequency)); + HM2_PRINT(" version: %d\n", hm2->tp_pwmgen.version); + HM2_PRINT(" pwmgen_master_rate_dds: 0x%08X (%d)\n", hm2->tp_pwmgen.pwmgen_master_rate_dds_reg, hm2->tp_pwmgen.pwmgen_master_rate_dds_reg); + HM2_PRINT(" pwmgen_master_rate_dds_addr: 0x%04X\n", hm2->tp_pwmgen.pwmgen_master_rate_dds_addr); + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + HM2_PRINT(" instance %d:\n", i); + HM2_PRINT(" hw:\n"); + HM2_PRINT(" pwm_value_addr: 0x%04X\n", hm2->tp_pwmgen.pwm_value_addr); + HM2_PRINT(" pwm_val:0x%08X\n", hm2->tp_pwmgen.pwm_value_reg[i]); + HM2_PRINT(" enable: 0x%08X\n", hm2->tp_pwmgen.enable_reg[i]); + HM2_PRINT(" setup: 0x%08X\n", hm2->tp_pwmgen.setup_reg[i]); + } +} + + +void hm2_tp_pwmgen_prepare_tram_write(hostmot2_t *hm2) { + int i; + + if (hm2->tp_pwmgen.num_instances <= 0) return; + + for (i = 0; i < hm2->tp_pwmgen.num_instances; i ++) { + double scaled_Avalue; + double scaled_Bvalue; + double scaled_Cvalue; + + // When disabled the 3pwmgen sets both sets of drivers to zero. No need to handle that here + // (In contrast to the conventional pwmgen) + + // Check for /0 problems + if (hm2->tp_pwmgen.instance[i].hal.param.scale ==0) { + hm2->tp_pwmgen.instance[i].hal.param.scale = 1; + HM2_ERR("3pwmgen scale must be greater than zero. Scale set to %i", (int)hm2->tp_pwmgen.instance[i].hal.param.scale); + } + + scaled_Avalue = (*hm2->tp_pwmgen.instance[i].hal.pin.Avalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + scaled_Bvalue = (*hm2->tp_pwmgen.instance[i].hal.pin.Bvalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + scaled_Cvalue = (*hm2->tp_pwmgen.instance[i].hal.pin.Cvalue / hm2->tp_pwmgen.instance[i].hal.param.scale); + + if (scaled_Avalue > 1.0) scaled_Avalue = 1.0; + else if (scaled_Avalue < -1.0) scaled_Avalue = -1.0; + if (scaled_Bvalue > 1.0) scaled_Bvalue = 1.0; + else if (scaled_Avalue < -1.0) scaled_Avalue = -1.0; + if (scaled_Cvalue > 1.0) scaled_Cvalue = 1.0; + else if (scaled_Avalue < -1.0) scaled_Avalue = -1.0; + + // duty_cycle goes from 0.0 to 1.0, and needs to be puffed out to 10 bits + + hm2->tp_pwmgen.pwm_value_reg[i] = ( + ((long)(scaled_Cvalue * (511.0) + 512.0) << 20) + + ((long)(scaled_Bvalue * (511.0) + 512.0) << 10) + + ((long)(scaled_Avalue * (511.0) + 512.0) << 0) + ); + + } + +} + + +void hm2_tp_pwmgen_cleanup(hostmot2_t *hm2) { + if (hm2->tp_pwmgen.num_instances <= 0) return; + if (hm2->tp_pwmgen.setup_reg != NULL) { + kfree(hm2->tp_pwmgen.setup_reg); + hm2->tp_pwmgen.enable_reg = NULL; + } + if (hm2->tp_pwmgen.enable_reg != NULL) { + kfree(hm2->tp_pwmgen.enable_reg); + hm2->tp_pwmgen.enable_reg = NULL; + } + if (hm2->pwmgen.instance != NULL) { + kfree(hm2->pwmgen.instance); + hm2->pwmgen.instance = NULL; + } + hm2->tp_pwmgen.num_instances = 0; +} +