void runEncoders()
{
nMotorEncoder[frontLeft] = 0;
nMotorEncoder[frontRight] = 0;
//lError = distance - nMotorEncoder[frontLeft];
//rError = distance - nMotorEncoder[frontRight];
//We just set the encoders to 0, so we don't have to subtract anything!
motor[frontLeft] = motor[backLeft] = BOUND(leftEncoder * kp, -100, 100);
motor[frontRight] = motor[backRight] = BOUND(rightEncoder * kp, -100, 100);
while(nMotorRunState[frontLeft] != runStateIdle || nMotorRunState[frontRight] != runStateIdle)
{
if(nMotorEncoder[frontLeft] != nMotorEncoder[frontRight]) //If the encoders aren't equal (if one's faster)
{
error = nMotorEncoder[frontLeft] - nMotorEncoder[frontRight]; //Find out how far ahead/behind the left encoder is, then add it to the right motor (to fix the difference)
lError = leftEncoder - nMotorEncoder[frontLeft]; //Adding the error to both won't fix the issue
rError = rightEncoder - nMotorEncoder[frontRight] + error;
} else {
lError = leftEncoder - nMotorEncoder[frontLeft];
rError = rightEncoder - nMotorEncoder[frontRight];
}
if(abs(lError) <= 0 || abs(rError) <= 0)
{
motor[frontLeft] = motor[backLeft] = motor[frontRight] = motor[backRight] = 0;
} else {
motor[frontLeft] = motor[backLeft] = BOUND(lError * kp, -100, 100);
motor[frontRight] = motor[backRight] = BOUND(rError * kp, -100, 100);
}
}
}
static __inline int
FbClipImageReg (pixman_region16_t *region,
pixman_region16_t *clip,
int dx,
int dy)
{
if (pixman_region_num_rects (region) == 1 &&
pixman_region_num_rects (clip) == 1)
{
pixman_box16_t *pRbox = pixman_region_rects (region);
pixman_box16_t *pCbox = pixman_region_rects (clip);
int v;
if (pRbox->x1 < (v = pCbox->x1 + dx))
pRbox->x1 = BOUND(v);
if (pRbox->x2 > (v = pCbox->x2 + dx))
pRbox->x2 = BOUND(v);
if (pRbox->y1 < (v = pCbox->y1 + dy))
pRbox->y1 = BOUND(v);
if (pRbox->y2 > (v = pCbox->y2 + dy))
pRbox->y2 = BOUND(v);
if (pRbox->x1 >= pRbox->x2 ||
pRbox->y1 >= pRbox->y2)
{
pixman_region_empty (region);
}
}
else
{
pixman_region_translate (region, dx, dy);
pixman_region_intersect (region, clip, region);
pixman_region_translate (region, -dx, -dy);
}
return 1;
}
static inline pixman_bool_t
miClipPictureReg(pixman_region16_t * pRegion,
pixman_region16_t * pClip, int dx, int dy)
{
if (pixman_region_n_rects(pRegion) == 1 &&
pixman_region_n_rects(pClip) == 1) {
pixman_box16_t *pRbox = pixman_region_rectangles(pRegion, NULL);
pixman_box16_t *pCbox = pixman_region_rectangles(pClip, NULL);
int v;
if (pRbox->x1 < (v = pCbox->x1 + dx))
pRbox->x1 = BOUND(v);
if (pRbox->x2 > (v = pCbox->x2 + dx))
pRbox->x2 = BOUND(v);
if (pRbox->y1 < (v = pCbox->y1 + dy))
pRbox->y1 = BOUND(v);
if (pRbox->y2 > (v = pCbox->y2 + dy))
pRbox->y2 = BOUND(v);
if (pRbox->x1 >= pRbox->x2 || pRbox->y1 >= pRbox->y2) {
pixman_region_init(pRegion);
}
}
else if (!pixman_region_not_empty(pClip))
return FALSE;
else {
if (dx || dy)
pixman_region_translate(pRegion, -dx, -dy);
if (!pixman_region_intersect(pRegion, pRegion, pClip))
return FALSE;
if (dx || dy)
pixman_region_translate(pRegion, dx, dy);
}
return pixman_region_not_empty(pRegion);
}
void DriveTask()
{
if (g_driveEnabled)
{
float driveErr = g_driveTarget - GetDriveDistance();
if (abs(driveErr) > g_driveTolerance)
{
//
// Simple proportional PID control.
// Limit the outpout to the range of -100 to 100 for drive
// and -50 to 50 for turn
//
int drivePower = BOUND((int)(g_driveKp*driveErr), -100, 100);
float turnErr = GyroGetHeading(g_Gyro);
int turnPower = BOUND((int)(g_turnKp*turnErr), -100, 100);
motor[motorLeft] = BOUND(drivePower + turnPower, -100, 100);
motor[motorRight] = BOUND(drivePower - turnPower, -100, 100);
nxtDisplayTextLine(7, "drvErr=%f", driveErr);
}
else
{
motor[motorLeft] = 0;
motor[motorRight] = 0;
g_driveEnabled = false;
}
nxtDisplayTextLine(4, "Distance=%f", GetDriveDistance());
}
}
/*
* updateFreq: Update the PWM output's frequency and duty cycle
*
* Arguments: hertz - the requested PWM output frequency in Hz units
* duty - the requested PWM output duty cycle in % units
*
*/
void updateFreq(unsigned int hertz,unsigned int duty)
{
unsigned int freqValue = BOUND(MIN_PERIOD,1000000/hertz,MAX_PERIOD);
unsigned int dutyCycle = BOUND(10,duty,90);
LPC_PWM1->MR0 = freqValue;
LPC_PWM1->MR3 = (freqValue * dutyCycle) / 100;
LPC_PWM1->LER = LER0_EN | LER3_EN ; //Load Shadow register contents
}
void main_wheel( int joy1_x1, int joy1_y1 )
{
int k=8;
int drivePower = DEADBAND(joy1_x1/k);
int turnPower = DEADBAND(joy1_y1/k);
motor[motorA] = BOUND(drivePower - turnPower);
motor[motorB] = BOUND(drivePower + turnPower);
}
void main_wheel( int joy1_x1, int joy1_y1 )
{
int drivePower = DEADBAND(joy1_x1);
int turnPower = DEADBAND(joy1_y1);
motor[LeftBase] = BOUND(drivePower - turnPower);
motor[RightBase] = BOUND(drivePower + turnPower);
}
//亮度调节
void ChangeBrightness(int nDelta, BYTE *r, BYTE *g, BYTE *b)
{
int R = *r + nDelta;
int G = *g + nDelta;
int B = *b + nDelta;
*r = (BYTE)BOUND(R, 0, 255);
*g = (BYTE)BOUND(G, 0, 255);
*b = (BYTE)BOUND(B, 0, 255);
}
static struct panel *get_panel_skills(void) {
struct panel *p = panel_allocate(8);
int skill;
byte attr;
const char *desc;
#define BOUND(x, min, max) MIN(max, MAX(min, x))
/* Saving throw */
skill = BOUND(player->state.skills[SKILL_SAVE], 0, 100);
panel_line(p, colour_table[skill / 10], "Saving Throw", "%d%%", skill);
/* Stealth */
desc = likert(player->state.skills[SKILL_STEALTH], 1, &attr);
panel_line(p, attr, "Stealth", "%s", desc);
/* Disarming: -5 because we assume we're disarming a dungeon trap */
skill = BOUND(player->state.skills[SKILL_DISARM] - 5, 2, 100);
panel_line(p, colour_table[skill / 10], "Disarming", "%d%%", skill);
/* Magic devices */
skill = player->state.skills[SKILL_DEVICE];
panel_line(p, colour_table[skill / 13], "Magic Devices", "%d", skill);
/* Search frequency */
skill = MAX(player->state.skills[SKILL_SEARCH_FREQUENCY], 1);
if (skill >= 50) {
panel_line(p, colour_table[10], "Perception", "1 in 1");
} else {
/* convert to chance of searching */
skill = 50 - skill;
panel_line(p, colour_table[(100 - skill*2) / 10],
"Perception", "1 in %d", skill);
}
/* Searching ability */
skill = BOUND(player->state.skills[SKILL_SEARCH], 0, 100);
panel_line(p, colour_table[skill / 10], "Searching", "%d%%", skill);
/* Infravision */
panel_line(p, COLOUR_L_GREEN, "Infravision", "%d ft",
player->state.see_infra * 10);
/* Speed */
skill = player->state.speed;
if (player->timed[TMD_FAST]) skill -= 10;
if (player->timed[TMD_SLOW]) skill += 10;
if (player->searching) skill += 10;
attr = skill < 110 ? COLOUR_L_UMBER : COLOUR_L_GREEN;
panel_line(p, attr, "Speed", "%s", show_speed());
return p;
}
// calculate which internal storage cell contains a device
int UnitDiscRadio::device_cell(Device* d) {
const flo* p = d->body->position();
int row = (int)floor((p[0]-cell_left)/range);
int col = (int)floor((p[1]-cell_bottom)/range);
row = BOUND(row,0,cell_rows-1); col = BOUND(col,0,cell_cols-1);
int n = row*cell_cols + col;
if(cell_lvls>1) { // 3D
int lvl = (int)floor((p[2]-cell_floor)/range);
lvl = BOUND(lvl,0,cell_lvls-1);
n += lvl_size*lvl;
}
return n;
}
void TurnTask()
{
if (g_turnEnabled)
{
float error = GyroGetHeading(g_Gyro) - g_turnTarget;
if (abs(error) > g_turnTolerance)
{
//
// Simple proportional PID control.
// Limit the outpout to the range of -70 to 70.
//
int turnPower = BOUND((int)(g_turnKp*error), -100, 100);
motor[motorLeft] = turnPower;
motor[motorRight] = -turnPower;
}
else
{
motor[motorLeft] = 0;
motor[motorRight] = 0;
g_turnEnabled = false;
nxtDisplayTextLine(7, "err=%f", error);
}
nxtDisplayTextLine(3, "Heading=%f", GyroGetHeading(g_Gyro));
nxtDisplayTextLine(6, "target=%f,tol=%f", g_turnTarget, g_turnTolerance);
}
}
static double
hover_forward_roll_of_quat(const quat_t * const q)
{
double r23 = 2*(q->q2*q->q3 + q->q0*q->q1);
BOUND(r23, -1, 1);
return asin(r23);
}
void Scrollbar::UpdateScrollerPos(Window &_win)
{
double DELTA = this->getScrollerRatio();
// determine new scroller position
if (btnUp->isPressed())
{
scrollPos -= DELTA;
}
else if (btnDown->isPressed())
{
scrollPos += DELTA;
}
else if(scroller->isPressed()) // we are dragging scroller
{
position2di pos = _win.reciever.GetMouseState().positionCurr - position2di(absXpos, absYpos);
switch (scrollBarOrientation)
{
case HORIZANTAL:
scrollPos = pos.X;
break;
case VERTICAL:
scrollPos = pos.Y;
break;
default:
// ;-;
break;
};
}
// Update scroller position
switch (scrollBarOrientation)
{
case HORIZANTAL:
this->scrollPos = BOUND(this->getScrollerLength() / 2 + WIDTH, scrollPos, -this->getScrollerLength() / 2 + pxLength - WIDTH);
scroller->setPos(scrollPos - this->getScrollerLength() / 2, 0);
break;
case VERTICAL:
this->scrollPos = BOUND(this->getScrollerLength() / 2 + WIDTH, scrollPos, -this->getScrollerLength() / 2 + pxLength - WIDTH);
scroller->setPos(0, scrollPos - getScrollerLength() / 2);
break;
default:
// ;-;
break;
};
}
static struct panel *get_panel_skills(void) {
struct panel *p = panel_allocate(8);
int skill;
byte attr;
const char *desc;
int depth = cave ? cave->depth : 0;
#define BOUND(x, min, max) MIN(max, MAX(min, x))
/* Saving throw */
skill = BOUND(player->state.skills[SKILL_SAVE], 0, 100);
panel_line(p, colour_table[skill / 10], "Saving Throw", "%d%%", skill);
/* Stealth */
desc = likert(player->state.skills[SKILL_STEALTH], 1, &attr);
panel_line(p, attr, "Stealth", "%s", desc);
/* Physical disarming: assume we're disarming a dungeon trap */
skill = BOUND(player->state.skills[SKILL_DISARM_PHYS] - depth / 5, 2, 100);
panel_line(p, colour_table[skill / 10], "Disarm - phys.", "%d%%", skill);
/* Magical disarming */
skill = BOUND(player->state.skills[SKILL_DISARM_MAGIC] - depth / 5, 2, 100);
panel_line(p, colour_table[skill / 10], "Disarm - magic", "%d%%", skill);
/* Magic devices */
skill = player->state.skills[SKILL_DEVICE];
panel_line(p, colour_table[skill / 13], "Magic Devices", "%d", skill);
/* Searching ability */
skill = BOUND(player->state.skills[SKILL_SEARCH], 0, 100);
panel_line(p, colour_table[skill / 10], "Searching", "%d%%", skill);
/* Infravision */
panel_line(p, COLOUR_L_GREEN, "Infravision", "%d ft",
player->state.see_infra * 10);
/* Speed */
skill = player->state.speed;
if (player->timed[TMD_FAST]) skill -= 10;
if (player->timed[TMD_SLOW]) skill += 10;
attr = skill < 110 ? COLOUR_L_UMBER : COLOUR_L_GREEN;
panel_line(p, attr, "Speed", "%s", show_speed());
return p;
}
void
toytronics_set_sp_absolute_forward_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
const euler_t * const e_n2b = get_e_n2b();
// local copies to allow implementing a deadband
double rcp = rc->pitch;
double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND);
double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
euler_t e_n2sp;
e_n2sp.pitch = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + absolute_forward_pitch_trim_deg)*M_PI/180.0;
e_n2sp.roll = rcr * SETPOINT_MAX_STICK_ANGLE_DEG * M_PI/180.0;
// integrate stick to get setpoint heading
setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcy;
setpoint.setpoint_heading += dt*e_n2sp.roll*roll_to_yaw_rate_ff_factor;
// accel turn coordination
setpoint.setpoint_heading -= dt*tc_fading(q_n2b)*forward_accel_tc_gain*get_y_accel();
wrap_to_pi( &setpoint.setpoint_heading );
// bound setpoint heading
setpoint.estimated_heading = get_fudged_yaw( q_n2b, e_n2b );
double error_heading = setpoint.setpoint_heading - setpoint.estimated_heading;
wrap_to_pi(&error_heading);
BOUND(error_heading, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
setpoint.setpoint_heading = setpoint.estimated_heading + error_heading;
// generate setpoint
e_n2sp.yaw = setpoint.setpoint_heading;
quat_of_euler321( &setpoint.q_n2sp, &e_n2sp );
// smooth transition
smooth_transition_angle -= dt*M_PI/180.0*SETPOINT_MODE_1_2_TRANSITION_DEGREES_PER_SEC;
if (smooth_transition_angle < 0) smooth_transition_angle = 0.0;
quat_t q_st;
q_st.q0 = cos(0.5*smooth_transition_angle);
q_st.q1 = 0.0;
q_st.q2 = sin(0.5*smooth_transition_angle);
q_st.q3 = 0.0;
setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, q_st);
// calculate body to setpoint quat
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
/**
* Find a clear space around supplied object that hero can walk to
*/
bool ObjectHandler::findObjectSpace(object_t *obj, int16 *destx, int16 *desty) {
debugC(1, kDebugObject, "findObjectSpace(obj, %d, %d)", *destx, *desty);
seq_t *curImage = obj->currImagePtr;
int16 y = obj->y + curImage->y2 - 1;
bool foundFl = true;
// Try left rear corner
for (int16 x = *destx = obj->x + curImage->x1; x < *destx + HERO_MAX_WIDTH; x++) {
if (BOUND(x, y))
foundFl = false;
}
if (!foundFl) { // Try right rear corner
foundFl = true;
for (int16 x = *destx = obj->x + curImage->x2 - HERO_MAX_WIDTH + 1; x <= obj->x + (int16)curImage->x2; x++) {
if (BOUND(x, y))
foundFl = false;
}
}
if (!foundFl) { // Try left front corner
foundFl = true;
y += 2;
for (int16 x = *destx = obj->x + curImage->x1; x < *destx + HERO_MAX_WIDTH; x++) {
if (BOUND(x, y))
foundFl = false;
}
}
if (!foundFl) { // Try right rear corner
foundFl = true;
for (int16 x = *destx = obj->x + curImage->x2 - HERO_MAX_WIDTH + 1; x <= obj->x + (int16)curImage->x2; x++) {
if (BOUND(x, y))
foundFl = false;
}
}
*desty = y;
return foundFl;
}
static __inline Bool
miClipPictureReg (RegionPtr pRegion,
RegionPtr pClip,
int dx,
int dy)
{
if (REGION_NUM_RECTS(pRegion) == 1 &&
REGION_NUM_RECTS(pClip) == 1)
{
BoxPtr pRbox = REGION_RECTS(pRegion);
BoxPtr pCbox = REGION_RECTS(pClip);
int v;
if (pRbox->x1 < (v = pCbox->x1 + dx))
pRbox->x1 = BOUND(v);
if (pRbox->x2 > (v = pCbox->x2 + dx))
pRbox->x2 = BOUND(v);
if (pRbox->y1 < (v = pCbox->y1 + dy))
pRbox->y1 = BOUND(v);
if (pRbox->y2 > (v = pCbox->y2 + dy))
pRbox->y2 = BOUND(v);
if (pRbox->x1 >= pRbox->x2 ||
pRbox->y1 >= pRbox->y2)
{
REGION_EMPTY(pScreen, pRegion);
}
}
else
{
REGION_TRANSLATE(pScreen, pRegion, dx, dy);
if (!REGION_INTERSECT (pScreen, pRegion, pRegion, pClip))
return FALSE;
REGION_TRANSLATE(pScreen, pRegion, -dx, -dy);
}
return TRUE;
}
void ArmTask()
{
if (g_armEnabled)
{
float error = g_armTarget - GetArmPostion();
nxtDisplayTextLine(4, "ArmErr=%f",error);
if (abs(error) > g_armTolerance)
{
motor[tower] = BOUND((int)(g_armKp*error), -50,50);
}
else
{
motor[tower] = 0;
g_armEnabled = false;
}
}
}
void
toytronics_set_sp_hover_forward_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
// estimated heading for telemetry and bounding
setpoint.estimated_heading = hover_forward_yaw_of_quat( q_n2b );
// local copies to allow implementing a deadband
double rcp = rc->pitch;
double rcr = rc->roll;
double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
// set pitch/yaw from stick
double pitch_body = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + hover_pitch_trim_deg)*M_PI/180.0;
double roll_body = rcr * SETPOINT_MAX_STICK_ANGLE_DEG*M_PI/180.0;
// integrate stick to get setpoint heading
setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcy;
// body roll feedforward turn coordination
#define START_FADING_DEG -50
#define FINISH_FADING_DEG -70
double ff_fading_slider;
if (pitch_body > START_FADING_DEG*M_PI/180.0)
ff_fading_slider = 0;
else if (pitch_body < FINISH_FADING_DEG*M_PI/180.0)
ff_fading_slider = 1;
else
ff_fading_slider = (pitch_body*180.0/M_PI - START_FADING_DEG)/(FINISH_FADING_DEG - START_FADING_DEG);
setpoint.setpoint_heading += dt*roll_body*roll_to_yaw_rate_ff_factor*ff_fading_slider;
// accel y turn coordination
setpoint.setpoint_heading -= dt*tc_fading(q_n2b)*hover_forward_accel_tc_gain*get_y_accel();
// bound heading error
double heading_error = setpoint.setpoint_heading - setpoint.estimated_heading;
wrap_to_pi(&heading_error);
BOUND(heading_error, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
setpoint.setpoint_heading = setpoint.estimated_heading + heading_error;
wrap_to_pi(&setpoint.setpoint_heading);
// start straight up
quat_t q_y90 = {sqrt(2)/2.0, 0, sqrt(2)/2.0, 0};
quat_memcpy( &setpoint.q_n2sp, &q_y90 );
// yaw
quat_t q_yaw = {1,0,0,0};
q_yaw.q0 = cos(0.5*setpoint.setpoint_heading);
q_yaw.q1 = -sin(0.5*setpoint.setpoint_heading);
quat_t q_temp;
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_yaw );
// roll
quat_t q_roll = {1,0,0,0};
q_roll.q0 = cos(0.5*roll_body);
q_roll.q3 = sin(0.5*roll_body);
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_roll );
// pitch
quat_t q_pitch = {1,0,0,0};
q_pitch.q0 = cos(0.5*pitch_body);
q_pitch.q2 = sin(0.5*pitch_body);
quat_memcpy( &q_temp, &setpoint.q_n2sp );
quat_mult( &setpoint.q_n2sp, &q_temp, &q_pitch );
/* // calculate body to setpoint quat */
/* quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp)); */
/* // now bound setpoint quat to not get too far away from estimated quat */
/* BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0); */
/* BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0); */
/* BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0); */
/* // let setpoint decay back to body */
/* discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt ); */
/* discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt ); */
/* discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt ); */
/* // normalize */
/* setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3)); */
/* // update n2sp quat */
/* quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp)); */
// update setpoint.heading
setpoint.setpoint_heading = hover_forward_yaw_of_quat( &setpoint.q_n2sp );
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
int
FbComputeCompositeRegion (pixman_region16_t *region,
pixman_image_t *iSrc,
pixman_image_t *iMask,
pixman_image_t *iDst,
int16_t xSrc,
int16_t ySrc,
int16_t xMask,
int16_t yMask,
int16_t xDst,
int16_t yDst,
uint16_t width,
uint16_t height)
{
int v;
int x1, y1, x2, y2;
/* XXX: This code previously directly set the extents of the
region here. I need to decide whether removing that has broken
this. Also, it might be necessary to just make the pixman_region16_t
data structure transparent anyway in which case I can just put
the code back. */
x1 = xDst;
v = xDst + width;
x2 = BOUND(v);
y1 = yDst;
v = yDst + height;
y2 = BOUND(v);
/* Check for empty operation */
if (x1 >= x2 ||
y1 >= y2)
{
pixman_region_empty (region);
return 1;
}
/* clip against src */
if (!FbClipImageSrc (region, iSrc, xDst - xSrc, yDst - ySrc))
{
pixman_region_destroy (region);
return 0;
}
if (iSrc->alphaMap)
{
if (!FbClipImageSrc (region, iSrc->alphaMap,
xDst - (xSrc + iSrc->alphaOrigin.x),
yDst - (ySrc + iSrc->alphaOrigin.y)))
{
pixman_region_destroy (region);
return 0;
}
}
/* clip against mask */
if (iMask)
{
if (!FbClipImageSrc (region, iMask, xDst - xMask, yDst - yMask))
{
pixman_region_destroy (region);
return 0;
}
if (iMask->alphaMap)
{
if (!FbClipImageSrc (region, iMask->alphaMap,
xDst - (xMask + iMask->alphaOrigin.x),
yDst - (yMask + iMask->alphaOrigin.y)))
{
pixman_region_destroy (region);
return 0;
}
}
}
if (!FbClipImageReg (region, iDst->pCompositeClip, 0, 0))
{
pixman_region_destroy (region);
return 0;
}
if (iDst->alphaMap)
{
if (!FbClipImageReg (region, iDst->alphaMap->pCompositeClip,
-iDst->alphaOrigin.x,
-iDst->alphaOrigin.y))
{
pixman_region_destroy (region);
return 0;
}
}
return 1;
}
wexlsr3d wexlsr_init(wexcub3d cub,
int px,
int py,
float dsmax
)
/*< initialize >*/
{
int ix, iy;
float kx, ky;
int jx, jy;
float xx, yy;
int ilx,ily;
int ompith;
/*------------------------------------------------------------*/
wexlsr3d lsr;
lsr = (wexlsr3d) sf_alloc(1,sizeof(*lsr));
X2K(cub->amx,lsr->bxx,px);
X2K(cub->amy,lsr->byy,py);
/* allocate K-domain storage */
lsr->wk = sf_complexalloc3(lsr->bxx.n,lsr->byy.n,cub->ompnth);
lsr->wt = sf_complexalloc3(lsr->bxx.n,lsr->byy.n,cub->ompnth);
/* precompute wavenumbers */
lsr->kk = sf_floatalloc2(lsr->bxx.n,lsr->byy.n);
lsr->kw = sf_floatalloc2(lsr->bxx.n,lsr->byy.n);
for (iy=0; iy<lsr->byy.n; iy++) {
jy = KMAP(iy,lsr->byy.n);
ky = lsr->byy.o + jy*lsr->byy.d;
for (ix=0; ix<lsr->bxx.n; ix++) {
jx = KMAP(ix,lsr->bxx.n);
kx = lsr->bxx.o + jx*lsr->bxx.d;
lsr->kk[iy][ix] = kx*kx+ky*ky;
lsr->kw[iy][ix] = 1.;
}
}
/* precompute indices */
lsr->lx = sf_intalloc(cub->amx.n);
lsr->ly = sf_intalloc(cub->amy.n);
for (iy=0; iy<cub->amy.n; iy++) {
yy = cub->amy.o + iy*cub->amy.d;
ily = INDEX( yy,cub->aly);
lsr->ly[iy] = BOUND(ily,cub->aly.n); /* x-line index */
}
for (ix=0; ix<cub->amx.n; ix++) {
xx = cub->amx.o + ix*cub->amx.d;
ilx = INDEX( xx,cub->alx);
lsr->lx[ix] = BOUND(ilx,cub->alx.n); /* i-line index */
}
/* square-root expansion coefficients */
if(!sf_getint("nsc",&lsr->nsc)) lsr->nsc = 0;
if(lsr->nsc>5) lsr->nsc=5;
lsr->csc[0]= 1.;
lsr->csc[1]= 1./ 2.;
lsr->csc[2]= 3./ 8.;
lsr->csc[3]= 5./ 16.;
lsr->csc[4]=35./128.;
/* initialize FFT */
lsr->f2d = (wexfft2d*) sf_alloc(cub->ompnth,sizeof(wexfft2d));
for(ompith=0;ompith<cub->ompnth;ompith++) {
lsr->f2d[ompith] = wexfft_init(cub,lsr->bxx.n,lsr->byy.n);
}
lsr->dsmax2 = dsmax*dsmax;
lsr->dsmax2*= lsr->dsmax2;
return lsr;
}
void CVideoEffect::SharpLaplacianCl(BYTE* image0, BYTE* image1, unsigned int w, unsigned int h, int db)
{
//将图像转化为矩阵形式
BYTE** imageBuf0 = CreatImage(image0,m_ImageBufSrc, w, h);
BYTE** imageBuf1 = CreatImage(image1,m_ImageBufDst, w, h);
int a = 0;
int index,index1;
int m1,m2,m3;
//依次对原图像的每个像素进行处理
for(int y=1; y<h-1; y++)
{
for(int x=1; x<w-1; x++)
{
m2 = x * BIT_WIDTH;
m1 = m2 - BIT_WIDTH;
m3 = m2 + BIT_WIDTH;
index = m2;
int n1 = y-1;
int n2 = y+1;
{
//i=0 a+=imageBuf0[y-1+i][(x-1+j)*BIT_WIDTH+c] * templtlp[i*3+j];//加权求和
a-=imageBuf0[n1][m1];
a-=imageBuf0[n1][m2];
a-=imageBuf0[n1][m3];
//i=1
a-=imageBuf0[y][m1];
a+=imageBuf0[y][m2] * templtlp[4];
a-=imageBuf0[y][m3];
//i=2
a-=imageBuf0[n2][m1];
a-=imageBuf0[n2][m2];
a-=imageBuf0[n2][m3];
a/= db;
a=imageBuf0[y][index]+a;//对中心像素进行增强
a = BOUND(a,0,255);
imageBuf1[y][index]=a;
}
++index; ++m1; ++m2; ++m3;
{
//i=0 a+=imageBuf0[y-1+i][(x-1+j)*BIT_WIDTH+c] * templtlp[i*3+j];//加权求和
a-=imageBuf0[n1][m1];
a-=imageBuf0[n1][m2];
a-=imageBuf0[n1][m3];
//i=1
a-=imageBuf0[y][m1];
a+=imageBuf0[y][m2] * templtlp[4];
a-=imageBuf0[y][m3];
//i=2
a-=imageBuf0[n2][m1];
a-=imageBuf0[n2][m2];
a-=imageBuf0[n2][m3];
a/= db;
a=imageBuf0[y][index]+a;//对中心像素进行增强
a = BOUND(a,0,255);
imageBuf1[y][index]=a;
}
++index; ++m1; ++m2; ++m3;
{
//i=0 a+=imageBuf0[y-1+i][(x-1+j)*BIT_WIDTH+c] * templtlp[i*3+j];//加权求和
a-=imageBuf0[n1][m1];
a-=imageBuf0[n1][m2];
a-=imageBuf0[n1][m3];
//i=1
a-=imageBuf0[y][m1];
a+=imageBuf0[y][m2] * templtlp[4];
a-=imageBuf0[y][m3];
//i=2
a-=imageBuf0[n2][m1];
a-=imageBuf0[n2][m2];
a-=imageBuf0[n2][m3];
a/= db;
a=imageBuf0[y][index]+a;//对中心像素进行增强
a = BOUND(a,0,255);
imageBuf1[y][index]=a;
}
}
}
}
task main()
{
while(true)
{
getJoystickSettings(joystick);
if(abs(joystick.joy1_y1) > threshold)
{
//joy1y1 = Bound(joystick.joy1_y1, -100, 100);
joy1y1 = joystick.joy1_y1; //We probably don't have to limit the joystick value because we're limiting it when we set the motors
} else {
joy1y1 = 0;
}
if(abs(joystick.joy1_x2) > threshold)
{
//joy1x2 = Bound(joystick.joy1_x2, -100, 100);
joy1x2 = joystick.joy1_x2;
} else {
joy1x2 = 0;
}
/*
Logic:
ASSUME joy1y1 and joy1x2 are both positive. You want all the motors to go forward & right (it makes sense on joystick when the values are positive), so imagine which value(s) you need to negate. Then just take the individual equations and add the velocities together. When the values are negative, the equation should then make it go another direction correctly!
Go forward:
motor[frontLeft] = joy1y1;
motor[frontRight] = joy1y1;
motor[backLeft] = joy1y1;
motor[backRight] = joy1y1;
Turn right (the direction it goes when joy1x2 is positive):
motor[frontLeft] = joy1x2;
motor[frontRight] = -joy1x2;
motor[backLeft] = joy1x2;
motor[backRight] = -joy1x2;
Front-left: (motor[frontLeft] = joy1y1 & motor[frontLeft] = joy1x2)
Front-right: (motor[frontRight] = joy1y1 & motor[frontRight] = -joy1x2)
Back-left: (motor[backLeft] = joy1y1 & motor[backLeft] = joy1x2)
Back-right: (motor[backRight] = joy1y1 & motor[backRight] = -joy1x2)
Then we Bound() it so we don't set it too high of a number (maybe it might mess up the encoder. Idk)
*/
motor[frontLeft] = BOUND(joy1y1 + joy1x2, -100, 100);
motor[frontRight] = BOUND(joy1y1 - joy1x2, -100, 100);
motor[backLeft] = BOUND(joy1y1 + joy1x2, -100, 100);
motor[backRight] = BOUND(joy1y1 - joy1x2, -100, 100);
if(joy1Btn(5)==1) //Button to reset the encoders
{
nMotorEncoder[frontLeft] = 0;
nMotorEncoder[frontRight] = 0;
}
if(joy1Btn(6)==1) //Button to save the encoders to run
{
leftEncoder = nMotorEncoder[frontLeft];
rightEncoder = nMotorEncoder[frontRight];
}
if(joy1Btn(7)==1) //Button to run the saved encoders
{
if(leftEncoder != 0 && rightEncoder != 0)
{
runEncoders();
}
}
}
}
static ya_result
config_main_section_postprocess(struct config_section_descriptor_s *csd)
{
u32 port = 0;
u32 cpu_per_core = (sys_has_hyperthreading())?2:1;
int ret;
char tmp[PATH_MAX];
if(FAIL(parse_u32_check_range(g_config->server_port, &port, 1, MAX_U16, 10)))
{
port = DNS_DEFAULT_PORT;
ttylog_err("config: main: wrong dns port set in main '%s', defaulted to %d", g_config->server_port, port);
}
if(g_config->hostname_chaos == NULL)
{
#if _BSD_SOURCE || _XOPEN_SOURCE >= 500 || /* Since glibc 2.12: */ _POSIX_C_SOURCE >= 200112L
if(gethostname(tmp, sizeof(tmp)) == 0)
{
g_config->hostname_chaos = strdup(tmp);
}
else
{
osformatln(termerr,"config: main: unable to get hostname: %r", ERRNO_ERROR);
g_config->hostname_chaos = strdup("not disclosed");
}
#else
g_config->hostname_chaos = strdup("not disclosed");
#endif
}
if(g_config->thread_affinity_multiplier == 0)
{
g_config->thread_affinity_multiplier = cpu_per_core;
}
class_ch_set_hostname(g_config->hostname_chaos);
class_ch_set_id_server(g_config->serverid_chaos);
class_ch_set_version(g_config->version_chaos);
host_set_default_port_value(g_config->listen, ntohs(port));
if(g_config->server_flags & SERVER_FL_CHROOT)
{
uid_t euid = geteuid();
if(euid != 0)
{
ttylog_err("config: main: chroot has been enabled but euid is not root (%i != 0)", (int)euid);
return INVALID_STATE_ERROR;
}
}
else // disables the base-path/chroot-path feature
{
if(strcmp(g_config->chroot_path, "/") != 0)
{
free(g_config->chroot_path);
g_config->chroot_path = strdup("/");
chroot_set_path(g_config->chroot_path);
}
}
message_edns0_setmaxsize(g_config->edns0_max_size);
g_config->total_interfaces = host_address_count(g_config->listen);
if(g_config->total_interfaces > MAX_INTERFACES)
{
ttylog_err("error: more than %d listening addresses defined.", MAX_INTERFACES);
return CONFIG_TOO_MANY_HOSTS;
}
#ifndef SO_REUSEPORT
if(g_config->network_model == 1)
{
ttylog_err("error: network-model 1 requires features not available on this system (SO_REUSEPORT)");
return FEATURE_NOT_SUPPORTED;
}
#endif
g_config->axfr_retry_jitter = BOUND(AXFR_RETRY_JITTER_MIN, g_config->axfr_retry_jitter, g_config->axfr_retry_delay);
g_config->dnssec_thread_count = BOUND(1, g_config->dnssec_thread_count, sys_get_cpu_count());
if(g_config->cpu_count_override > 0)
{
sys_set_cpu_count(g_config->cpu_count_override);
}
if(g_config->thread_count_by_address == 0)
{
ttylog_err("config: single thread engine has been removed, thread-count-by-address set to 1");
g_config->thread_count_by_address = 1;
}
if(g_config->thread_count_by_address < 0)
{
g_config->thread_count_by_address = MAX(sys_get_cpu_count() / cpu_per_core, 1);
}
else if((g_config->thread_count_by_address > sys_get_cpu_count()))
{
g_config->thread_count_by_address = MAX(sys_get_cpu_count() / cpu_per_core, 1);
ttylog_err("config: bounding down thread-count-by-address to the number of physical CPUs (%d)", g_config->thread_count_by_address);
}
g_config->tcp_query_min_rate_us = g_config->tcp_query_min_rate * 0.000001;
#if HAS_DNSSEC_SUPPORT
g_config->dnssec_thread_count = BOUND(1, g_config->dnssec_thread_count, sys_get_cpu_count());
if(!IS_TYPE_PRIVATE(g_config->sig_signing_type))
{
ttylog_err("error: signing type is not in the accepted range: %hx", g_config->sig_signing_type);
return CONFIG_WRONG_SIG_TYPE;
}
if(g_config->sig_validity_interval > SIGNATURE_VALIDITY_INTERVAL_MAX)
{
ttylog_err("error: signature validity interval too high");
return CONFIG_WRONG_SIG_VALIDITY;
}
if(g_config->sig_validity_regeneration * SIGNATURE_VALIDITY_REGENERATION_S * 2 > g_config->sig_validity_interval * SIGNATURE_VALIDITY_INTERVAL_S)
{
ttylog_err("error: default signature regeneration is more than half the interval (%ds * 2 > %ds)",
g_config->sig_validity_regeneration * SIGNATURE_VALIDITY_REGENERATION_S,
g_config->sig_validity_interval * SIGNATURE_VALIDITY_INTERVAL_S);
return CONFIG_WRONG_SIG_REGEN;
}
#endif
/// @note config_main_verify_and_update_directory updates the folder with the base_path
/**
*
* @note All base paths are updated with the chroot variable so it does not
* need to be added all the time.
*
*/
const char *base_path = g_config->chroot_path;
if(FAIL(ret = config_main_verify_and_update_directory(base_path, &g_config->data_path)))
{
return ret;
}
if(FAIL(ret = config_main_verify_and_update_directory(base_path, &g_config->keys_path)))
{
return ret;
}
if(FAIL(ret = config_main_verify_and_update_directory(base_path, &g_config->log_path)))
{
return ret;
}
#if 0 /* fix */
#else
if(FAIL(ret = config_main_verify_and_update_file(base_path, &g_config->pid_file)))
{
return ret;
}
#endif
g_config->reloadable = TRUE;
if((g_config->server_flags & SERVER_FL_CHROOT) != 0)
{
if(FAIL(chroot_manage_path(&g_config->config_file, g_config->config_file, FALSE)))
{
log_warn("config file '%s' will not be accessible within the chroot jail '%s' : config reload will not work", g_config->config_file, base_path);
g_config->reloadable = FALSE;
}
}
if(FAIL(ret = config_main_verify_and_update_directory(base_path, &g_config->xfr_path)))
{
return ret;
}
#if ZDB_HAS_DNSSEC_SUPPORT
dnssec_keystore_setpath(g_config->keys_path);
dnssec_set_xfr_path(g_config->xfr_path);
journal_set_xfr_path(g_config->xfr_path);
#endif
if((logger_get_uid() != g_config->uid) || (logger_get_gid() != g_config->gid))
{
logger_set_uid(g_config->uid);
logger_set_gid(g_config->gid);
logger_reopen();
}
return SUCCESS;
}
real penalty(real x, real y)
{
#define BOUND(x) ((x) < 0 ? x : 0)
return BOUND(x-0)+BOUND(10-x)+BOUND(y-0)+BOUND(10-y)+BOUND(15-x-y)+BOUND(x*y-0.75);
}
/****************** core functions ******************/
void
toytronics_set_sp_absolute_hover_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
#ifdef SWAP_STICKS_FOR_SCOTT
// local copies to allow implementing a deadband
double rcp = rc->pitch;
double rcy = rc->yaw;
double rcr = -apply_deadband(rc->roll, SETPOINT_DEADBAND);
#else
// local copies to allow implementing a deadband
double rcp = rc->pitch;
double rcy = rc->roll;
double rcr = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
#endif
// integrate stick to get setpoint heading
setpoint.setpoint_heading += dt*SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*rcr;
wrap_to_pi(&(setpoint.setpoint_heading));
// set pitch/yaw from stick based on current body roll
double pitch_body = (rcp * SETPOINT_MAX_STICK_ANGLE_DEG + hover_pitch_trim_deg)*M_PI/180.0;
double yaw_body = rcy * SETPOINT_MAX_STICK_ANGLE_DEG*M_PI/180.0;
// rotate pitch/yaw commands into setpoint frame
setpoint.estimated_heading = get_heading_from_q_n2b(q_n2b);
double error_heading = setpoint.setpoint_heading - setpoint.estimated_heading;
wrap_to_pi(&error_heading);
// don't let setpoint drift too far
BOUND(error_heading, -setpoint_absolute_heading_bound_deg*M_PI/180.0, setpoint_absolute_heading_bound_deg*M_PI/180.0);
setpoint.setpoint_heading = setpoint.estimated_heading + error_heading;
double pitch_setpoint = pitch_body*cos(error_heading) - yaw_body*sin(error_heading);
double yaw_setpoint = pitch_body*sin(error_heading) + yaw_body*cos(error_heading);
// generate the setpoint quaternion
// start straight up
setpoint.q_n2sp.q0 = sqrt(2.0)/2.0;
setpoint.q_n2sp.q1 = 0.0;
setpoint.q_n2sp.q2 = sqrt(2.0)/2.0;
setpoint.q_n2sp.q3 = 0.0;
// rotate by heading
quat_t q_heading = {cos(0.5*setpoint.setpoint_heading), -sin(0.5*setpoint.setpoint_heading), 0.0,0.0};
setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, q_heading);
// rotate by stick commands
double total_angle = sqrt(pitch_setpoint*pitch_setpoint + yaw_setpoint*yaw_setpoint+1e-9);
setpoint.q_pitch_yaw_setpoint.q0 = cos(0.5*total_angle);
setpoint.q_pitch_yaw_setpoint.q1 = 0.0;
setpoint.q_pitch_yaw_setpoint.q2 = sin(0.5*total_angle)*pitch_setpoint/total_angle;
setpoint.q_pitch_yaw_setpoint.q3 = sin(0.5*total_angle)*yaw_setpoint/total_angle;
setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, setpoint.q_pitch_yaw_setpoint);
// calculate body to setpoint quat
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// output "pitch"/"yaw" estimated quat
quat_mult_inv(&(setpoint.q_pitch_yaw_estimated), &(setpoint.q_pitch_yaw_setpoint),
&(setpoint.q_b2sp));
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
Bool
miComputeCompositeRegion (RegionPtr pRegion,
PicturePtr pSrc,
PicturePtr pMask,
PicturePtr pDst,
INT16 xSrc,
INT16 ySrc,
INT16 xMask,
INT16 yMask,
INT16 xDst,
INT16 yDst,
CARD16 width,
CARD16 height)
{
int v;
pRegion->extents.x1 = xDst;
v = xDst + width;
pRegion->extents.x2 = BOUND(v);
pRegion->extents.y1 = yDst;
v = yDst + height;
pRegion->extents.y2 = BOUND(v);
pRegion->data = 0;
/* Check for empty operation */
if (pRegion->extents.x1 >= pRegion->extents.x2 ||
pRegion->extents.y1 >= pRegion->extents.y2)
{
pixman_region_init (pRegion);
return FALSE;
}
/* clip against dst */
if (!miClipPictureReg (pRegion, pDst->pCompositeClip, 0, 0))
{
pixman_region_fini (pRegion);
return FALSE;
}
if (pDst->alphaMap)
{
if (!miClipPictureReg (pRegion, pDst->alphaMap->pCompositeClip,
-pDst->alphaOrigin.x,
-pDst->alphaOrigin.y))
{
pixman_region_fini (pRegion);
return FALSE;
}
}
/* clip against src */
if (!miClipPictureSrc (pRegion, pSrc, xDst - xSrc, yDst - ySrc))
{
pixman_region_fini (pRegion);
return FALSE;
}
if (pSrc->alphaMap)
{
if (!miClipPictureSrc (pRegion, pSrc->alphaMap,
xDst - (xSrc - pSrc->alphaOrigin.x),
yDst - (ySrc - pSrc->alphaOrigin.y)))
{
pixman_region_fini (pRegion);
return FALSE;
}
}
/* clip against mask */
if (pMask)
{
if (!miClipPictureSrc (pRegion, pMask, xDst - xMask, yDst - yMask))
{
pixman_region_fini (pRegion);
return FALSE;
}
if (pMask->alphaMap)
{
if (!miClipPictureSrc (pRegion, pMask->alphaMap,
xDst - (xMask - pMask->alphaOrigin.x),
yDst - (yMask - pMask->alphaOrigin.y)))
{
pixman_region_fini (pRegion);
return FALSE;
}
}
}
miCompositeSourceValidate (pSrc);
if (pMask)
miCompositeSourceValidate (pMask);
return TRUE;
}
/*------------------------------------------------------------*/
cam3d cam3_init(cub3d cub,
int pmx,
int pmy,
int phx,
int tmx,
int tmy,
int thx,
float dsmax
)
/*< initialize >*/
{
int imy, imx, ihx;
float kmx, khx;
int jmx, jhx;
int ilx, ily;
float my, mx, hx, k;
/*------------------------------------------------------------*/
cam3d cam;
cam = (cam3d) sf_alloc(1,sizeof(*cam));
X2K(cub->amx,cam->bmx,pmx);
X2K(cub->amy,cam->bmy,pmy);
X2K(cub->ahx,cam->bhx,phx);
/* allocate K-domain storage */
cam->wk = sf_complexalloc4 (cam->bmx.n,cam->bmy.n,cam->bhx.n,cub->ompnth);
cam->pk = sf_complexalloc4 (cam->bmx.n,cam->bmy.n,cam->bhx.n,cub->ompnth);
/* allocate X-domain storage */
cam->wt = sf_floatalloc4 (cub->amx.n,cub->amy.n,cub->ahx.n,cub->ompnth);
cam->ksx = sf_floatalloc2(cam->bmx.n,cam->bhx.n);
cam->krx = sf_floatalloc2(cam->bmx.n,cam->bhx.n);
for (imx=0; imx<cam->bmx.n; imx++) {
jmx = KMAP(imx,cam->bmx.n);
kmx = cam->bmx.o + jmx*cam->bmx.d;
for (ihx=0; ihx<cam->bhx.n; ihx++) {
jhx = KMAP(ihx,cam->bhx.n);
khx = cam->bhx.o + jhx*cam->bhx.d;
k = 0.5*(kmx-khx);
cam->ksx[ihx][imx] = k*k; /* ksx^2 */
k = 0.5*(kmx+khx);
cam->krx[ihx][imx] = k*k; /* krx^2 */
}
}
/* precompute indices */
cam->jx = sf_intalloc(cub->amx.n);
cam->jy = sf_intalloc(cub->amy.n);
cam->is = sf_intalloc2(cub->amx.n,cub->ahx.n); /* source index */
cam->ir = sf_intalloc2(cub->amx.n,cub->ahx.n); /* receiver index */
for (imy=0; imy<cub->amy.n; imy++) {
my = cub->amy.o + imy*cub->amy.d;
ily = INDEX( my,cub->aly);
cam->jy[imy] = BOUND(ily,cub->aly.n); /* x-line index */
}
for (imx=0; imx<cub->amx.n; imx++) {
mx = cub->amx.o + imx*cub->amx.d;
ilx = INDEX( mx,cub->alx);
cam->jx[imx] = BOUND(ilx,cub->alx.n); /* i-line index */
for (ihx=0; ihx<cub->ahx.n; ihx++) {
hx = cub->ahx.o + ihx*cub->ahx.d;
ilx = INDEX(mx-hx,cub->alx);
cam->is[ihx][imx] = BOUND( ilx,cub->alx.n); /* source index */
ilx = INDEX(mx+hx,cub->alx);
cam->ir[ihx][imx] = BOUND( ilx,cub->alx.n); /* receiver index */
}
}
/* initialize FFT */
cam->f3d = ompfft3_init(cub,cam->bmx.n,cam->bmy.n,cam->bhx.n);
cam->dsmax2 = dsmax*dsmax;
cam->dsmax2*= cam->dsmax2;
return cam;
}
Bool
miComputeCompositeRegion (RegionPtr pRegion,
PicturePtr pSrc,
PicturePtr pMask,
PicturePtr pDst,
INT16 xSrc,
INT16 ySrc,
INT16 xMask,
INT16 yMask,
INT16 xDst,
INT16 yDst,
CARD16 width,
CARD16 height)
{
int v;
pRegion->extents.x1 = xDst;
v = xDst + width;
pRegion->extents.x2 = BOUND(v);
pRegion->extents.y1 = yDst;
v = yDst + height;
pRegion->extents.y2 = BOUND(v);
pRegion->data = 0;
/* Check for empty operation */
if (pRegion->extents.x1 >= pRegion->extents.x2 ||
pRegion->extents.y1 >= pRegion->extents.y2)
{
REGION_EMPTY (pDst->pDrawable->pScreen, pRegion);
return TRUE;
}
/* clip against src */
if (!miClipPictureSrc (pRegion, pSrc, xDst - xSrc, yDst - ySrc))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
if (pSrc->alphaMap)
{
if (!miClipPictureSrc (pRegion, pSrc->alphaMap,
xDst - (xSrc + pSrc->alphaOrigin.x),
yDst - (ySrc + pSrc->alphaOrigin.y)))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
}
/* clip against mask */
if (pMask)
{
if (!miClipPictureSrc (pRegion, pMask, xDst - xMask, yDst - yMask))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
if (pMask->alphaMap)
{
if (!miClipPictureSrc (pRegion, pMask->alphaMap,
xDst - (xMask + pMask->alphaOrigin.x),
yDst - (yMask + pMask->alphaOrigin.y)))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
}
}
if (!miClipPictureReg (pRegion, pDst->pCompositeClip, 0, 0))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
if (pDst->alphaMap)
{
if (!miClipPictureReg (pRegion, pDst->alphaMap->pCompositeClip,
-pDst->alphaOrigin.x,
-pDst->alphaOrigin.y))
{
REGION_UNINIT (pScreen, pRegion);
return FALSE;
}
}
return TRUE;
}
void
toytronics_set_sp_incremental_from_rc()
{
double dt = 1.0/RC_UPDATE_FREQ;
const rc_t * const rc = get_rc();
const quat_t * const q_n2b = get_q_n2b();
// local copies to allow implementing a deadband
double rcp = apply_deadband(rc->pitch, SETPOINT_DEADBAND);
double rcr = apply_deadband(rc->roll, SETPOINT_DEADBAND);
double rcy = apply_deadband(rc->yaw, SETPOINT_DEADBAND);
// rotation vector in body frame
xyz_t w_dt_body = {rcr * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
rcp * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt,
rcy * SETPOINT_MAX_STICK_DEG_PER_SEC*M_PI/180.0*dt};
// try accelerometer turn coordination
w_dt_body.z -= dt*tc_fading(q_n2b)*aerobatic_accel_tc_gain*get_y_accel();
// old body to setpoint quat q_b2sp
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// rotation vector in setpoint frame
xyz_t w_dt_sp;
rot_vec_by_quat_a2b( &w_dt_sp, &(setpoint.q_b2sp), &w_dt_body);
// form diff quat
double total_angle = sqrt( w_dt_sp.x*w_dt_sp.x
+ w_dt_sp.y*w_dt_sp.y
+ w_dt_sp.z*w_dt_sp.z
+ 1e-9);
quat_t diff_quat = {cos(total_angle/2.0),
sin(total_angle/2.0)*w_dt_sp.x/total_angle,
sin(total_angle/2.0)*w_dt_sp.y/total_angle,
sin(total_angle/2.0)*w_dt_sp.z/total_angle};
// use diff quat to update setpoint quat
setpoint.q_n2sp = quat_mult_ret(setpoint.q_n2sp, diff_quat);
// calculate body to setpoint quat
quat_inv_mult( &(setpoint.q_b2sp), q_n2b, &(setpoint.q_n2sp));
// now bound setpoint quat to not get too far away from estimated quat
BOUND(setpoint.q_b2sp.q1, -setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.x*M_PI/180.0/2.0);
BOUND(setpoint.q_b2sp.q2, -setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.y*M_PI/180.0/2.0);
BOUND(setpoint.q_b2sp.q3, -setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0, setpoint_incremental_bounds_deg.z*M_PI/180.0/2.0);
// let setpoint decay back to body
discrete_exponential_decay( &setpoint.q_b2sp.q1, setpoint_aerobatic_decay_time.x, dt );
discrete_exponential_decay( &setpoint.q_b2sp.q2, setpoint_aerobatic_decay_time.y, dt );
discrete_exponential_decay( &setpoint.q_b2sp.q3, setpoint_aerobatic_decay_time.z, dt );
// normalize
setpoint.q_b2sp.q0 = sqrt(1 - SQR(setpoint.q_b2sp.q1) - SQR(setpoint.q_b2sp.q2) - SQR(setpoint.q_b2sp.q3));
// update n2sp quat
quat_mult( &(setpoint.q_n2sp), q_n2b, &(setpoint.q_b2sp));
// set stabilization setpoint
set_stabilization_setpoint(&setpoint.q_n2sp);
}
