/*
* General method for tests that erode an input image with a morphMask and write the result to filenameOut if != NULL
*/
void testErosion(cudaPaddedImage input, cudaImage output, rect2d roi, morphMask mask, const char* filenameOut, const char* textOnError) {
performErosion(getData(input), getPitch(input), getData(output), getPitch(output), roi, mask, input.border);
if (filenameOut != NULL) {
float *host_out = copyImageToHost(output);
printf("output width: %d , height: %d\n", output.width, output.height);
savePGM(filenameOut, host_out, output.width, output.height);
freeHost(host_out, PINNED);
}
exitOnError(textOnError);
}
//! Regenerates the mip map levels of the texture. Useful after locking and
//! modifying the texture
void CD3D8Texture::regenerateMipMapLevels(void* mipmapData)
{
if (mipmapData)
{
core::dimension2du size = TextureSize;
u32 level=0;
do
{
if (size.Width>1)
size.Width /=2;
if (size.Height>1)
size.Height /=2;
++level;
IDirect3DSurface8* mipSurface = 0;
HRESULT hr = Texture->GetSurfaceLevel(level, &mipSurface);
if (FAILED(hr) || !mipSurface)
{
os::Printer::log("Could not get mipmap level", ELL_WARNING);
return;
}
D3DSURFACE_DESC mipDesc;
mipSurface->GetDesc(&mipDesc);
D3DLOCKED_RECT miplr;
// lock mipmap surface
if (FAILED(mipSurface->LockRect(&miplr, NULL, 0)))
{
mipSurface->Release();
os::Printer::log("Could not lock texture", ELL_WARNING);
return;
}
memcpy(miplr.pBits, mipmapData, size.getArea()*getPitch()/TextureSize.Width);
mipmapData = (u8*)mipmapData+size.getArea()*getPitch()/TextureSize.Width;
// unlock
mipSurface->UnlockRect();
// release
mipSurface->Release();
} while (size.Width != 1 || size.Height != 1);
}
else if (HasMipMaps)
{
// create mip maps.
#ifndef _IRR_USE_D3DXFilterTexture_
// The D3DXFilterTexture function seems to get linked wrong when
// compiling with both D3D8 and 9, causing it not to work in the D3D9 device.
// So mipmapgeneration is replaced with my own bad generation in d3d 8 when
// compiling with both D3D 8 and 9.
HRESULT hr = D3DXFilterTexture(Texture, NULL, D3DX_DEFAULT , D3DX_DEFAULT );
if (FAILED(hr))
#endif
createMipMaps();
}
}
bool SDLVideo::processOneFrame(Data data) {
SDL_Event event;
while (SDL_PollEvent(&event)) {
switch (event.type) {
case SDL_WINDOWEVENT:
if (event.window.event == SDL_WINDOWEVENT_RESIZED) {
SDL_RenderSetViewport(renderer, nullptr);
displayrect->w = event.window.data1;
displayrect->h = event.window.data2;
}
break;
case SDL_QUIT:
#ifdef _MSC_VER
GenerateConsoleCtrlEvent(CTRL_C_EVENT, 0);
#else
std::raise(SIGTERM);
#endif
return false;
}
}
auto pic = safe_cast<const DataPicture>(data);
if (pic->getFormat() != pictureFormat) {
pictureFormat = pic->getFormat();
createTexture();
}
auto const now = m_clock->now();
auto const timestamp = pic->getTime() + PREROLL_DELAY; // assume timestamps start at zero
auto const delay = std::max<int64_t>(0, timestamp - now);
auto const delayInMs = clockToTimescale(delay, 1000);
SDL_Delay((Uint32)delayInMs);
if (pictureFormat.format == YUV420P) {
SDL_UpdateYUVTexture(texture, nullptr,
pic->getPlane(0), (int)pic->getPitch(0),
pic->getPlane(1), (int)pic->getPitch(1),
pic->getPlane(2), (int)pic->getPitch(2));
} else {
SDL_UpdateTexture(texture, nullptr, pic->getPlane(0), (int)pic->getPitch(0));
}
SDL_RenderCopy(renderer, texture, nullptr, displayrect.get());
SDL_RenderPresent(renderer);
m_NumFrames++;
return true;
}
//------------------------------------------------------------------------------
// atReleaseInit() -- Init weapon data at release
//------------------------------------------------------------------------------
void Missile::atReleaseInit()
{
// First the base class will setup the initial conditions
BaseClass::atReleaseInit();
if (getDynamicsModel() == 0) {
// set initial commands
cmdPitch = (LCreal) getPitch();
cmdHeading = (LCreal) getHeading();
cmdVelocity = vpMax;
if (getTargetTrack() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetTrack()->getPosition();
trng = los.length();
trngT = trng;
}
else if (getTargetPlayer() != 0) {
// Set initial range and range dot
osg::Vec3 los = getTargetPosition();
trng = los.length();
trngT = trng;
}
else {
trng = 0;
}
// Range dot
trdot = 0;
trdotT = 0;
}
}
void radioTransceiverTask(const void *arg) {
unsigned long tv;
static unsigned long radio_last_tv2 = 0;
while (true){
#if 1
#if 0 //debug
tv=millis();
Serial.println(tv-radio_last_tv2);
radio_last_tv2=tv;
#endif
memset(transceiverBuffer, '\0', sizeof(transceiverBuffer));
sprintf(transceiverBuffer,
"@%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%3.2f:%d:%d:%d:%d:%d#",
getRoll(), getPitch(), getYaw(), getPidSp(&rollAttitudePidSettings),
getPidSp(&pitchAttitudePidSettings), getYawCenterPoint() + getPidSp(&yawAttitudePidSettings),
getRollGyro(), getPitchGyro(), getYawGyro(),
getThrottlePowerLevel(), getMotorPowerLevelCCW1(),
getMotorPowerLevelCW1(), getMotorPowerLevelCCW2(),
getMotorPowerLevelCW2());
Udp.beginPacket(broadcastIP, localPort);
Udp.write(transceiverBuffer,strlen(transceiverBuffer));
Udp.endPacket();
increasePacketAccCounter();
os_thread_yield();
delay(TRANSMIT_TIMER);
#endif
}
}
bool CMusicRoomHandler::pollInstrument(MusicInstrument instrument) {
int &arrIndex = _startPos[instrument];
if (arrIndex < 0) {
_instruments[instrument]->clear();
return false;
}
const CMusicSong &song = *_songs[instrument];
if (arrIndex >= song.size()) {
arrIndex = -1;
_instruments[instrument]->clear();
return false;
}
const CValuePair &vp = song[arrIndex];
uint duration = static_cast<int>(getAnimDuration(instrument, arrIndex) * 44100.0) & ~1;
if (vp._data == 0x7FFFFFFF || _array1[instrument]._muteControl)
_instruments[instrument]->reset(duration);
else
_instruments[instrument]->chooseWaveFile(getPitch(instrument, arrIndex), duration);
if (_array1[instrument]._directionControl == _array2[instrument]._directionControl) {
++arrIndex;
} else {
--arrIndex;
}
return true;
}
bool Camera::toggleVanityMode(bool enable)
{
// Changing the view will stop all playing animations, so if we are playing
// anything important, queue the view change for later
if (!mPreviewMode)
{
mVanityToggleQueued = true;
return false;
}
if(!mVanity.allowed && enable)
return false;
if(mVanity.enabled == enable)
return true;
mVanity.enabled = enable;
processViewChange();
float offset = mPreviewCam.offset;
Ogre::Vector3 rot(0.f, 0.f, 0.f);
if (mVanity.enabled) {
rot.x = Ogre::Degree(-30.f).valueRadians();
mMainCam.offset = mCamera->getPosition().z;
} else {
rot.x = getPitch();
offset = mMainCam.offset;
}
rot.z = getYaw();
mCamera->setPosition(0.f, 0.f, offset);
rotateCamera(rot, false);
return true;
}
bool Player::toggleVanityMode(bool enable, bool force)
{
if ((mVanity.forced && !force) ||
(!mVanity.allowed && (force || enable)))
{
return false;
} else if (mVanity.enabled == enable) {
return true;
}
mVanity.enabled = enable;
mVanity.forced = force && enable;
float offset = mPreviewCam.offset;
Ogre::Vector3 rot(0.f, 0.f, 0.f);
if (mVanity.enabled) {
rot.x = Ogre::Degree(-30.f).valueRadians();
mMainCam.offset = mCamera->getPosition().z;
setLowHeight(true);
} else {
rot.x = getPitch();
offset = mMainCam.offset;
setLowHeight(!mFirstPersonView);
}
rot.z = getYaw();
mCamera->setPosition(0.f, 0.f, offset);
rotateCamera(rot, false);
return true;
}
void ofApp::audioOut(float* output, int bufferSize, int nChannels) {
float sum = 0;
for (int i = 0; i < bufferSize; i++){
output[i * nChannels] = floatBuffer[bufferPosition * nChannels];
output[i * nChannels + 1] = floatBuffer[bufferPosition * nChannels + 1];
curBuffer[i * nChannels] = output[i * nChannels] * (1<<15);
curBuffer[i * nChannels + 1] = output[i * nChannels + 1] * (1<<15);
if(i % 2 == 0) { // drop 96khz to 48khz
bufferPosition++;
if(bufferPosition == bufferFrames) {
bufferPosition = 0;
relativePosition = 0;
exportXml();
}
}
}
timecoder_submit(&timecoder, &curBuffer[0], bufferSize);
relativeTtm[ttmPosition] = getRelative();
absoluteTtm[ttmPosition] = getAbsolute();
if(exporting) {
wholeBuffer.push_back(absoluteTtm[ttmPosition] );
}
pitchTtm[ttmPosition] = getPitch();
ttmPosition++;
if(ttmPosition == relativeTtm.size()) {
ttmPosition = 0;
}
}
AudioSource* AudioSource::clone(NodeCloneContext &context) const
{
GP_ASSERT(_buffer);
ALuint alSource = 0;
AL_CHECK( alGenSources(1, &alSource) );
if (AL_LAST_ERROR())
{
GP_ERROR("Error generating audio source.");
return NULL;
}
AudioSource* audioClone = new AudioSource(_buffer, alSource);
_buffer->addRef();
audioClone->setLooped(isLooped());
audioClone->setGain(getGain());
audioClone->setPitch(getPitch());
audioClone->setVelocity(getVelocity());
if (Node* node = getNode())
{
Node* clonedNode = context.findClonedNode(node);
if (clonedNode)
{
audioClone->setNode(clonedNode);
}
}
return audioClone;
}
bool Location::equals(const Location &other) const
{
return getRegion() == other.getRegion() &&
getX() == other.getX() &&
getY() == other.getY() &&
getZ() == other.getZ() &&
getYaw() == other.getYaw()&&
getPitch() == other.getPitch();
}
void Camera::rotateCamera(const Ogre::Vector3 &rot, bool adjust)
{
if (adjust) {
setYaw(getYaw() + rot.z);
setPitch(getPitch() + rot.x);
} else {
setYaw(rot.z);
setPitch(rot.x);
}
Ogre::Quaternion xr(Ogre::Radian(getPitch() + Ogre::Math::HALF_PI), Ogre::Vector3::UNIT_X);
if (!mVanity.enabled && !mPreviewMode) {
mCamera->getParentNode()->setOrientation(xr);
} else {
Ogre::Quaternion zr(Ogre::Radian(getYaw()), Ogre::Vector3::UNIT_Z);
mCamera->getParentNode()->setOrientation(zr * xr);
}
}
void Camera::rotateCamera(float pitch, float yaw, bool adjust)
{
if (adjust)
{
pitch += getPitch();
yaw += getYaw();
}
setYaw(yaw);
setPitch(pitch);
}
float ofApp::getAbsolute() {
float when;
float pos = timecoder_get_position(&timecoder, &when);
if(pos != -1) {
absolutePosition = pos;
} else {
float pitch = getPitch();
absolutePosition += (bufferSize / 45.7) * pitch; // not sure where the 45.7 comes from...
}
return absolutePosition;
}
inline std::ostream& operator << (std::ostream& io, base::Pose const& pose)
{
io << "Position "
<< pose.position.transpose()
<< " Orientation (RPY)"
<< getRoll(pose.orientation) << " "
<< getPitch(pose.orientation) << " "
<< getYaw(pose.orientation);
;
return io;
}
float CFBMatch::getBallPosRateBySide(FBDefs::SIDE side)
{
auto& pos = this->getBallPosition();
auto pitch = getPitch();
float rate = pos.x / pitch->getPitchWidth();
if (side == FBDefs::SIDE::RIGHT)
{
rate = 1 - rate;
}
return rate;
}
void TransformState::getProjMatrix(mat4& projMatrix) const {
double halfFov = std::atan(0.5 / getAltitude());
double topHalfSurfaceDistance = std::sin(halfFov) * getAltitude() /
std::sin(M_PI / 2.0f - getPitch() - halfFov);
// Calculate z value of the farthest fragment that should be rendered.
double farZ = std::cos(M_PI / 2.0f - getPitch()) * topHalfSurfaceDistance + getAltitude();
matrix::perspective(projMatrix, 2.0f * std::atan((size.height / 2.0f) / getAltitude()),
double(size.width) / size.height, 0.1, farZ);
matrix::translate(projMatrix, projMatrix, 0, 0, -getAltitude());
// After the rotateX, z values are in pixel units. Convert them to
// altitude unites. 1 altitude unit = the screen height.
const bool flippedY = viewportMode == ViewportMode::FlippedY;
matrix::scale(projMatrix, projMatrix, 1, flippedY ? 1 : -1,
1.0f / (rotatedNorth() ? size.width : size.height));
using NO = NorthOrientation;
switch (getNorthOrientation()) {
case NO::Rightwards: matrix::rotate_y(projMatrix, projMatrix, getPitch()); break;
case NO::Downwards: matrix::rotate_x(projMatrix, projMatrix, -getPitch()); break;
case NO::Leftwards: matrix::rotate_y(projMatrix, projMatrix, -getPitch()); break;
default: matrix::rotate_x(projMatrix, projMatrix, getPitch()); break;
}
matrix::rotate_z(projMatrix, projMatrix, getAngle() + getNorthOrientationAngle());
matrix::translate(projMatrix, projMatrix, pixel_x() - size.width / 2.0f,
pixel_y() - size.height / 2.0f, 0);
}
/**
* update attitude
*
* @param
* void
*
* @return
* void
*
*/
void attitudeUpdate(){
float yrpAttitude[3];
float pryRate[3];
float xyzAcc[3];
float xComponent[3];
float yComponent[3];
float zComponent[3];
float xyzMagnet[3];
#if CHECK_ATTITUDE_UPDATE_LOOP_TIME
struct timeval tv_c;
static struct timeval tv_l;
unsigned long timeDiff=0;
gettimeofday(&tv_c,NULL);
timeDiff=GET_USEC_TIMEDIFF(tv_c,tv_l);
_DEBUG(DEBUG_NORMAL,"attitude update duration=%ld us\n",timeDiff);
UPDATE_LAST_TIME(tv_c,tv_l);
#endif
getYawPitchRollInfo(yrpAttitude, pryRate, xyzAcc, xComponent, yComponent, zComponent,xyzMagnet);
setYaw(yrpAttitude[0]);
setRoll(yrpAttitude[1]);
setPitch(yrpAttitude[2]);
setYawGyro(-pryRate[2]);
setPitchGyro(pryRate[0]);
setRollGyro(-pryRate[1]);
setXAcc(xyzAcc[0]);
setYAcc(xyzAcc[1]);
setZAcc(xyzAcc[2]);
setXGravity(zComponent[0]);
setYGravity(zComponent[1]);
setZGravity(zComponent[2]);
setVerticalAcceleration(deadband((getXAcc() * zComponent[0] + getYAcc() * zComponent[1]
+ getZAcc() * zComponent[2] - 1.f) * 100.f,3.f) );
setXAcceleration(deadband((getXAcc() * xComponent[0] + getYAcc() * xComponent[1]
+ getZAcc() * xComponent[2]) * 100.f,3.f) );
setYAcceleration(deadband((getXAcc() * yComponent[0] + getYAcc() * yComponent[1]
+ getZAcc() * yComponent[2]) * 100.f,3.f) );
_DEBUG(DEBUG_ATTITUDE,
"(%s-%d) ATT: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n", __func__,
__LINE__, getRoll(), getPitch(), getYaw());
_DEBUG(DEBUG_GYRO,
"(%s-%d) GYRO: Roll=%3.3f Pitch=%3.3f Yaw=%3.3f\n",
__func__, __LINE__, getRollGyro(), getPitchGyro(),
getYawGyro());
_DEBUG(DEBUG_ACC, "(%s-%d) ACC: x=%3.3f y=%3.3f z=%3.3f\n",
__func__, __LINE__, getXAcc(), getYAcc(), getZAcc());
}
bool CFBMatch::isBallOnTheSide(FBDefs::SIDE side)
{
auto& pos = this->getBallPosition();
auto pitch = getPitch();
if (side == FBDefs::SIDE::LEFT)
{
return pos.x < pitch->getPitchWidth() * 0.5f;
}
else
{
return pos.x > pitch->getPitchWidth() * 0.5f;
}
return false;
}
void AIController::rotateY() {
bool decreased = false;
if (fabs(shipCoord().getY()) < EPS ||
fabs(getPitch()) >= fabs(MAX_ROTATE_POWER + fabs(shipCoord().getY()) / ROTATE_CONST)) {
// std::cout << "#RY " << getPitch() << " ";
decreased = true;
if (fabs(getPitch()) > ZERO_ANGULAR){
if (getPitch() > 0) {
minusPitch();
} else {
plusPitch();
}
}
}
if (shipCoord().getY() > EPS && getPitch() < MAX_ROTATE_POWER + shipCoord().getY() / ROTATE_CONST && !decreased) {
// std::cout << "!RY " << getPitch() << " ";
minusPitch();
}
if (shipCoord().getY() < -EPS && getPitch() > -MAX_ROTATE_POWER - shipCoord().getY() / ROTATE_CONST && !decreased) {
// std::cout << "@RY " << getPitch() << " ";
plusPitch();
}
}
void Collectible::onPrepare( float dt )
{
//update yaw
setYaw(getYaw() + (SPIN_SPEED * dt));
//get bullet representation
btMotionState *ms = getRigidBody()->getMotionState();
btTransform transform = getRigidBody()->getWorldTransform();
//update bullet representation
btQuaternion q = btQuaternion(getYaw(), getPitch(), 0.0f);
transform *= btTransform(q);
ms->setWorldTransform(transform);
}
bool
EventFilterDialog::keepEvent(Event* const &e)
{
if ((*e).isa(Note::EventType)) {
long property = 0;
// pitch
(*e).get<Int>(BaseProperties::PITCH, property);
if (!eventInRange(getPitch(), property)) {
RG_DEBUG << "EventFilterDialog::keepEvent(): rejecting event; pitch " << property
<< " out of range." << endl;
return false;
}
property = 0;
// velocity
(*e).get<Int>(BaseProperties::VELOCITY, property);
if (!EventFilterDialog::eventInRange(getVelocity(), property)) {
RG_DEBUG << "EventFilterDialog::keepEvent(): rejecting event; velocity " << property
<< " out of range." << endl;
return false;
}
property = 0;
// duration
property = m_useNotationDuration->isChecked() ? (*e).getNotationDuration() : (*e).getDuration();
if (!EventFilterDialog::eventInRange(getDuration(), property)) {
RG_DEBUG << "EventFilterDialog::keepEvent(): rejecting event; duration " << property
<< " out of range." << endl;
return false;
}
property = 0;
return true;
} else if ((*e).isa(Note::EventRestType)) {
if (m_selectRests->isChecked()) {
long property = m_useNotationDuration->isChecked() ? (*e).getNotationDuration() : (*e).getDuration();
if (!EventFilterDialog::eventInRange(getDuration(), property)) {
RG_DEBUG << "EventFilterDialog::keepEvent(): rejecting rest; duration " << property
<< " out of range." << endl;
return false;
}
return true;
}
}
return false;
}
void Camera::updateCamera(osg::Camera *cam)
{
if (mTrackingPtr.isEmpty())
return;
osg::Vec3d position = getFocalPoint();
osg::Quat orient = osg::Quat(getPitch(), osg::Vec3d(1,0,0)) * osg::Quat(getYaw(), osg::Vec3d(0,0,1));
osg::Vec3d offset = orient * osg::Vec3d(0, -mCameraDistance, 0);
position += offset;
osg::Vec3d forward = orient * osg::Vec3d(0,1,0);
osg::Vec3d up = orient * osg::Vec3d(0,0,1);
cam->setViewMatrixAsLookAt(position, position + forward, up);
}
int main(void)
{
USART1_Config();
chuck_init();
while(1){
printf("Joystick Angle: %d\r\n",getJoyAngle());
printf("Roll: %d\r\n",getRoll());
printf("Pitch: %d\r\n",getPitch());
printf("C: %d\r\n",getCstate());
printf("Z: %d\r\n",getZstate());
Delay(0xFFFFF);
}
}
/**
* get the output of attitude PID controler, this output will become a input for angular velocity PID controler
*
* @param
* void
*
* @return
* value
*
*/
void getAttitudePidOutput() {
rollAttitudeOutput = LIMIT_MIN_MAX_VALUE(
pidCalculation(&rollAttitudePidSettings, getRoll(),true,true,true),
-getGyroLimit(), getGyroLimit());
pitchAttitudeOutput =
LIMIT_MIN_MAX_VALUE(
pidCalculation(&pitchAttitudePidSettings, getPitch(),true,true,true),
-getGyroLimit(), getGyroLimit());
yawAttitudeOutput =
LIMIT_MIN_MAX_VALUE(
pidCalculation(&yawAttitudePidSettings, yawTransform(getYaw()),true,true,true),
-getGyroLimit(), getGyroLimit());
_DEBUG(DEBUG_ATTITUDE_PID_OUTPUT,
"(%s-%d) attitude pid output: roll=%.5f, pitch=%.5f, yaw=%.5f\n",
__func__, __LINE__, rollAttitudeOutput, pitchAttitudeOutput,
yawAttitudeOutput);
}
/*
* read and calculate heading with basic tilt compensation
* @returns float containing heading value -> -180 to +180
*
*/
float getHeading(void) {
readMagneto();
readAccel();
double pitch, roll;
if ((Az > 9500 && Az < 10050) || (Az < -9500 && Az > -10050)) {
pitch = roll = 0;
} else {
pitch = getPitch();
roll = getRoll();
}
int Xh = Mx * cos(pitch) + Mz * sin(pitch); // Virtual X and Virtual Y (corrected)
int Yh = Mx * sin(pitch) * sin(roll) + My * cos(roll)
- Mz * sin(roll) * cos(pitch);
float heading = (atan2f(Yh, Xh) * 180 / M_PI);
if (heading < 0) {
heading = 360 + heading;
}
return heading;
}
void Player::togglePreviewMode(bool enable)
{
if (mPreviewMode == enable) {
return;
}
mPreviewMode = enable;
float offset = mCamera->getPosition().z;
if (mPreviewMode) {
mMainCam.offset = offset;
offset = mPreviewCam.offset;
setLowHeight(true);
} else {
mPreviewCam.offset = offset;
offset = mMainCam.offset;
setLowHeight(!mFirstPersonView);
}
mCamera->setPosition(0.f, 0.f, offset);
rotateCamera(Ogre::Vector3(getPitch(), 0.f, getYaw()), false);
}
spn::ByteBuff Surface::extractAsContinuous(uint32_t dstFmt) const {
auto& myformat = getFormat();
if(dstFmt == 0)
dstFmt = myformat.format;
auto lk = lock();
int w = width(),
h = height();
// ピクセルデータが隙間なく詰まっていて、なおかつフォーマットも同じならそのままメモリをコピー
if(isContinuous() && dstFmt==myformat.format) {
auto* src = reinterpret_cast<uint8_t*>(lk.getBits());
return spn::ByteBuff(src, src + w*h*myformat.BytesPerPixel);
}
auto upFmt = MakeUPFormat(dstFmt);
size_t dstSize = w * h * upFmt->BytesPerPixel;
spn::ByteBuff dst(dstSize);
SDLEC(Trap, SDL_ConvertPixels,
w,h,
myformat.format, lk.getBits(), lk.getPitch(),
dstFmt, &dst[0], w*upFmt->BytesPerPixel);
return dst;
}
int main()
{
while (true) {
//thrust PID
error = setpoint - getHeight();
integral = integral + error * dt;
derivative = (error - previous_error);
thrust = Kp * error + Ki * integral + Kd * derivative;
//Pitch PID
error_P = 0 - getPitch();
integral_P = integral_P + error_P * dt;
derivative_P = (error_P - pre_error_P);
pitch = Kp * error_P + Ki * integral_P + Kd * derivative_P;
//Roll PID
error_R = 0 - getRoll();
integral_R = integral_R + error_R * dt;
derivative_R = (error_R - pre_error_R);
roll = Kp * error_R + Ki * integral_P + Kd * derivative_R;
//Motor controls
if (thrust > 255) thrust = 255;
else if (thrust < 0) thrust = 0;
setFR(makeValid(thrust + pitch + roll)); // front right
setFL(makeValid(thrust + pitch - roll)); // front left
setBR(makeValid(thrust - pitch + roll)); // back right
setBL(makeValid(thrust - pitch - roll)); // back left
//set previous errors
previous_error = error;
pre_error_P = error_P;
pre_error_R = error_R;
Sleep(1000 * dt);
}
}
bool SpektrumRX::initOkay(void){
// check if all sticks are centered and land selected all is okay
if (getThrottle() == 0){
if (getRoll() == 0){
if (getPitch() == 0){
if (getYaw() == 0){
if (getAuto() == 0)
return true;
else
return false;
}
else
return false;
}
else
return false;
}
else
return false;
}
else
return false;
}