void drawXFormPixelWithAlpha(const uint32_t i, const uint32_t j,
const GRect &srcRect, const GIRect &dstRect,
const GBitmap &src, const GBitmap &dst,
const uint8_t alpha,
const BlendFunc blend = blend_srcover) {
GVec3f ctxPt(static_cast<float>(dstRect.fLeft + i) + 0.5f,
static_cast<float>(dstRect.fTop + j) + 0.5f,
1.0f);
ctxPt = m_CTMInv * ctxPt;
if(ContainsPoint(srcRect, ctxPt[0], ctxPt[1])) {
uint32_t x = static_cast<uint32_t>(ctxPt[0] - srcRect.fLeft);
uint32_t y = static_cast<uint32_t>(ctxPt[1] - srcRect.fTop);
GPixel *srcRow = GetRow(src, y);
GPixel *dstRow = GetRow(dst, j+dstRect.fTop) + dstRect.fLeft;
uint32_t srcA = fixed_multiply(GPixel_GetA(srcRow[x]), alpha);
uint32_t srcR = fixed_multiply(GPixel_GetR(srcRow[x]), alpha);
uint32_t srcG = fixed_multiply(GPixel_GetG(srcRow[x]), alpha);
uint32_t srcB = fixed_multiply(GPixel_GetB(srcRow[x]), alpha);
GPixel src = GPixel_PackARGB(srcA, srcR, srcG, srcB);
dstRow[i] = blend(dstRow[i], src);
}
}
static GPixel blend_srcover(GPixel dst, GPixel src) {
uint32_t srcA = GPixel_GetA(src);
if(srcA == 255) {
return src;
}
uint32_t srcR = GPixel_GetR(src);
uint32_t srcG = GPixel_GetG(src);
uint32_t srcB = GPixel_GetB(src);
uint32_t dstA = GPixel_GetA(dst);
uint32_t dstR = GPixel_GetR(dst);
uint32_t dstG = GPixel_GetG(dst);
uint32_t dstB = GPixel_GetB(dst);
return GPixel_PackARGB(srcA + fixed_multiply(dstA, 255 - srcA),
srcR + fixed_multiply(dstR, 255 - srcA),
srcG + fixed_multiply(dstG, 255 - srcA),
srcB + fixed_multiply(dstB, 255 - srcA));
}
void dv_idct_248(dv_248_coeff_t *x248)
{
dv_248_coeff_t tmp[64];
dv_248_coeff_t *in, *out;
dv_248_coeff_t u,v,w,z;
dv_248_coeff_t in0, in1, in2, in3, in4, in5, in6, in7;
gint i;
#if ! IDCT_248_UNIT_TEST
/* prescale - 64 mults */
for(i=0; i<64; i++) {
x248[i] *= dv_idct_248_prescale[i];
} // for
#endif // ! IDCT_248_UNIT_TEST
// Now, tmp = inv(h2) * inv(g2) * (prescale = inv(d2) * x248 * d)
// 32 mults, 64 adds, 80 shifts, 16 negates
in = x248;
out = tmp;
#if IDCT_248_UNIT_TEST
printf("\nt0:\n");
for(i=0;i<64; i++) {
printf("%d ", (in[i] + 0x2000) >> 14);
if((i+1) % 8 == 0) printf("\n");
} // for
#endif // IDCT_248_UNIT_TEST
for(i=0; i<8; i++) {
u = in[0+i];
v = in[2*8+i];
w = in[1*8+i];
z = in[3*8+i];
out[0*8+i] = DIV_FOUR(u) + DIV_TWO(v);
out[1*8+i] = DIV_FOUR(u) - DIV_TWO(v);
out[2*8+i] = fixed_multiply(w,beta0) + fixed_multiply(z,beta1);
out[3*8+i] = -(DIV_TWO(w+z));
u = in[4*8+i];
v = in[6*8+i];
w = in[5*8+i];
z = in[7*8+i];
out[4*8+i] = DIV_FOUR(u) + DIV_TWO(v);
out[5*8+i] = DIV_FOUR(u) - DIV_TWO(v);
out[6*8+i] = fixed_multiply(w,beta0) + fixed_multiply(z,beta1);
out[7*8+i] = -(DIV_TWO(w+z));
} // for
#if IDCT_248_UNIT_TEST
printf("\nt1:\n");
for(i=0;i<64; i++) {
printf("%d ", (out[i] + 0x2000) >> 14);
if((i+1) % 8 == 0) printf("\n");
} // for
#endif // IDCT_248_UNIT_TEST
in = tmp;
out = x248;
// Do out = inv(f) * inv(L2) * in (butterfly)
// 192 adds, 64 shifts
for(i=0; i<8; i++) {
u = in[8*0+i];
v = in[8*3+i];
w = in[8*4+i];
z = in[8*7+i];
out[8*0+i] = DIV_FOUR(u - v + w - z);
out[8*1+i] = DIV_FOUR(u - v - w + z);
out[8*6+i] = DIV_FOUR(u + v + w + z);
out[8*7+i] = DIV_FOUR(u + v - w - z);
u = in[8*1+i];
v = in[8*2+i];
w = in[8*5+i];
z = in[8*6+i];
out[i+8*2] = DIV_FOUR(u + v + w + z);
out[i+8*3] = DIV_FOUR(u + v - w - z);
out[i+8*4] = DIV_FOUR(u - v + w - z);
out[i+8*5] = DIV_FOUR(u - v - w + z);
} // for
#if IDCT_248_UNIT_TEST
printf("\nt2:\n");
for(i=0;i<64; i++) {
printf("%d ", (out[i] + 0x2000) >> 14);
if((i+1) % 8 == 0) printf("\n");
} // for
#endif // IDCT_248_UNIT_TEST
in = x248;
out = tmp;
// Do out = in * p * b1 * b2 * m
// 48 mults, 48 adds
for(i=0; i<8; i++) {
out[i*8+0] = in[i*8+0];
out[i*8+1] = in[i*8+4];
u = in[i*8+2];
v = in[i*8+6];
out[i*8+2] = fixed_multiply(u - v,beta2);
out[i*8+3] = u + v;
u = in[i*8+1];
v = in[i*8+3];
w = in[i*8+5];
z = in[i*8+7];
out[i*8+4] = fixed_multiply(u - z,beta3) + fixed_multiply(v - w,beta4);
out[i*8+5] = fixed_multiply(u - v - w + z,beta2);
out[i*8+6] = fixed_multiply(u - z,beta4) + fixed_multiply(w - v,beta3);
out[i*8+7] = u + v + w + z;
} // for
#if IDCT_248_UNIT_TEST
printf("\nt3:\n");
for(i=0;i<64; i++) {
printf("%d ", (out[i] + 0x2000) >> 14);
if((i+1) % 8 == 0) printf("\n");
} // for
#endif // IDCT_248_UNIT_TEST
in = out;
out = x248;
// Do out = in * a1 * a2 * a3 (butterflys...)
// 272 adds (will gcc factor some of these out?)
for(i=0; i<8; i++) {
in0 = in[i*8+0];
in1 = in[i*8+1];
in2 = in[i*8+2];
in3 = in[i*8+3];
in4 = in[i*8+4];
in5 = in[i*8+5];
in6 = in[i*8+6];
in7 = in[i*8+7];
out[i*8+0] = in0 + in1 + in2 + in3 + in6 + in7;
out[i*8+1] = in0 - in1 + in2 + in5 + in6;
out[i*8+2] = in0 - in1 - in2 - in4 + in5;
out[i*8+3] = in0 + in1 - in2 - in3 - in4;
out[i*8+4] = in0 + in1 - in2 - in3 + in4;
out[i*8+5] = in0 - in1 - in2 + in4 - in5;
out[i*8+6] = in0 - in1 + in2 - in5 - in6;
out[i*8+7] = in0 + in1 + in2 + in3 - in6 - in7;
} // for
#if IDCT_248_UNIT_TEST
printf("\nout:\n");
for(i=0;i<64; i++) {
printf("%d ", (out[i] + 0x2000) >> 14);
if((i+1) % 8 == 0) printf("\n");
} // for
#endif // IDCT_248_UNIT_TEST
for(i=0; i<64; i++)
out[i] = (out[i] + 0x2000) >> 14;
} // dv_idct_248
int16_t regulator_position_to_pwm(int16_t current_position)
// This function takes the current servo position as input and outputs a pwm
// value for the servo motors. The current position value must be within the
// range 0 and 1023. The output will be within the range of -255 and 255 with
// values less than zero indicating clockwise rotation and values more than
// zero indicating counter-clockwise rotation.
{
int16_t k1;
int16_t k2;
int16_t output;
int16_t command_position;
int16_t current_velocity;
int16_t current_error;
// Get the command position to where the servo is moving to from the registers.
command_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
// Get estimated velocity
current_velocity = (int16_t) registers_read_word(REG_VELOCITY_HI, REG_VELOCITY_LO);
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Update the system registers with an adjusted reverse sense
// position. With reverse sense, the position value to grows from
// a low value to high value in the clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) (MAX_POSITION - current_position));
// Adjust command position for the reverse sense.
command_position = MAX_POSITION - command_position;
}
else
{
// No. Update the system registers with a non-reverse sense position.
// Normal position value grows from a low value to high value in the
// counter-clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) current_position);
}
// Get the control parameters.
k1 = (int16_t) registers_read_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
k2 = (int16_t) registers_read_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO);
// Determine the current error.
current_error = command_position - current_position;
// The following operations are fixed point operations. To add/substract
// two fixed point values they must have the same fractional precision
// (the same number of bits behind the decimal). When two fixed point
// values are multiplied the fractional precision of the result is the sum
// of the fractional precision of the the the factors (the sum of the bits
// behind the decimal of each factor). To reach the best possible precision
// the fixed point bit is chosen for each variable separately according to
// its maximum and dimension. A shift factor is then applied after
// multiplication in the fixed_multiply() function to adjust the fractional
// precision of the product for addition or subtraction.
// Used fixed point bits, counted from the lowest bit:
// Control Param. k1: fp_k1 = 5
// Control Param. k2: fp_k2 = 5
// Position state z1: fp_z1 = 5
// Velocity state z2: fp_z2 = 11
// Real Position x1: fp_x1 = 0
// PWM output: fp_output = 0
// output = k1 * x1 + k2 * x2
output = fixed_multiply(k1, current_error, 5); // fp: 5 + 0 -> 0 : rshift = 5
output += fixed_multiply(k2, -current_velocity, 16); // fp: 5 + 11 -> 0 : rshift = 16
// Check for output saturation.
if (output > MAX_OUTPUT) output = MAX_OUTPUT;
if (output < MIN_OUTPUT) output = MIN_OUTPUT;
return output;
}
int16_t ipd_position_to_pwm(int16_t current_position)
// This function takes the current servo position as input and outputs a pwm
// value for the servo motors. The current position value must be within the
// range 0 and 1023. The output will be within the range of -255 and 255 with
// values less than zero indicating clockwise rotation and values more than
// zero indicating counter-clockwise rotation.
//
// The feedback approach implemented here was first published in Richard Phelan's
// Automatic Control Systems, Cornell University Press, 1977 (ISBN 0-8014-1033-9)
//
// The theory of operation of this function will be filled in later, but the
// diagram below should gives a general picture of how it is intended to work.
//
//
// +<------- bounds checking -------+
// | |
// |¯¯¯¯¯| |¯¯¯¯¯¯¯¯| |¯¯¯¯¯| |¯¯¯¯¯¯¯¯¯| |
// command -->| - |-->|integral|-->| - |-->| motor |-->+-> actuator
// |_____| |________| |_____| |_________| |
// | | |
// | +<-- Kv * velocity --+
// | | |
// | +<-- Kp * position --+
// | |
// +<-------------Ki * position ---------------+
//
//
{
int16_t deadband;
int16_t command_position;
int16_t maximum_position;
int16_t minimum_position;
int16_t current_velocity;
int16_t command_error;
int16_t output;
int16_t position_output;
int16_t velocity_output;
int16_t integral_output;
uint16_t position_gain;
uint16_t velocity_gain;
uint16_t integral_gain;
// Determine the velocity as the difference between the current and previous position.
current_velocity = current_position - previous_position;
// Update the previous position.
previous_position = current_position;
// Get the command position to where the servo is moving to from the registers.
command_position = (int16_t) registers_read_word(REG_SEEK_POSITION_HI, REG_SEEK_POSITION_LO);
minimum_position = (int16_t) registers_read_word(REG_MIN_SEEK_HI, REG_MIN_SEEK_LO);
maximum_position = (int16_t) registers_read_word(REG_MAX_SEEK_HI, REG_MAX_SEEK_LO);
// Set the deadband value and divide by two for calculations below.
deadband = 2;
// Are we reversing the seek sense?
if (registers_read_byte(REG_REVERSE_SEEK) != 0)
{
// Yes. Update the system registers with an adjusted reverse sense
// position. With reverse sense, the position value to grows from
// a low value to high value in the clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) (MAX_POSITION - current_position));
// Adjust command position for the reverse sense.
command_position = MAX_POSITION - command_position;
minimum_position = MAX_POSITION - minimum_position;
maximum_position = MAX_POSITION - maximum_position;
}
else
{
// No. Update the system registers with a non-reverse sense position.
// Normal position value grows from a low value to high value in the
// counter-clockwise direction.
registers_write_word(REG_POSITION_HI, REG_POSITION_LO, (uint16_t) current_position);
}
// Sanity check the command position. We do this because this value is
// passed from the outside to the servo and it could be an invalid value.
if (command_position < minimum_position) command_position = minimum_position;
if (command_position > maximum_position) command_position = maximum_position;
// The command error is the difference between the command position and current position.
command_error = command_position - current_position;
// Adjust proportional error due to deadband. The potentiometer readings are a
// bit noisy and there is typically one or two units of difference from reading
// to reading when the servo is holding position. Adding deadband decreases some
// of the twitchiness in the servo caused by this noise.
if (command_error > deadband)
{
// Factor out deadband from the command error.
command_error -= deadband;
}
else if (command_error < -deadband)
{
// Factor out deadband from the command error.
command_error += deadband;
}
else
{
// Adjust to command error to zero within deadband.
command_error = 0;
}
// Get the positional, velocity and integral gains from the registers.
position_gain = registers_read_word(REG_PID_PGAIN_HI, REG_PID_PGAIN_LO);
velocity_gain = registers_read_word(REG_PID_DGAIN_HI, REG_PID_DGAIN_LO);
integral_gain = registers_read_word(REG_PID_IGAIN_HI, REG_PID_IGAIN_LO);
// Add the command error scaled by the position gain to the integral accumulator.
// The integral accumulator maintains a sum of total error over each iteration.
integral_accumulator_update(command_error, integral_gain);
// Get the integral output. There is no gain applied to the integral output.
integral_output = integral_accumulator_get();
// Determine the position output component. We multiply the position gain
// by the current position of the servo to create the position output.
position_output = fixed_multiply(current_position, position_gain);
// Determine the velocity output component. We multiply the velocity gain
// by the current velocity of the servo to create the velocity output.
velocity_output = fixed_multiply(current_velocity, velocity_gain);
// registers_write_word(REG_RESERVED_18, REG_RESERVED_19, (uint16_t) position_output);
// registers_write_word(REG_RESERVED_1A, REG_RESERVED_1B, (uint16_t) velocity_output);
// registers_write_word(REG_RESERVED_1C, REG_RESERVED_1D, (uint16_t) integral_output);
// The integral output drives the output and the position and velocity outputs
// function as a frictional component to counter the integral output.
output = integral_output - position_output - velocity_output;
// Is the output saturated? If so we need limit the output and clip
// the integral accumulator just at the saturation level.
if (output < MIN_OUTPUT)
{
// Calculate a new integral accumulator based on the output range. This value
// is calculated to keep the integral output just on the verge of saturation.
integral_accumulator_reset(position_output + velocity_output + MIN_OUTPUT);
// Limit the output.
output = MIN_OUTPUT;
}
else if (output > MAX_OUTPUT)
{
// Calculate a new integral accumulator based on the output range. This value
// is calculated to keep the integral output just on the verge of saturation.
integral_accumulator_reset(position_output + velocity_output + MAX_OUTPUT);
// Limit the output.
output = MAX_OUTPUT;
}
// Return the output.
return output;
}
// This code draws a bitmap assuming that we only have translation and scale,
// which allows us to perform certain optimizations...
void drawBitmapSimple(const GBitmap &bm, const GPaint &paint) {
const GBitmap &ctxbm = GetInternalBitmap();
GRect ctxRect = GRect::MakeXYWH(0, 0, ctxbm.width(), ctxbm.height());
GRect bmRect = GRect::MakeXYWH(0, 0, bm.width(), bm.height());
GRect pixelRect = GetTransformedBoundingBox(bmRect);
GRect rect;
if(!(rect.setIntersection(ctxRect, pixelRect))) {
return;
}
// We know that since we're only doing scale and translation, that all of the pixel
// centers contained in rect are going to be drawn, so we only need to know the
// dimensions of the mapping...
GVec3f origin(0, 0, 1);
GVec3f offset(1, 1, 1);
origin = m_CTM * origin;
offset = m_CTM * offset;
float xScale = 1.0f / (offset.X() - origin.X());
float yScale = 1.0f / (offset.Y() - origin.Y());
GVec2f start = GVec2f(0, 0);
if(xScale < 0.0f) {
start.X() = pixelRect.fRight - 1.0f;
}
if(yScale < 0.0f) {
start.Y() = pixelRect.fBottom - 1.0f;
}
GIRect dstRect = rect.round();
// Construct new bitmap
int32_t offsetX = ::std::max(0, -dstRect.fLeft);
int32_t offsetY = ::std::max(0, -dstRect.fTop);
GBitmap fbm;
fbm.fWidth = bm.width();
fbm.fHeight = bm.height();
fbm.fPixels = GetRow(bm, offsetY) + offsetX;
fbm.fRowBytes = bm.fRowBytes;
BlendFunc blend = blend_srcover;
float alpha = paint.getAlpha();
if(alpha >= kOpaqueAlpha) {
for(uint32_t j = 0; j < dstRect.height(); j++) {
uint32_t srcIdxY = static_cast<uint32_t>(start.Y() + static_cast<float>(j) * yScale);
GPixel *srcPixels = GetRow(fbm, Clamp<int>(srcIdxY, 0, fbm.height()));
GPixel *dstPixels = GetRow(ctxbm, dstRect.fTop + j) + dstRect.fLeft;
for(uint32_t i = 0; i < dstRect.width(); i++) {
uint32_t srcIdxX = static_cast<uint32_t>(start.X() + static_cast<float>(i) * xScale);
dstPixels[i] = blend(dstPixels[i], srcPixels[Clamp<int>(srcIdxX, 0, fbm.width())]);
}
}
} else {
const uint32_t alphaVal = static_cast<uint32_t>((alpha * 255.0f) + 0.5f);
for(uint32_t j = 0; j < dstRect.height(); j++) {
uint32_t srcIdxY = static_cast<uint32_t>(start.Y() + static_cast<float>(j) * yScale);
GPixel *srcPixels = GetRow(fbm, srcIdxY);
GPixel *dstPixels = GetRow(ctxbm, dstRect.fTop + j) + dstRect.fLeft;
for(uint32_t i = 0; i < dstRect.width(); i++) {
uint32_t srcIdxX = static_cast<uint32_t>(start.X() + static_cast<float>(i) * xScale);
uint32_t srcA = fixed_multiply(GPixel_GetA(srcPixels[srcIdxX]), alphaVal);
uint32_t srcR = fixed_multiply(GPixel_GetR(srcPixels[srcIdxX]), alphaVal);
uint32_t srcG = fixed_multiply(GPixel_GetG(srcPixels[srcIdxX]), alphaVal);
uint32_t srcB = fixed_multiply(GPixel_GetB(srcPixels[srcIdxX]), alphaVal);
GPixel src = GPixel_PackARGB(srcA, srcR, srcG, srcB);
dstPixels[i] = blend(dstPixels[i], src);
}
}
}
}
