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;
}
}
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return result;
}
// Altitude P-Controller
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
} else {
setVel = setVelocity;
}
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
result += errorVelocityI / 8192; // I in range +/-200
// D
result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
return result;
}
int32_t calculateBaroPid(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
uint32_t newBaroPID = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return newBaroPID;
}
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
newBaroPID = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
errorAltitudeI += (pidProfile->I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
newBaroPID += errorAltitudeI / 1024; // I in range +/-200
// D
newBaroPID -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
return newBaroPID;
}
float AdvancedJoystick::GetRawAxis (axis_t channel) {
update();
if ((channel < 6) && (channel != 3))
return applyDeadband(m_gamepad->GetRawAxis(channel));
else if (channel == 3)
return m_gamepad->GetRawAxis(3);
else
{
if (channel == AdvancedJoystick::kLeftTrigger)
{
if (m_gamepad->GetRawAxis(3) < 0)
m_gamepad->GetRawAxis(3);
else
return 0.0;
}
else
{
if (m_gamepad->GetRawAxis(3) > 0)
return fabs(m_gamepad->GetRawAxis(3));
else
return 0.0;
}
}
}
float AdvancedJoystick::GetRawAxis (axis_t channel) {
update();
if ((channel < 2) || (channel > 3))
return applyDeadband(m_gamepad->GetRawAxis(channel));
else
return m_gamepad->GetRawAxis(channel);
}
int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband)
{
int16_t stickDeflection;
stickDeflection = constrain(rawData - masterConfig.rxConfig.midrc, -500, 500);
stickDeflection = applyDeadband(stickDeflection, deadband);
return rcLookup(stickDeflection, rate);
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
fp_angles_t rpy;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
// the accel values have to be rotated into the earth frame
rpy.angles.roll = -(float)anglerad[AI_ROLL];
rpy.angles.pitch = -(float)anglerad[AI_PITCH];
rpy.angles.yaw = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
rotateV(&accel_ned.V, &rpy);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
// rotate acc into Earth frame and calculate acceleration in it
void acc_calc(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth;
float rpy[3];
t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame
rpy[0] = -(float)anglerad[ROLL];
rpy[1] = -(float)anglerad[PITCH];
rpy[2] = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
rotateV(&accel_ned.V, rpy);
if (cfg.acc_unarmedcal == 1) {
if (!f.ARMED) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (deltaT / (fc_acc + deltaT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), cfg.accxy_deadband);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), cfg.accxy_deadband);
accSum[Z] += applyDeadband(lrintf(accz_smooth), cfg.accz_deadband);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
bool adjustFixedWingAltitudeFromRCInput(void)
{
int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], posControl.rcControlsConfig->alt_hold_deadband);
if (rcAdjustment) {
// set velocity proportional to stick movement
float rcClimbRate = -rcAdjustment * posControl.navConfig->max_manual_climb_rate / (500.0f - posControl.rcControlsConfig->alt_hold_deadband);
updateAltitudeTargetFromClimbRate(rcClimbRate, CLIMB_RATE_RESET_SURFACE_TARGET);
return true;
}
else {
return false;
}
}
static void calculateVirtualPositionTarget_FW(float trackingPeriod)
{
float posErrorX = posControl.desiredState.pos.V.X - posControl.actualState.pos.V.X;
float posErrorY = posControl.desiredState.pos.V.Y - posControl.actualState.pos.V.Y;
float distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
// Limit minimum forward velocity to 1 m/s
float trackingDistance = trackingPeriod * MAX(posControl.actualState.velXY, 100.0f);
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = isApproachingLastWaypoint()
&& (distanceToActualTarget <= (posControl.navConfig->fw_loiter_radius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f);
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {
// We are closing in on a waypoint, calculate circular loiter
float loiterAngle = atan2_approx(-posErrorY, -posErrorX) + DEGREES_TO_RADIANS(45.0f);
float loiterTargetX = posControl.desiredState.pos.V.X + posControl.navConfig->fw_loiter_radius * cos_approx(loiterAngle);
float loiterTargetY = posControl.desiredState.pos.V.Y + posControl.navConfig->fw_loiter_radius * sin_approx(loiterAngle);
// We have temporary loiter target. Recalculate distance and position error
posErrorX = loiterTargetX - posControl.actualState.pos.V.X;
posErrorY = loiterTargetY - posControl.actualState.pos.V.Y;
distanceToActualTarget = sqrtf(sq(posErrorX) + sq(posErrorY));
}
// Calculate virtual waypoint
virtualDesiredPosition.V.X = posControl.actualState.pos.V.X + posErrorX * (trackingDistance / distanceToActualTarget);
virtualDesiredPosition.V.Y = posControl.actualState.pos.V.Y + posErrorY * (trackingDistance / distanceToActualTarget);
// Shift position according to pilot's ROLL input (up to max_manual_speed velocity)
if (posControl.flags.isAdjustingPosition) {
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband);
if (rcRollAdjustment) {
float rcShiftY = rcRollAdjustment * posControl.navConfig->max_manual_speed / 500.0f * trackingPeriod;
// Rotate this target shift from body frame to to earth frame and apply to position target
virtualDesiredPosition.V.X += -rcShiftY * posControl.actualState.sinYaw;
virtualDesiredPosition.V.Y += rcShiftY * posControl.actualState.cosYaw;
posControl.flags.isAdjustingPosition = true;
}
}
}
// rotate acc into Earth frame and calculate acceleration in it
void imuCalculateAcceleration(uint32_t deltaT)
{
static int32_t accZoffset = 0;
static float accz_smooth = 0;
float dT;
t_fp_vector accel_ned;
// deltaT is measured in us ticks
dT = (float)deltaT * 1e-6f;
accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2];
imuTransformVectorBodyToEarth(&accel_ned);
if (imuRuntimeConfig->acc_unarmedcal == 1) {
if (!ARMING_FLAG(ARMED)) {
accZoffset -= accZoffset / 64;
accZoffset += accel_ned.V.Z;
}
accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis
} else
accel_ned.V.Z -= acc_1G;
accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter
// apply Deadband to reduce integration drift and vibration influence
accSum[X] += applyDeadband(lrintf(accel_ned.V.X), accDeadband->xy);
accSum[Y] += applyDeadband(lrintf(accel_ned.V.Y), accDeadband->xy);
accSum[Z] += applyDeadband(lrintf(accz_smooth), accDeadband->z);
// sum up Values for later integration to get velocity and distance
accTimeSum += deltaT;
accSumCount++;
}
void calculateEstimatedAltitude(uint32_t currentTime)
{
static uint32_t previousTime;
uint32_t dTime;
int32_t baroVel;
float dt;
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
int32_t sonarAlt = -1;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#ifdef SONAR
int16_t tiltAngle;
#endif
dTime = currentTime - previousTime;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
return;
previousTime = currentTime;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
#else
BaroAlt = 0;
#endif
#ifdef SONAR
tiltAngle = calculateTiltAngle(&inclination);
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
#endif
if (sonarAlt > 0 && sonarAlt < 200) {
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= 300) {
sonarTransition = (300 - sonarAlt) / 100.0f;
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
} else {
accZ_tmp = 0;
}
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt;
accalttemp=lrintf(100*accAlt); //Checking how acc measures height // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
imuResetAccelerationSum();
#ifdef BARO
if (!isBaroCalibrationComplete()) {
return;
}
#endif
if (sonarAlt > 0 && sonarAlt < 200) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;
}
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
if (1)//!(ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband))
{
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
}//dronadrona_1200am
Temp=pidProfile->I8[PIDALT];
barometerConfig->baro_cf_alt=1-Temp/1000 ;
accZ_old = accZ_tmp;
}
int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
{
int32_t result = 0;
int32_t error;
int32_t setVel;
if (!isThrustFacingDownwards(&inclination)) {
return result;
}
// Altitude P-Controller
if(!ARMING_FLAG(ARMED))//Drona alt
{AltHold= EstAlt + 100;
//initialThrottleHold=1500;//Drona pras
}//default alt = 100cm;
if (!velocityControl) {
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
//led2_op(false);
} else {
//led2_op(true);
setVel = setVelocity;
}
/*if((setVelocity>50))
{led1_op(true);}
else
{led1_op(false);}*/
/*if(setVel>50)
{
led0_op(true);
}
else
{
led0_op(false);
}
*/
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
// I
if(ARMING_FLAG(ARMED))/*//Drona alt*/
{
errorVelocityI += (pidProfile->I8[PIDVEL] * error);
}
else
{
errorVelocityI = 0;
}/*//Drona alt*/
errorVelocityI = constrain(errorVelocityI, -(8192 * 300), (8192 * 300));
result += errorVelocityI / 8192; // I in range +/-200
// D
result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
return result;
}
void calculateEstimatedAltitude(uint32_t currentTime)
{
static uint32_t previousTime;
uint32_t dTime;
int32_t baroVel;
float dt;
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#ifdef SONAR
int16_t tiltAngle;
#endif
dTime = currentTime - previousTime;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
return;
previousTime = currentTime;
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
#ifdef SONAR
tiltAngle = calculateTiltAngle(&inclination);
sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
#endif
if (sonarAlt > 0 && sonarAlt < 200) {
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0) {
sonarTransition = (300 - sonarAlt) / 100.0f;
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
accZ_tmp = (float)accSum[2] / (float)accSumCount;
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
vel += vel_acc;
#if 1
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
accSum_reset();
if (!isBaroCalibrationComplete()) {
return;
}
if (sonarAlt > 0 && sonarAlt < 200) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;
}
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
BaroPID = calculateBaroPid(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
void calculateEstimatedAltitude(uint32_t currentTime)
{
static uint32_t previousTime;
uint32_t dTime;
int32_t baroVel;
float dt;
float vel_acc;
int32_t vel_tmp;
float accZ_tmp;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
#ifdef SONAR
int32_t sonarAlt = SONAR_OUT_OF_RANGE;
static int32_t baroAlt_offset = 0;
float sonarTransition;
#endif
dTime = currentTime - previousTime;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
return;
previousTime = currentTime;
#ifdef BARO
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
}
BaroAlt = baroCalculateAltitude();
#else
BaroAlt = 0;
#endif
#ifdef SONAR
sonarAlt = sonarRead();
sonarAlt = sonarCalculateAltitude(sonarAlt, getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// just use the SONAR
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0 && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
#endif
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / (float)accSumCount;
} else {
accZ_tmp = 0;
}
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
#ifdef DEBUG_ALT_HOLD
debug[1] = accSum[2] / accSumCount; // acceleration
debug[2] = vel; // velocity
debug[3] = accAlt; // height
#endif
imuResetAccelerationSum();
#ifdef BARO
if (!isBaroCalibrationComplete()) {
return;
}
#endif
#ifdef SONAR
if (sonarAlt > 0 && sonarAlt < sonarCfAltCm) {
// the sonar has the best range
EstAlt = BaroAlt;
} else {
EstAlt = accAlt;
}
#else
EstAlt = accAlt;
#endif
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
void calculateEstimatedAltitude(timeUs_t currentTimeUs)
{
static timeUs_t previousTimeUs = 0;
const uint32_t dTime = currentTimeUs - previousTimeUs;
if (dTime < BARO_UPDATE_FREQUENCY_40HZ) {
return;
}
previousTimeUs = currentTimeUs;
static float vel = 0.0f;
static float accAlt = 0.0f;
int32_t baroAlt = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
performBaroCalibrationCycle();
vel = 0;
accAlt = 0;
} else {
baroAlt = baroCalculateAltitude();
estimatedAltitude = baroAlt;
}
}
#endif
#ifdef SONAR
if (sensors(SENSOR_SONAR)) {
int32_t sonarAlt = sonarCalculateAltitude(sonarRead(), getCosTiltAngle());
if (sonarAlt > 0 && sonarAlt >= sonarCfAltCm && sonarAlt <= sonarMaxAltWithTiltCm) {
// SONAR in range, so use complementary filter
float sonarTransition = (float)(sonarMaxAltWithTiltCm - sonarAlt) / (sonarMaxAltWithTiltCm - sonarCfAltCm);
sonarAlt = (float)sonarAlt * sonarTransition + baroAlt * (1.0f - sonarTransition);
estimatedAltitude = sonarAlt;
}
}
#endif
float accZ_tmp = 0;
#ifdef ACC
if (sensors(SENSOR_ACC)) {
const float dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
if (accSumCount) {
accZ_tmp = (float)accSum[2] / accSumCount;
}
const float vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt) + (float)baro.BaroAlt * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_alt)); // complementary filter for altitude estimation (baro & acc)
vel += vel_acc;
estimatedAltitude = accAlt;
}
#endif
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_ACC, accSum[2] / accSumCount);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_VEL, vel);
DEBUG_SET(DEBUG_ALTITUDE, DEBUG_ALTITUDE_HEIGHT, accAlt);
imuResetAccelerationSum();
int32_t baroVel = 0;
#ifdef BARO
if (sensors(SENSOR_BARO)) {
if (!isBaroCalibrationComplete()) {
return;
}
static int32_t lastBaroAlt = 0;
baroVel = (baroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = baroAlt;
baroVel = constrain(baroVel, -1500, 1500); // constrain baro velocity +/- 1500cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
}
#endif
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel) + baroVel * (1.0f - CONVERT_PARAMETER_TO_FLOAT(barometerConfig()->baro_cf_vel));
int32_t vel_tmp = lrintf(vel);
// set vario
estimatedVario = applyDeadband(vel_tmp, 5);
static float accZ_old = 0.0f;
altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
accZ_old = accZ_tmp;
}
/**
* @brief Process inputs and arming
*
* This will always process the arming signals as for now the transmitter
* is always in charge. When a transmitter is not detected control will
* fall back to the failsafe module. If the flight mode is in tablet
* control position then control will be ceeded to that module.
*/
int32_t transmitter_control_update()
{
lastSysTime = PIOS_Thread_Systime();
float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
bool validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM];
if (settings_updated) {
settings_updated = false;
ManualControlSettingsGet(&settings);
}
/* Update channel activity monitor */
uint8_t arm_status;
FlightStatusArmedGet(&arm_status);
if (arm_status == FLIGHTSTATUS_ARMED_DISARMED) {
if (updateRcvrActivity(&activity_fsm)) {
/* Reset the aging timer because activity was detected */
lastActivityTime = lastSysTime;
}
}
if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) {
resetRcvrActivity(&activity_fsm);
lastActivityTime = lastSysTime;
}
if (ManualControlCommandReadOnly()) {
FlightTelemetryStatsData flightTelemStats;
FlightTelemetryStatsGet(&flightTelemStats);
if(flightTelemStats.Status != FLIGHTTELEMETRYSTATS_STATUS_CONNECTED) {
/* trying to fly via GCS and lost connection. fall back to transmitter */
UAVObjMetadata metadata;
ManualControlCommandGetMetadata(&metadata);
UAVObjSetAccess(&metadata, ACCESS_READWRITE);
ManualControlCommandSetMetadata(&metadata);
}
// Don't process anything else when GCS is overriding the objects
return 0;
}
if (settings.RssiType != MANUALCONTROLSETTINGS_RSSITYPE_NONE) {
int32_t value = 0;
extern uintptr_t pios_rcvr_group_map[];
switch (settings.RssiType) {
case MANUALCONTROLSETTINGS_RSSITYPE_PWM:
value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM], settings.RssiChannelNumber);
break;
case MANUALCONTROLSETTINGS_RSSITYPE_PPM:
value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM], settings.RssiChannelNumber);
break;
case MANUALCONTROLSETTINGS_RSSITYPE_ADC:
#if defined(PIOS_INCLUDE_ADC)
value = PIOS_ADC_GetChannelRaw(settings.RssiChannelNumber);
#endif
break;
case MANUALCONTROLSETTINGS_RSSITYPE_OPENLRS:
#if defined(PIOS_INCLUDE_OPENLRS_RCVR)
value = PIOS_OpenLRS_RSSI_Get();
#endif /* PIOS_INCLUDE_OPENLRS_RCVR */
break;
case MANUALCONTROLSETTINGS_RSSITYPE_FRSKYPWM:
#if defined(PIOS_INCLUDE_FRSKY_RSSI)
value = PIOS_FrSkyRssi_Get();
#endif /* PIOS_INCLUDE_FRSKY_RSSI */
break;
case MANUALCONTROLSETTINGS_RSSITYPE_SBUS:
#if defined(PIOS_INCLUDE_SBUS)
value = PIOS_RCVR_Read(pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS], settings.RssiChannelNumber);
#endif /* PIOS_INCLUDE_SBUS */
break;
}
if(value < 0)
value = 0;
if (settings.RssiMax == settings.RssiMin)
cmd.Rssi = 0;
else
cmd.Rssi = ((float)(value - settings.RssiMin)/((float)settings.RssiMax-settings.RssiMin)) * 100;
cmd.RawRssi = value;
}
bool valid_input_detected = true;
// Read channel values in us
for (uint8_t n = 0;
n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM;
++n) {
extern uintptr_t pios_rcvr_group_map[];
if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
cmd.Channel[n] = PIOS_RCVR_INVALID;
validChannel[n] = false;
} else {
cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]],
settings.ChannelNumber[n]);
}
// If a channel has timed out this is not valid data and we shouldn't update anything
// until we decide to go to failsafe
if(cmd.Channel[n] == (uint16_t) PIOS_RCVR_TIMEOUT) {
valid_input_detected = false;
validChannel[n] = false;
} else {
scaledChannel[n] = scaleChannel(n, cmd.Channel[n]);
validChannel[n] = validInputRange(n, cmd.Channel[n], CONNECTION_OFFSET);
}
}
// Check settings, if error raise alarm
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
// Check all channel mappings are valid
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_INVALID ||
// Check the driver exists
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] == (uint16_t) PIOS_RCVR_NODRIVER ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] == (uint16_t) PIOS_RCVR_NODRIVER ||
// Check the FlightModeNumber is valid
settings.FlightModeNumber < 1 || settings.FlightModeNumber > MANUALCONTROLSETTINGS_FLIGHTMODEPOSITION_NUMELEM ||
// If we've got more than one possible valid FlightMode, we require a configured FlightMode channel
((settings.FlightModeNumber > 1) && (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE)) ||
// Whenever FlightMode channel is configured, it needs to be valid regardless of FlightModeNumber settings
((settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) && (
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_INVALID ||
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] == (uint16_t) PIOS_RCVR_NODRIVER ))) {
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_SETTINGS);
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
ManualControlCommandSet(&cmd);
// Need to do this here since we don't process armed status. Since this shouldn't happen in flight (changed config)
// immediately disarm
pending_control_event = CONTROL_EVENTS_DISARM;
return -1;
}
// the block above validates the input configuration. this simply checks that the range is valid if flight mode is configured.
bool flightmode_valid_input = settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE ||
validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// because arming is optional we must determine if it is needed before checking it is valid
bool arming_valid_input = (settings.Arming < MANUALCONTROLSETTINGS_ARMING_SWITCH) ||
validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ARMING];
// decide if we have valid manual input or not
valid_input_detected &= validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] &&
validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] &&
validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] &&
validChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] &&
flightmode_valid_input &&
arming_valid_input;
// Implement hysteresis loop on connection status
if (valid_input_detected && (++connected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_TRUE;
connected_count = 0;
disconnected_count = 0;
} else if (!valid_input_detected && (++disconnected_count > 10)) {
cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE;
connected_count = 0;
disconnected_count = 0;
}
if (cmd.Connected == MANUALCONTROLCOMMAND_CONNECTED_FALSE) {
// These values are not used but just put ManualControlCommand in a sane state. When
// Connected is false, then the failsafe submodule will be in control.
cmd.Throttle = -1;
cmd.Roll = 0;
cmd.Yaw = 0;
cmd.Pitch = 0;
cmd.Collective = 0;
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_NORX);
} else if (valid_input_detected) {
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_NONE);
// Scale channels to -1 -> +1 range
cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL];
cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH];
cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW];
cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE];
cmd.ArmSwitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ARMING] > 0 ?
MANUALCONTROLCOMMAND_ARMSWITCH_ARMED : MANUALCONTROLCOMMAND_ARMSWITCH_DISARMED;
flight_mode_value = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE];
// Apply deadband for Roll/Pitch/Yaw stick inputs
if (settings.Deadband) {
applyDeadband(&cmd.Roll, settings.Deadband);
applyDeadband(&cmd.Pitch, settings.Deadband);
applyDeadband(&cmd.Yaw, settings.Deadband);
}
if(cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_INVALID &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_NODRIVER &&
cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE] != (uint16_t) PIOS_RCVR_TIMEOUT) {
cmd.Collective = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_COLLECTIVE];
}
AccessoryDesiredData accessory;
// Set Accessory 0
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0];
if(AccessoryDesiredInstSet(0, &accessory) != 0) //These are allocated later and that allocation might fail
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
}
// Set Accessory 1
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1];
if(AccessoryDesiredInstSet(1, &accessory) != 0) //These are allocated later and that allocation might fail
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
}
// Set Accessory 2
if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] !=
MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) {
accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2];
if(AccessoryDesiredInstSet(2, &accessory) != 0) //These are allocated later and that allocation might fail
set_manual_control_error(SYSTEMALARMS_MANUALCONTROL_ACCESSORY);
}
}
// Process arming outside conditional so system will disarm when disconnected. Notice this
// is processed in the _update method instead of _select method so the state system is always
// evalulated, even if not detected.
process_transmitter_events(&cmd, &settings, valid_input_detected);
// Update cmd object
ManualControlCommandSet(&cmd);
return 0;
}
int getEstimatedAltitude(void)
{
static uint32_t previousT;
uint32_t currentT = micros();
uint32_t dTime;
int32_t error;
int32_t baroVel;
int32_t vel_tmp;
int32_t BaroAlt_tmp;
int32_t setVel;
float dt;
float vel_acc;
float accZ_tmp;
static float accZ_old = 0.0f;
static float vel = 0.0f;
static float accAlt = 0.0f;
static int32_t lastBaroAlt;
static int32_t baroGroundAltitude = 0;
static int32_t baroGroundPressure = 0;
int16_t tiltAngle = max(abs(angle[ROLL]), abs(angle[PITCH]));
dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL)
return 0;
previousT = currentT;
if (calibratingB > 0) {
baroGroundPressure -= baroGroundPressure / 8;
baroGroundPressure += baroPressureSum / (cfg.baro_tab_size - 1);
baroGroundAltitude = (1.0f - powf((baroGroundPressure / 8) / 101325.0f, 0.190295f)) * 4433000.0f;
vel = 0;
accAlt = 0;
calibratingB--;
}
// calculates height from ground via baro readings
// see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
BaroAlt_tmp = lrintf((1.0f - powf((float)(baroPressureSum / (cfg.baro_tab_size - 1)) / 101325.0f, 0.190295f)) * 4433000.0f); // in cm
BaroAlt_tmp -= baroGroundAltitude;
BaroAlt = lrintf((float)BaroAlt * cfg.baro_noise_lpf + (float)BaroAlt_tmp * (1.0f - cfg.baro_noise_lpf)); // additional LPF to reduce baro noise
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
if (tiltAngle > 250)
sonarAlt = -1;
else
sonarAlt = sonarAlt * (900.0f - tiltAngle) / 900.0f;
// do sonarAlt and baroAlt fusion
if (sonarAlt > 0 && sonarAlt < 200) {
baroAlt_offset = BaroAlt - sonarAlt;
BaroAlt = sonarAlt;
} else {
BaroAlt -= baroAlt_offset;
if (sonarAlt > 0) {
sonarTransition = (300 - sonarAlt) / 100.0f;
BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
}
}
dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
// Integrator - velocity, cm/sec
accZ_tmp = (float)accSum[2] / (float)accSumCount;
vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
// Integrator - Altitude in cm
accAlt += (vel_acc * 0.5f) * dt + vel * dt; // integrate velocity to get distance (x= a/2 * t^2)
accAlt = accAlt * cfg.baro_cf_alt + (float)BaroAlt * (1.0f - cfg.baro_cf_alt); // complementary filter for Altitude estimation (baro & acc)
// when the sonar is in his best range
if (sonarAlt > 0 && sonarAlt < 200)
EstAlt = BaroAlt;
else
EstAlt = accAlt;
vel += vel_acc;
#if 0
debug[0] = accSum[2] / accSumCount; // acceleration
debug[1] = vel; // velocity
debug[2] = accAlt; // height
#endif
accSum_reset();
baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = BaroAlt;
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * cfg.baro_cf_vel + baroVel * (1 - cfg.baro_cf_vel);
vel_tmp = lrintf(vel);
// set vario
vario = applyDeadband(vel_tmp, 5);
if (tiltAngle < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
// Altitude P-Controller
error = constrain(AltHold - EstAlt, -500, 500);
error = applyDeadband(error, 10); // remove small P parametr to reduce noise near zero position
setVel = constrain((cfg.P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
// Velocity PID-Controller
// P
error = setVel - vel_tmp;
BaroPID = constrain((cfg.P8[PIDVEL] * error / 32), -300, +300);
// I
errorAltitudeI += (cfg.I8[PIDVEL] * error) / 8;
errorAltitudeI = constrain(errorAltitudeI, -(1024 * 200), (1024 * 200));
BaroPID += errorAltitudeI / 1024; // I in range +/-200
// D
BaroPID -= constrain(cfg.D8[PIDVEL] * (accZ_tmp + accZ_old) / 64, -150, 150);
} else {
BaroPID = 0;
}
accZ_old = accZ_tmp;
return 1;
}
void DriveTrain::Drive(float throttle, float turn, bool isQuickTurn) {
// Modified Cheesy Drive; base code courtesy of FRC Team 254
throttle *= -1;
// Limit values to [-1 .. 1]
throttle = limit(throttle, 1.f);
turn = limit(turn, 1.f);
/* Apply joystick deadband
* (Negate turn since joystick X-axis is reversed)
*/
throttle = applyDeadband(throttle);
turn = applyDeadband(turn);
double negInertia = turn - m_oldTurn;
m_oldTurn = turn;
float turnNonLinearity = m_settings.GetDouble("TURN_NON_LINEARITY");
/* Apply a sine function that's scaled to make turning sensitivity feel
* better. turnNonLinearity should never be zero, but can be close
*/
turn = std::sin(M_PI / 2.0 * turnNonLinearity * turn) /
std::sin(M_PI / 2.0 * turnNonLinearity);
double angularPower = 0.f;
double linearPower = throttle;
double leftPwm = linearPower, rightPwm = linearPower;
// Negative inertia!
double negInertiaScalar;
if (turn * negInertia > 0) {
negInertiaScalar = m_settings.GetDouble("INERTIA_DAMPEN");
} else {
if (std::fabs(turn) > 0.65) {
negInertiaScalar = m_settings.GetDouble("INERTIA_HIGH_TURN");
} else {
negInertiaScalar = m_settings.GetDouble("INERTIA_LOW_TURN");
}
}
m_negInertiaAccumulator +=
negInertia * negInertiaScalar; // adds negInertiaPower
// Apply negative inertia
turn += m_negInertiaAccumulator;
if (m_negInertiaAccumulator > 1) {
m_negInertiaAccumulator -= 1;
} else if (m_negInertiaAccumulator < -1) {
m_negInertiaAccumulator += 1;
} else {
m_negInertiaAccumulator = 0;
}
// QuickTurn!
if (isQuickTurn) {
if (std::fabs(linearPower) < 0.2) {
double alpha = 0.1;
m_quickStopAccumulator = (1 - alpha) * m_quickStopAccumulator +
alpha * limit(turn, 1.f) * 5;
}
angularPower = turn;
} else {
angularPower =
std::fabs(throttle) * turn * m_sensitivity - m_quickStopAccumulator;
if (m_quickStopAccumulator > 1) {
m_quickStopAccumulator -= 1;
} else if (m_quickStopAccumulator < -1) {
m_quickStopAccumulator += 1;
} else {
m_quickStopAccumulator = 0.0;
}
}
// Adjust straight path for turn
leftPwm += angularPower;
rightPwm -= angularPower;
// Limit PWM bounds to [-1..1]
if (leftPwm > 1.0) {
// If overpowered turning enabled
if (isQuickTurn) {
rightPwm -= (leftPwm - 1.f);
}
leftPwm = 1.0;
} else if (rightPwm > 1.0) {
// If overpowered turning enabled
if (isQuickTurn) {
leftPwm -= (rightPwm - 1.f);
}
rightPwm = 1.0;
} else if (leftPwm < -1.0) {
// If overpowered turning enabled
if (isQuickTurn) {
rightPwm += (-leftPwm - 1.f);
}
leftPwm = -1.0;
} else if (rightPwm < -1.0) {
// If overpowered turning enabled
if (isQuickTurn) {
leftPwm += (-rightPwm - 1.f);
}
rightPwm = -1.0;
}
m_leftGrbx.setManual(leftPwm);
m_rightGrbx.setManual(rightPwm);
}
bool adjustFixedWingPositionFromRCInput(void)
{
int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], posControl.rcControlsConfig->pos_hold_deadband);
return (rcRollAdjustment);
}
