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--; } }