pwmgen.c

CNC 的开放码,EMC2 V2.2.8版

	    break;
	case PWM_PDM:
	    /* add desired high time to running total */
	    pwmgen->high_timer += pwmgen->high_time;
	    if ( pwmgen->curr_output ) {
		/* current state is high, subtract actual high time */
		pwmgen->high_timer -= periodns;
	    }
	    if ( pwmgen->high_timer > 0 ) {
		pwmgen->curr_output = 1;
	    } else {
		pwmgen->curr_output = 0;
	    }
	    break;
	case PWM_DISABLED:
	default:
	    /* disabled, drive output off and zero accumulator */
	    pwmgen->curr_output = 0;
	    pwmgen->high_timer = 0;
	    pwmgen->period_timer = 0;
	    break;
	}
	/* generate output, based on output type */
	if (pwmgen->output_type < 2) {
	    /* PWM (and maybe DIR) output */
	    /* DIR is set by update(), we only do PWM */
	    *(pwmgen->out[PWM_PIN]) = pwmgen->curr_output;
	} else {
	    /* UP and DOWN output */
	    *(pwmgen->out[UP_PIN]) = pwmgen->curr_output & ~pwmgen->direction;
	    *(pwmgen->out[DOWN_PIN]) = pwmgen->curr_output & pwmgen->direction;
	}
	/* move on to next PWM generator */
	pwmgen++;
    }
    /* done */
}

static void update(void *arg, long period)
{
    pwmgen_t *pwmgen;
    int n, high_periods;
    unsigned char new_pwm_mode;
    float tmpdc, outdc;

    /* update the PWM generators */
    pwmgen = arg;
    for (n = 0; n < num_chan; n++) {

	/* validate duty cycle limits, both limits must be between
	   0.0 and 1.0 (inclusive) and max must be greater then min */
	if ( pwmgen->max_dc > 1.0 ) {
	    pwmgen->max_dc = 1.0;
	}
	if ( pwmgen->min_dc > pwmgen->max_dc ) {
	    pwmgen->min_dc = pwmgen->max_dc;
	}
	if ( pwmgen->min_dc < 0.0 ) {
	    pwmgen->min_dc = 0.0;
	}
	if ( pwmgen->max_dc < pwmgen->min_dc ) {
	    pwmgen->max_dc = pwmgen->min_dc;
	}
	/* do scale calcs only when scale changes */
	if ( pwmgen->scale != pwmgen->old_scale ) {
	    /* get ready to detect future scale changes */
	    pwmgen->old_scale = pwmgen->scale;
	    /* validate the new scale value */
	    if ((pwmgen->scale < 1e-20)
		&& (pwmgen->scale > -1e-20)) {
		/* value too small, divide by zero is a bad thing */
		pwmgen->scale = 1.0;
	    }
	    /* we will need the reciprocal */
	    pwmgen->scale_recip = 1.0 / pwmgen->scale;
	}
	if ( *(pwmgen->enable) == 0 ) {
	    new_pwm_mode = PWM_DISABLED;
	} else if ( pwmgen->pwm_freq == 0 ) {
	    new_pwm_mode = PWM_PDM;
	} else if ( pwmgen->dither_pwm != 0 ) {
	    new_pwm_mode = PWM_DITHER;
	} else {
	    new_pwm_mode = PWM_PURE;
	}
	/* force recalc if max_freq is changed */
	if ( pwmgen->pwm_freq != pwmgen->old_pwm_freq ) {
	    pwmgen->pwm_mode = PWM_DISABLED;
	}
	/* do the period calcs only when mode or pwm_freq changes */
	if ( pwmgen->pwm_mode != new_pwm_mode ) {
	    /* disable output during calcs */
	    pwmgen->pwm_mode = PWM_DISABLED;
	    /* validate max_freq */
	    if ( pwmgen->pwm_freq <= 0.0 ) {
		/* zero or negative means PDM mode */
		pwmgen->pwm_freq = 0.0;
		pwmgen->period = periodns;
	    } else {
		/* positive means PWM mode */
		if ( pwmgen->pwm_freq < 0.5 ) {
 		    /* min freq is 0.5 Hz (2 billion nsec period) */
		    pwmgen->pwm_freq = 0.5;
		} else if ( pwmgen->pwm_freq > ((1e9/2.0) / periodns) ) {
		    /* max freq is 2 base periods */
		    pwmgen->pwm_freq = (1e9/2.0) / periodns;
		}
		if ( new_pwm_mode == PWM_PURE ) {
		    /* period must be integral multiple of periodns */
		    pwmgen->periods = (( 1e9 / pwmgen->pwm_freq ) / periodns ) + 0.5;
		    pwmgen->periods_recip = 1.0 / pwmgen->periods;
		    pwmgen->period = pwmgen->periods * periodns;
		    /* actual max freq after rounding */
		    pwmgen->pwm_freq = 1.0e9 / pwmgen->period;
		} else {
		    pwmgen->period = 1.0e9 / pwmgen->pwm_freq;
		}
	    }
	    /* save freq to detect changes */
	    pwmgen->old_pwm_freq = pwmgen->pwm_freq;
	}
	/* convert value command to duty cycle */
	tmpdc = *(pwmgen->value) * pwmgen->scale_recip + pwmgen->offset;
	if ( pwmgen->output_type == 0 ) {
	    /* unidirectional mode, no negative output */
	    if ( tmpdc < 0.0 ) {
		tmpdc = 0.0;
	    }
	}
	/* limit the duty cycle */
	if (tmpdc >= 0.0) {
	    if ( tmpdc > pwmgen->max_dc ) {
		tmpdc = pwmgen->max_dc;
	    } else if ( tmpdc < pwmgen->min_dc ) {
		tmpdc = pwmgen->min_dc;
	    }
	    pwmgen->direction = 0;
	    outdc = tmpdc;
	} else {
	    if ( tmpdc < -pwmgen->max_dc ) {
		tmpdc = -pwmgen->max_dc;
	    } else if ( tmpdc > -pwmgen->min_dc ) {
		tmpdc = -pwmgen->min_dc;
	    }
	    pwmgen->direction = 1;
	    outdc = -tmpdc;
	}
	if ( new_pwm_mode == PWM_PURE ) {
	    /* round to nearest pure PWM duty cycle */
	    high_periods = (pwmgen->periods * outdc) + 0.5;
	    pwmgen->high_time = high_periods * periodns;
	    /* save rounded value to curr_dc param */
	    if ( tmpdc >= 0 ) {
		pwmgen->curr_dc = high_periods * pwmgen->periods_recip;
	    } else {
		pwmgen->curr_dc = -high_periods * pwmgen->periods_recip;
	    }
	} else {
	    pwmgen->high_time = ( pwmgen->period * outdc ) + 0.5;
	    /* save duty cycle to curr_dc param */
	    pwmgen->curr_dc = tmpdc;
	}
	/* if using PWM/DIR outputs, set DIR pin */
	if ( pwmgen->output_type == 1 ) {
	    *(pwmgen->out[DIR_PIN]) = pwmgen->direction;
	}
	/* save new mode */
	pwmgen->pwm_mode = new_pwm_mode;
	/* move on to next channel */
	pwmgen++;
    }
    /* done */
}


/***********************************************************************
*                   LOCAL FUNCTION DEFINITIONS                         *
************************************************************************/

static int export_pwmgen(int num, pwmgen_t * addr, int output_type)
{
    int retval, msg;

    /* This function exports a lot of stuff, which results in a lot of
       logging if msg_level is at INFO or ALL. So we save the current value
       of msg_level and restore it later.  If you actually need to log this
       function's actions, change the second line below */
    msg = rtapi_get_msg_level();
    rtapi_set_msg_level(RTAPI_MSG_WARN);

    /* export paramameters */
    retval = hal_param_float_newf(HAL_RW, &(addr->scale), comp_id,
	    "pwmgen.%d.scale", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_float_newf(HAL_RW, &(addr->offset), comp_id,
	    "pwmgen.%d.offset", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_bit_newf(HAL_RW, &(addr->dither_pwm), comp_id,
	    "pwmgen.%d.dither-pwm", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_float_newf(HAL_RW, &(addr->pwm_freq), comp_id,
	    "pwmgen.%d.pwm-freq", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_float_newf(HAL_RW, &(addr->min_dc), comp_id,
	    "pwmgen.%d.min-dc", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_float_newf(HAL_RW, &(addr->max_dc), comp_id,
	    "pwmgen.%d.max-dc", num);
    if (retval != 0) {
	return retval;
    }
    retval = hal_param_float_newf(HAL_RO, &(addr->curr_dc), comp_id,
	    "pwmgen.%d.curr-dc", num);
    if (retval != 0) {
	return retval;
    }
    /* export pins */
    retval = hal_pin_bit_newf(HAL_IN, &(addr->enable), comp_id,
	    "pwmgen.%d.enable", num);
    if (retval != 0) {
	return retval;
    }
    *(addr->enable) = 0;
    retval = hal_pin_float_newf(HAL_IN, &(addr->value), comp_id,
	    "pwmgen.%d.value", num);
    if (retval != 0) {
	return retval;
    }
    *(addr->value) = 0.0;
    if (output_type == 2) {
	/* export UP/DOWN pins */
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[UP_PIN]), comp_id,
		"pwmgen.%d.up", num);
	if (retval != 0) {
	    return retval;
	}
	/* init the pin */
	*(addr->out[UP_PIN]) = 0;
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DOWN_PIN]), comp_id,
		"pwmgen.%d.down", num);
	if (retval != 0) {
	    return retval;
	}
	/* init the pin */
	*(addr->out[DOWN_PIN]) = 0;
    } else {
	/* export PWM pin */
	retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[PWM_PIN]), comp_id,
		"pwmgen.%d.pwm", num);
	if (retval != 0) {
	    return retval;
	}
	/* init the pin */
	*(addr->out[PWM_PIN]) = 0;
	if ( output_type == 1 ) {
	    /* export DIR pin */
	    retval = hal_pin_bit_newf(HAL_OUT, &(addr->out[DIR_PIN]), comp_id,
		    "pwmgen.%d.dir", num);
	    if (retval != 0) {
		return retval;
	    }
	    /* init the pin */
	    *(addr->out[DIR_PIN]) = 0;
	}
    }
    /* set default parameter values */
    addr->scale = 1.0;
    addr->offset = 0.0;
    addr->dither_pwm = 0;
    addr->pwm_freq = 0;
    addr->min_dc = 0.0;
    addr->max_dc = 1.0;
    /* init other fields */
    addr->period = 50000;
    addr->high_time = 0;
    addr->period_timer = 0;
    addr->high_timer = 0;
    addr->curr_output = 0;
    addr->output_type = output_type;
    addr->pwm_mode = PWM_DISABLED;
    addr->direction = 0;
    addr->old_scale = addr->scale + 1.0;
    addr->old_pwm_freq = -1;
    addr->curr_dc = 0.0;
    /* restore saved message level */
    rtapi_set_msg_level(msg);
    return 0;
}