bool adjustMulticopterPositionFromRCInput(void)
{
int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], posControl.rcControlsConfig->pos_hold_deadband);
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband);
if (rcPitchAdjustment || rcRollAdjustment) {
// If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID
if (posControl.navConfig->flags.user_control_mode == NAV_GPS_CRUISE) {
float rcVelX = rcPitchAdjustment * posControl.navConfig->max_manual_speed / 500;
float rcVelY = rcRollAdjustment * posControl.navConfig->max_manual_speed / 500;
// Rotate these velocities from body frame to to earth frame
float neuVelX = rcVelX * posControl.actualState.cosYaw - rcVelY * posControl.actualState.sinYaw;
float neuVelY = rcVelX * posControl.actualState.sinYaw + rcVelY * posControl.actualState.cosYaw;
// Calculate new position target, so Pos-to-Vel P-controller would yield desired velocity
posControl.desiredState.pos.V.X = posControl.actualState.pos.V.X + (neuVelX / posControl.pids.pos[X].param.kP);
posControl.desiredState.pos.V.Y = posControl.actualState.pos.V.Y + (neuVelY / posControl.pids.pos[Y].param.kP);
}
return true;
}
else {
// Adjusting finished - reset desired position to stay exactly where pilot released the stick
if (posControl.flags.isAdjustingPosition) {
t_fp_vector stopPosition;
calculateMulticopterInitialHoldPosition(&stopPosition);
setDesiredPosition(&stopPosition, 0, NAV_POS_UPDATE_XY);
}
return false;
}
}
void Enemy::checkForAndResolveCollisions()
{
CCPoint playerPos = getPosition();
CCArray* tiles = getSurroundingTilesAtPosition(playerPos);
CCObject* obj;
_onGround = false;
bool isSideHit = false;
CCARRAY_FOREACH(tiles,obj)
{
CCDictionary* dict = (CCDictionary*)obj;
Integer* x = (Integer*)dict->objectForKey("x");
Integer* y = (Integer*)dict->objectForKey("y");
float height = _map->getTileSize().height;
float width = _map->getTileSize().width;
//_debugDraw->appendRect(ccp(x->getValue(), y->getValue()), width, height);
CCRect pRect = collisionBoundingBox();
//_debugDraw->appendRect(ccp(pRect.getMinX(), pRect.getMinY()), pRect.getMaxX() - pRect.getMinX() , pRect.getMaxY() - pRect.getMinY(), 1.0f, 0.0f, 0.0f);
Integer* gid = (Integer*)dict->objectForKey("gid");
if(gid->getValue())
{
CCRect tileRect = CCRectMake((float)x->getValue(),(float)y->getValue(), _map->getTileSize().width, _map->getTileSize().height);
if(pRect.intersectsRect(tileRect))
{
CCRect intersectRect = pRect.intersectsWithRect(tileRect);
int tileIndx = tiles->getIndexOfObject(obj);
//CCLOG("tileIndx %d : " ,tileIndx);
if(tileIndx == 0)
{
//CCLOG("0 intersect ");
//_debugDraw->appendRect(ccp(intersectRect.getMinX(), intersectRect.getMinY()), intersectRect.getMaxX() - intersectRect.getMinX() , intersectRect.getMaxY() - intersectRect.getMinY(), 1.0f, 0.0f, 0.0f);
_onGround = true;
setVelocity(ccp(getVelocity().x, 0.0f));
setDesiredPosition(ccp(getDesiredPosition().x, getDesiredPosition().y + intersectRect.size.height));
//_player->setDesiredPosition(ccp(_player->getDesiredPosition().x, 13));
//CCLOG("Player box %f", tileRect.getMaxY() - pRect.getMinY());
//CCLOG("tile box %f", tileRect.getMaxY());
//CCLOG("intersectRect box %f", intersectRect.size.height);
}
else if(tileIndx == 1)
{
//CCLOG("1 intersect ");
setDesiredPosition(ccp(getDesiredPosition().x, getDesiredPosition().y - intersectRect.size.height));
}
else if(tileIndx == 2)
{
//CCLOG("2 intersect ");
isSideHit = true;
setVelocity(ccp(0.0f,getVelocity().y));
setDesiredPosition(ccp(getDesiredPosition().x + intersectRect.size.width, getDesiredPosition().y));
}
else if(tileIndx == 3)
{
//CCLOG("3 intersect ");
isSideHit = true;
setVelocity(ccp(0.0f,getVelocity().y));
setDesiredPosition(ccp(getDesiredPosition().x - intersectRect.size.width, getDesiredPosition().y));
}
else
{
if(intersectRect.size.width > intersectRect.size.height)
{
float resolutionHeight;
if(tileIndx > 5)
{
resolutionHeight = intersectRect.size.height;
setVelocity(ccp(getVelocity().x, 0.0f));
_onGround = true;
}
else
{
resolutionHeight = -intersectRect.size.height;
}
setDesiredPosition(ccp(getDesiredPosition().x, getDesiredPosition().y + resolutionHeight ));
}
else
{
float resolutionWidth;
if (tileIndx == 6 || tileIndx == 4)
{
resolutionWidth = intersectRect.size.width;
}
else
{
resolutionWidth = -intersectRect.size.width;
}
setDesiredPosition(ccp(getDesiredPosition().x + resolutionWidth, getDesiredPosition().y ));
}
//if((tileIndx == 5 || tileIndx == 4) && getVelocity().y > 0 && !isSideHit)
}
}
}
}
