void Scan_Samples(void){
fixed ybox[7]; // array box car derivative
s16 bin; // index into peak height array
fixed dt, tau, *yp0, *yp1, y, y_i;
fixed threshold; // trigger level for leading edge
fixed deriv;
fixed alpha, beta, gamma, peak;
s16 i;
configurations *cp = &configuration;
ADC_Stop();
tail = head = Get_Scan_Pos(); // get current absolute position in adc buffer
/*
if(cp->sig_filter){
// Set up Butterworth low pass filter
dt = FIXED_HALF; // sample time 0.5 uS
tau = fixed_from_int(cp->sig_filter); // filter time in uS
alpha = fixed_div(dt, tau + dt);
}
*/
ADC_Start();
while(TRUE){
if(tail == head){
head = Get_Scan_Pos();
// recalculate filter elements in case changed
/*
if(cp->sig_filter){
// Set up Butterworth low pass filter in case of changes
dt = FIXED_HALF; // sample time 0.5 uS
tau = fixed_from_int(cp->sig_filter); // filter time in uS
alpha = fixed_div(dt, tau + dt);
}
*/
}
if(tail == head)
continue;
// track live time
if(samp_cntr++ >= SAMP_PER_MS){
live_time++;
samp_cntr = 0;
}
// get new value, adjust for zero and inversion at same time
y = fixed_from_int(zero - scan_buffer[tail++]);
if(tail >= SCAN_BUFFER_SZ){
tail = 0;
}
// filter signal if needed
/*
if(cp->sig_filter){
// Butterworth low pass filter
y = *yp0 + fixed_mul(alpha, (y - *yp0));
}
*/
// shift the boxcar window and find derivative
yp0 =&ybox[0];
yp1 = &ybox[1];
for(i = 6; i > 0; i--){ // last box slot gets new y value
*yp0++ = *yp1++;
}
*yp0 = y; // place latest sample in end of boxcar
// compute the derivative
deriv = 0;
yp0 =&ybox[0];
deriv -= *yp0++;
deriv -= *yp0++;
alpha = *yp0;
deriv -= *yp0++;
beta = *yp0++;
gamma = *yp0;
deriv += *yp0++;
deriv += *yp0++;
deriv += *yp0++;
// process depending on state
switch(scan_state){
case RESTART:
scan_state = LEADING;
//cp->scope_valid = FALSE;
break;
case LEADING:
if(cp->sig_type == PULSE_POS && deriv > cp->sig_dvdt_lim){
scan_state = PEAK;
break;
}
if(cp->sig_type == PULSE_NEG && deriv < cp->sig_dvdt_lim){
scan_state = PEAK;
break;
}
// if no pulse then check the zero
avg_zero = (avg_zero * 20 + y)/21;
break;
case PEAK:
// reverse derivative indicates peak
if(cp->sig_type == PULSE_POS && deriv < 0){
scan_state = TAIL;
}
if(cp->sig_type == PULSE_NEG && deriv > 0){
scan_state = TAIL;
}
if(scan_state == TAIL){
// handle gaussian approximation if enabled
if(cp->sig_gaussian){
// p = ((a - g)/(a -2b + g))/2 = position of peak
peak = (fixed_div((alpha - gamma),(alpha - (2 * beta) + gamma))) / 2;
// y(p) = b - ((a - g) * p)/4 = peak value
peak = beta - (fixed_mul((alpha - gamma), peak) / 4);
} else {
peak = (alpha + beta + gamma) / 3;
}
if(cp->sig_type == PULSE_NEG){
peak = -peak;
}
// peak now always positive
if( peak > cp->sig_lo_lim && peak < cp->sig_hi_lim){
bin = fixed_to_int(peak);
pulse_height[bin]++; // increment count in spectrum array
cur_cnt++;
// handle rate meter beeping
if(cp->rate_beep && !alarm_on){
Beep(BEEP_500Hz, 10);
}
}
}
break;
case TAIL:
// find where curve turns back to baseline
if(cp->sig_type == PULSE_POS && deriv >= 0){
scan_state = LEADING;
}
if(cp->sig_type == PULSE_NEG && deriv <= 0){
scan_state = LEADING;
}
break;
} // switch(scan_state)
} // while ring buffer not empty
}
void set_frequency(struct wave * const wave, fixed_t frequency)
{
wave->settings.frequency = frequency;
const fixed_t max_frequency = fixed_from_int(F_TASK_FAST/(2*WAVE_STEPS));
wave->state.speed_prescaler = fixed_to_int(fixed_div(max_frequency, wave->settings.frequency));
}
void
image_downsize_gd_fixed_point(image *im)
{
int x, y;
fixed_t sy1, sy2, sx1, sx2;
int dstX = 0, dstY = 0, srcX = 0, srcY = 0;
fixed_t width_scale, height_scale;
int dstW = im->target_width;
int dstH = im->target_height;
int srcW = im->width;
int srcH = im->height;
if (im->height_padding) {
dstY = im->height_padding;
dstH = im->height_inner;
}
if (im->width_padding) {
dstX = im->width_padding;
dstW = im->width_inner;
}
width_scale = fixed_div(int_to_fixed(srcW), int_to_fixed(dstW));
height_scale = fixed_div(int_to_fixed(srcH), int_to_fixed(dstH));
for (y = dstY; (y < dstY + dstH); y++) {
sy1 = fixed_mul(int_to_fixed(y - dstY), height_scale);
sy2 = fixed_mul(int_to_fixed((y + 1) - dstY), height_scale);
for (x = dstX; (x < dstX + dstW); x++) {
fixed_t sx, sy;
fixed_t spixels = 0;
fixed_t red = 0, green = 0, blue = 0, alpha = 0;
if (!im->has_alpha)
alpha = FIXED_255;
sx1 = fixed_mul(int_to_fixed(x - dstX), width_scale);
sx2 = fixed_mul(int_to_fixed((x + 1) - dstX), width_scale);
sy = sy1;
/*
DEBUG_TRACE("sx1 %f, sx2 %f, sy1 %f, sy2 %f\n",
fixed_to_float(sx1), fixed_to_float(sx2), fixed_to_float(sy1), fixed_to_float(sy2));
*/
do {
fixed_t yportion;
//DEBUG_TRACE(" yportion(sy %f, sy1 %f, sy2 %f) = ", fixed_to_float(sy), fixed_to_float(sy1), fixed_to_float(sy2));
if (fixed_floor(sy) == fixed_floor(sy1)) {
yportion = FIXED_1 - (sy - fixed_floor(sy));
if (yportion > sy2 - sy1) {
yportion = sy2 - sy1;
}
sy = fixed_floor(sy);
}
else if (sy == fixed_floor(sy2)) {
yportion = sy2 - fixed_floor(sy2);
}
else {
yportion = FIXED_1;
}
//DEBUG_TRACE("%f\n", fixed_to_float(yportion));
sx = sx1;
do {
fixed_t xportion;
fixed_t pcontribution;
pix p;
//DEBUG_TRACE(" xportion(sx %f, sx1 %f, sx2 %f) = ", fixed_to_float(sx), fixed_to_float(sx1), fixed_to_float(sx2));
if (fixed_floor(sx) == fixed_floor(sx1)) {
xportion = FIXED_1 - (sx - fixed_floor(sx));
if (xportion > sx2 - sx1) {
xportion = sx2 - sx1;
}
sx = fixed_floor(sx);
}
else if (sx == fixed_floor(sx2)) {
xportion = sx2 - fixed_floor(sx2);
}
else {
xportion = FIXED_1;
}
//DEBUG_TRACE("%f\n", fixed_to_float(xportion));
pcontribution = fixed_mul(xportion, yportion);
p = get_pix(im, fixed_to_int(sx + srcX), fixed_to_int(sy + srcY));
/*
DEBUG_TRACE(" merging with pix %d, %d: src %x (%d %d %d %d), pcontribution %f\n",
fixed_to_int(sx + srcX), fixed_to_int(sy + srcY),
p, COL_RED(p), COL_GREEN(p), COL_BLUE(p), COL_ALPHA(p), fixed_to_float(pcontribution));
*/
red += fixed_mul(int_to_fixed(COL_RED(p)), pcontribution);
green += fixed_mul(int_to_fixed(COL_GREEN(p)), pcontribution);
blue += fixed_mul(int_to_fixed(COL_BLUE(p)), pcontribution);
if (im->has_alpha)
alpha += fixed_mul(int_to_fixed(COL_ALPHA(p)), pcontribution);
spixels += pcontribution;
sx += FIXED_1;
} while (sx < sx2);
sy += FIXED_1;
} while (sy < sy2);
// If rgba get too large for the fixed-point representation, fallback to the floating point routine
// This should only happen with very large images
if (red < 0 || green < 0 || blue < 0 || alpha < 0) {
warn("fixed-point overflow: %d %d %d %d\n", red, green, blue, alpha);
return image_downsize_gd(im);
}
if (spixels != 0) {
/*
DEBUG_TRACE(" rgba (%f %f %f %f) spixels %f\n",
fixed_to_float(red), fixed_to_float(green), fixed_to_float(blue), fixed_to_float(alpha), fixed_to_float(spixels));
*/
spixels = fixed_div(FIXED_1, spixels);
red = fixed_mul(red, spixels);
green = fixed_mul(green, spixels);
blue = fixed_mul(blue, spixels);
if (im->has_alpha)
alpha = fixed_mul(alpha, spixels);
}
/* Clamping to allow for rounding errors above */
if (red > FIXED_255) red = FIXED_255;
if (green > FIXED_255) green = FIXED_255;
if (blue > FIXED_255) blue = FIXED_255;
if (im->has_alpha && alpha > FIXED_255) alpha = FIXED_255;
/*
DEBUG_TRACE(" -> %d, %d %x (%d %d %d %d)\n",
x, y, COL_FULL(fixed_to_int(red), fixed_to_int(green), fixed_to_int(blue), fixed_to_int(alpha)),
fixed_to_int(red), fixed_to_int(green), fixed_to_int(blue), fixed_to_int(alpha));
*/
if (im->orientation != ORIENTATION_NORMAL) {
int ox, oy; // new destination pixel coordinates after rotating
image_get_rotated_coords(im, x, y, &ox, &oy);
if (im->orientation >= 5) {
// 90 and 270 rotations, width/height are swapped so we have to use alternate put_pix method
put_pix_rotated(
im, ox, oy, im->target_height,
COL_FULL(fixed_to_int(red), fixed_to_int(green), fixed_to_int(blue), fixed_to_int(alpha))
);
}
else {
put_pix(
im, ox, oy,
COL_FULL(fixed_to_int(red), fixed_to_int(green), fixed_to_int(blue), fixed_to_int(alpha))
);
}
}
else {
put_pix(
im, x, y,
COL_FULL(fixed_to_int(red), fixed_to_int(green), fixed_to_int(blue), fixed_to_int(alpha))
);
}
}
}
}
/**
Uses a PID controller to try and reach the target current.
The target currect used by this function should be set using
@ref mc_set_target_current or @ref mc_set_target_current_fixed.
The constants for the current controller are given by the @c kp, @c ki, and @c kd
given to @ref mc_init.
Updates the watchdog, and uses both a thermal limiting multiplier and integrating
current limits.
*/
void mc_pid_current(void)
{
volatile fixed mc_error = 0;
static unsigned char start_count = 2;
signed long int curr_pwm = 0;
long long int pid_sum, p_component, i_component;
fixed thermal_multiplier, mechanical_multiplier;
mc_update_watchdog(); //feed watchdog if running or sleeping
if (!start_count){ //wait a few iterations to allow the adcx to get meaningful data
fixed current = mc_data.get_current(); //motor current on the robot
fixed target_current = mc_target_current; //the target current
thermal_multiplier = mc_get_thermal_limiter(current); //thermal multiplier, est. temp and limit current
mechanical_multiplier = mc_smart_check_current(current);
//During direction control, we limit the target_current to the correct direction.
//Used to do PWM, but ran into problem with Integral control
if (target_current < 0) { //trying to go in negative direction
target_current = target_current * mc_negative;
} else if (target_current > 0) { //trying to go in positive direction, but not allowed
target_current = target_current * mc_positive;
}
if (mc_is_running())
{
// ******************************* calc prop and integral **************************
//Calculate the error:
mc_error = (target_current - current);
//Integral saturation during direction control is fixed by limiting the
//target_current during direction_control, not the PWM
mc_data.integral += mc_error;
// ****************** check for saturation of integral constant ********************
if (mc_data.integral > mc_data.max_integral){
mc_data.integral = mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
} else if (mc_data.integral < -1*mc_data.max_integral){
mc_data.integral = -1*mc_data.max_integral;
error_occurred(ERROR_MC_INTEG_SATUR);
}
// Sum the terms of the PID algorithm to give the new PWM duty cycle value
//Note: Need to bound each component such that their sum can't overflow
p_component = fixed_mult_to_long((mc_error),(mc_data.kp));
i_component = fixed_mult_to_long((mc_data.integral),(mc_data.ki));
pid_sum = p_component + i_component;
// check for fixed point overflow
if(pid_sum > FIXED_MAX) {
pid_sum = FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
} else if (pid_sum < -1*FIXED_MAX) {
pid_sum = -1*FIXED_MAX;
error_occurred(ERROR_MC_FIXED_LIMIT);
}
if(thermal_multiplier < mechanical_multiplier){ //use whichever multiplier is smaller
pid_sum = fixed_mult(pid_sum, thermal_multiplier);
} else {
pid_sum = fixed_mult(pid_sum, mechanical_multiplier);
}
curr_pwm = fixed_to_int((pid_sum));
// ********************** check if PWM is within limits ****************************
if (curr_pwm > mc_data.max_pwm){
curr_pwm = mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
else if (curr_pwm < -1*mc_data.max_pwm){
curr_pwm = -1*mc_data.max_pwm;
error_occurred(ERROR_MC_PWM_LIMIT);
}
mc_set_pwm(curr_pwm);
} else {
mc_set_pwm(0);
}
} else {
start_count--;
}
}
