int DMP::getAttitude()
{
if (!dmpReady) return -1;
// wait for FIFO count > 42 bits
do {
fifoCount = mpu.getFIFOCount();
}while (fifoCount<42);
if (fifoCount == 1024) {
// reset so we can continue cleanly
mpu.resetFIFO();
printf("FIFO overflow!\n");
return -1;
// otherwise, check for DMP data ready interrupt
//(this should happen frequently)
} else {
//read packet from fifo
mpu.getFIFOBytes(fifoBuffer, packetSize);
mpu.dmpGetQuaternion(&q, fifoBuffer);
mpu.dmpGetGravity(&gravity, &q);
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
for (int i=0;i<DIM;i++){
//offset removal
ypr[i]-=m_ypr_off[i];
//scaling for output in degrees
ypr[i]*=180/M_PI;
}
//printf(" %7.2f %7.2f %7.2f\n",ypr[0],ypr[1],ypr[2]);
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of ROLL, MPU is attached upside down
ypr[2]*=-1.0;
mpu.dmpGetGyro(g, fifoBuffer);
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
}
// printf("gyro %7.2f %7.2f %7.2f \n", (float)g[0]/131.0,
// (float)g[1]/131.0,
// (float)g[2]/131.0);
return 0;
}
}
ThrottleValues ModeStab::ComputeThrottle(float dt, const RCValues &rc,
float yaw, float pitch, float roll,
float omega_yaw, float omega_pitch, float omega_roll){
float rcp = -map(rc.pitch, 1000, 2000, -45, 45); //(pitch - 1500.0);
float rcr = map(rc.roll, 1000, 2000, -45, 45); //(roll - 1500.0);
float rcy = -map(rc.yaw, 1000, 2000, -50, 50); //(yaw - 1500.0);
// calculate desired rotation rate in degrees / sec
float sp = mPID[PID_STAB_PITCH].get_pid(rcp - pitch, dt);
float sr = mPID[PID_STAB_ROLL].get_pid(rcr - roll, dt);
float sy = mPID[PID_STAB_YAW].get_pid(wrap_180(rcy - mTargetYaw), dt);
if(abs(rc.yaw - 1500) > 10){
sy = rcy;
mTargetYaw = yaw;
}
// calculate the actual rate based on current gyro rate in degrees
float rp = mPID[PID_RATE_PITCH ].get_pid(sp - omega_pitch, dt);
float rr = mPID[PID_RATE_ROLL ].get_pid(sr - omega_roll, dt);
float ry = mPID[PID_RATE_YAW ].get_pid(sy - omega_yaw, dt);
// H-copter control test
/*
return glm::i16vec4(
// front left
- rr - rp + ry,
// back right
+ rr + rp - ry,
// back left
- rr + rp - ry,
// front right
+ rr - rp + ry);
*/
return glm::i16vec4(
// front
+ rp + ry,
// back
- rp + ry,
// left
+ rr - ry,
// right
- rr - ry
);
}
bool AP_Proximity_LightWareSF40C::convert_angle_to_sector(float angle_degrees, uint8_t §or) const
{
// sanity check angle
if (angle_degrees > 360.0f || angle_degrees < -180.0f) {
return false;
}
// convert to 0 ~ 360
if (angle_degrees < 0.0f) {
angle_degrees += 360.0f;
}
bool closest_found = false;
uint8_t closest_sector;
float closest_angle;
// search for which sector angle_degrees falls into
for (uint8_t i = 0; i < _num_sectors; i++) {
float angle_diff = fabsf(wrap_180(_sector_middle_deg[i] - angle_degrees));
// record if closest
if (!closest_found || angle_diff < closest_angle) {
closest_found = true;
closest_sector = i;
closest_angle = angle_diff;
}
if (fabsf(angle_diff) <= _sector_width_deg[i] / 2.0f) {
sector = i;
return true;
}
}
// angle_degrees might have been within a gap between sectors
if (closest_found) {
sector = closest_sector;
return true;
}
return false;
}
int ms_update() {
if (!dmpReady) {
printf("Error: DMP not ready!!\n");
return -1;
}
while (dmp_read_fifo(g,a,_q,&sensors,&fifoCount)!=0); //gyro and accel can be null because of being disabled in the efeatures
q = _q;
GetGravity(&gravity, &q);
GetYawPitchRoll(ypr, &q, &gravity);
mpu_get_temperature(&t);
temp=(float)t/65536L;
mpu_get_compass_reg(c);
//scaling for degrees output
for (int i=0;i<DIM;i++){
ypr[i]*=180/M_PI;
}
//unwrap yaw when it reaches 180
ypr[0] = wrap_180(ypr[0]);
//change sign of Pitch, MPU is attached upside down
ypr[1]*=-1.0;
//0=gyroX, 1=gyroY, 2=gyroZ
//swapped to match Yaw,Pitch,Roll
//Scaled from deg/s to get tr/s
for (int i=0;i<DIM;i++){
gyro[i] = (float)(g[DIM-i-1])/131.0/360.0;
accel[i] = (float)(a[DIM-i-1]);
compass[i] = (float)(c[DIM-i-1]);
}
return 0;
}
// rotate vector to correct for beacon system yaw orientation
Vector3f AP_Beacon_Backend::correct_for_orient_yaw(const Vector3f &vector)
{
// exit immediately if no correction
if (_frontend.orient_yaw == 0) {
return vector;
}
// check for change in parameter value and update constants
if (orient_yaw_deg != _frontend.orient_yaw) {
_frontend.orient_yaw = wrap_180(_frontend.orient_yaw.get());
// calculate rotation constants
orient_yaw_deg = _frontend.orient_yaw;
orient_cos_yaw = cosf(radians(orient_yaw_deg));
orient_sin_yaw = sinf(radians(orient_yaw_deg));
}
// rotate x,y by -orient_yaw
Vector3f vec_rotated;
vec_rotated.x = vector.x*orient_cos_yaw - vector.y*orient_sin_yaw;
vec_rotated.y = vector.x*orient_sin_yaw + vector.y*orient_cos_yaw;
vec_rotated.z = vector.z;
return vec_rotated;
}
// *Somehow* modified by me..
// Besides mwii credit must go to Sebbi, BRM! Hopefully they condone mentioning them above my trash.
// Sebbi for his rotation of the acc vector and BRM for his normalization ideas.
static void getEstimatedAttitude(void)
{
static t_fp_vector EstG, EstM;
static float accLPFALT[3], accLPFGPS[3], Tilt_25deg, INVsens1G;
static float INV_GYR_CMPF_FACTOR, INV_GYR_CMPFM_FACTOR, ACC_GPS_RC, ACC_ALT_RC, ACC_RC;
static uint32_t previousT, UpsDwnTimer;
static bool init = false;
float tmp[3], scale, deltaGyroAngle[3], ACCALTFac, ACCGPSFac, ACCFac, rollRAD, pitchRAD;
float NormFst = 0.0f, NormScnd, NormR, A, B, cr, sr, cp, sp, cy, sy, spcy, spsy, acc_south, acc_west, acc_up, FAC;
uint8_t i;
uint32_t tmpu32, currentT = micros();
ACCDeltaTimeINS = (float)(currentT - previousT) * 0.000001f;
previousT = currentT;
if(!init) // Setup variables & constants
{
init = true;
INVsens1G = 1.0f / cfg.sens_1G;
Tilt_25deg = cosf(25.0f * RADX);
INV_GYR_CMPF_FACTOR = 1.0f / (float)(cfg.gy_cmpf + 1); // Default 400
INV_GYR_CMPFM_FACTOR = 1.0f / (float)(cfg.gy_cmpfm + 1); // Default 200
ACC_ALT_RC = 0.5f / (M_PI * cfg.acc_altlpfhz); // Default 10 Hz
ACC_RC = 0.5f / (M_PI * cfg.acc_lpfhz); // Default 0,536 Hz
ACC_GPS_RC = 0.5f / (M_PI * cfg.acc_gpslpfhz); // Default 5 Hz
for (i = 0; i < 3; i++) // Preset some values to reduce runup time
{
tmp[0] = accADC[i] * INVsens1G;
accSmooth[i] = tmp[0];
accLPFGPS[i] = tmp[0];
accLPFALT[i] = tmp[0];
EstG.A[i] = tmp[0] * 0.5f;
}
}
ACCALTFac = ACCDeltaTimeINS / (ACC_ALT_RC + ACCDeltaTimeINS); // Adjust LPF to cycle time / do Hz cut off
ACCGPSFac = ACCDeltaTimeINS / (ACC_GPS_RC + ACCDeltaTimeINS); // Adjust LPF to cycle time / do Hz cut off
ACCFac = ACCDeltaTimeINS / (ACC_RC + ACCDeltaTimeINS);
scale = ACCDeltaTimeINS * GyroScale; // SCALE CHANGED TO RAD per SECONDS, DAMN
for (i = 0; i < 3; i++)
{
tmp[0] = accADC[i] * INVsens1G; // Reference all to 1G here
accLPFGPS[i] += ACCGPSFac * (tmp[0] - accLPFGPS[i]); // For GPS
accLPFALT[i] += ACCALTFac * (tmp[0] - accLPFALT[i]); // For Althold
accSmooth[i] += ACCFac * (tmp[0] - accSmooth[i]); // For Gyrodrift correction
NormFst += accSmooth[i] * accSmooth[i];
deltaGyroAngle[i] = gyroADC[i] * scale; // deltaGyroAngle is in RAD
}
RotGravAndMag(&EstG.V, &EstM.V, deltaGyroAngle); // Rotate Grav&Mag together to avoid doublecalculation
NormFst = sqrtf(NormFst);
tmpu32 = (uint32_t)(NormFst * 1000.0f); // Make it "shorter" for comparison in temp variable
NormScnd = sqrtf(EstG.A[0] * EstG.A[0] + EstG.A[1] * EstG.A[1] + EstG.A[2] * EstG.A[2]);
if (NormScnd) NormR = 1.0f / NormScnd;
else NormR = INVsens1G; // Feed vanilla value in that rare case, to let angle calculation always happen
for (i = 0; i < 3; i++) tmp[i] = EstG.A[i] * NormR; // tmp[0..2] contains normalized EstG
if (850 < tmpu32 && tmpu32 < 1150) // Gyro drift correction if ACC between 0.85G and 1.15G else skip filter, as EstV already rotated by Gyro
{
NormR = 1.0f / NormFst; // We just cmp filter together normalized vectors here. Div 0 not possible here.
for (i = 0; i < 3; i++) EstG.A[i] = (tmp[i] * (float)cfg.gy_cmpf + accSmooth[i] * NormR) * INV_GYR_CMPF_FACTOR;
}
rollRAD = atan2f(tmp[0], tmp[2]); // Note: One cycle after successful cmpf is done with old values.
pitchRAD = asinf(-tmp[1]); // Has to have the "wrong sign" relative to angle[PITCH]
angle[ROLL] = rollRAD * RADtoDEG10; // Use rounded values, eliminate jitter for main PID I and D
angle[PITCH] = -pitchRAD * RADtoDEG10;
TiltValue = cosf(rollRAD) * cosf(pitchRAD); // We do this correctly here
if (TiltValue >= 0) UpsDwnTimer = 0;
else if(!UpsDwnTimer) UpsDwnTimer = currentT + 20000; // Use Timer here to make absolutely sure we are upsidedown
if (UpsDwnTimer && currentT > UpsDwnTimer) UpsideDown = true;
else UpsideDown = false;
if (TiltValue > Tilt_25deg) f.SMALL_ANGLES_25 = 1;
else f.SMALL_ANGLES_25 = 0;
cr = cosf(rollRAD);
sr = sinf(rollRAD);
cp = cosf(pitchRAD);
sp = sinf(pitchRAD);
if (cfg.mag_calibrated) // mag_calibrated can just be true if MAG present
{
NormFst = sqrtf(EstM.A[0] * EstM.A[0] + EstM.A[1] * EstM.A[1] + EstM.A[2] * EstM.A[2]);
if (NormFst) NormR = 1.0f / NormFst;
else NormR = 1.0f; // Vanilla value for rare case
for (i = 0; i < 3; i++) tmp[i] = EstM.A[i] * NormR; // tmp[0..2] contains normalized EstM
if(HaveNewMag) // Only do Complementary filter when new MAG data are available
{
HaveNewMag = false;
NormFst = sqrtf(magADCfloat[0] * magADCfloat[0] + magADCfloat[1] * magADCfloat[1] + magADCfloat[2] * magADCfloat[2]);
if (NormFst) NormR = 1.0f / NormFst;
else NormR = 1.0f;
for (i = 0; i < 3; i++) EstM.A[i] = (tmp[i] * (float)cfg.gy_cmpfm + magADCfloat[i] * NormR) * INV_GYR_CMPFM_FACTOR;
}
A = tmp[1] * cp + tmp[0] * sr * sp + tmp[2] * cr * sp;
B = tmp[0] * cr - tmp[2] * sr;
heading = wrap_180(atan2f(-B, A) * RADtoDEG + magneticDeclination); // Get rad to Degree and add declination (note: without *10) // Wrap to -180 0 +180 Degree
} else heading = 0; // if no mag or not calibrated do bodyframe below
tmp[0] = heading * RADX; // Do GPS INS rotate ACC X/Y to earthframe no centrifugal comp. yet
cy = cosf(tmp[0]);
sy = sinf(tmp[0]);
cos_yaw_x = cy; // Store for general use
sin_yaw_y = sy; // Store for general use
spcy = sp * cy;
spsy = sp * sy;
FAC = 980.665f * ACCDeltaTimeINS; // vel factor for normalized output tmp3 = (9.80665f * (float)ACCDeltaTime) / 10000.0f;
acc_up = ((-sp) * accLPFALT[1] + sr * cp * accLPFALT[0] + cp * cr * accLPFALT[2]) - 1;// -1G
if(GroundAltInitialized) vario += acc_up * FAC * constrain(TiltValue + 0.05, 0.5f, 1.0f);// Positive when moving Up. Just do Vario when Baro completely initialized. Empirical hightdrop reduction on tilt.
acc_south = cp * cy * accLPFGPS[1] + (sr * spcy - cr * sy) * accLPFGPS[0] + ( sr * sy + cr * spcy) * accLPFGPS[2];
acc_west = cp * sy * accLPFGPS[1] + (cr * cy + sr * spsy) * accLPFGPS[0] + (-sr * cy + cr * spsy) * accLPFGPS[2];
ACC_speed[LAT] -= acc_south * FAC; // Positive when moving North cm/sec when no MAG this is speed to the front
ACC_speed[LON] -= acc_west * FAC; // Positive when moving East cm/sec when no MAG this is speed to the right
}
/*
update navigation for landing
*/
bool AP_Landing_Deepstall::verify_land(const Location &prev_WP_loc, Location &next_WP_loc, const Location ¤t_loc,
const float height, const float sink_rate, const float wp_proportion, const uint32_t last_flying_ms,
const bool is_armed, const bool is_flying, const bool rangefinder_state_in_range)
{
switch (stage) {
case DEEPSTALL_STAGE_FLY_TO_LANDING:
if (get_distance(current_loc, landing_point) > fabsf(2 * landing.aparm.loiter_radius)) {
landing.nav_controller->update_waypoint(current_loc, landing_point);
return false;
}
stage = DEEPSTALL_STAGE_ESTIMATE_WIND;
loiter_sum_cd = 0; // reset the loiter counter
FALLTHROUGH;
case DEEPSTALL_STAGE_ESTIMATE_WIND:
{
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1);
if (!landing.nav_controller->reached_loiter_target() || (fabsf(height) > DEEPSTALL_LOITER_ALT_TOLERANCE)) {
// wait until the altitude is correct before considering a breakout
return false;
}
// only count loiter progress when within the target altitude
int32_t target_bearing = landing.nav_controller->target_bearing_cd();
int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
delta *= (landing_point.flags.loiter_ccw ? -1 : 1);
if (delta > 0) { // only accumulate turns in the correct direction
loiter_sum_cd += delta;
}
last_target_bearing = target_bearing;
if (loiter_sum_cd < 36000) {
// wait until we've done at least one complete loiter at the correct altitude
return false;
}
stage = DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT;
loiter_sum_cd = 0; // reset the loiter counter
FALLTHROUGH;
}
case DEEPSTALL_STAGE_WAIT_FOR_BREAKOUT:
// rebuild the approach path if we have done less then a full circle to allow it to be
// more into the wind as the estimator continues to refine itself, and allow better
// compensation on windy days. This is limited to a single full circle though, as on
// a no wind day you could be in this loop forever otherwise.
if (loiter_sum_cd < 36000) {
build_approach_path(false);
}
if (!verify_breakout(current_loc, arc_entry, height)) {
int32_t target_bearing = landing.nav_controller->target_bearing_cd();
int32_t delta = wrap_180_cd(target_bearing - last_target_bearing);
if (delta > 0) { // only accumulate turns in the correct direction
loiter_sum_cd += delta;
}
last_target_bearing = target_bearing;
landing.nav_controller->update_loiter(landing_point, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1);
return false;
}
stage = DEEPSTALL_STAGE_FLY_TO_ARC;
memcpy(&breakout_location, ¤t_loc, sizeof(Location));
FALLTHROUGH;
case DEEPSTALL_STAGE_FLY_TO_ARC:
if (get_distance(current_loc, arc_entry) > 2 * landing.aparm.loiter_radius) {
landing.nav_controller->update_waypoint(breakout_location, arc_entry);
return false;
}
stage = DEEPSTALL_STAGE_ARC;
FALLTHROUGH;
case DEEPSTALL_STAGE_ARC:
{
Vector2f groundspeed = landing.ahrs.groundspeed_vector();
if (!landing.nav_controller->reached_loiter_target() ||
(fabsf(wrap_180(target_heading_deg -
degrees(atan2f(-groundspeed.y, -groundspeed.x) + M_PI))) >= 10.0f)) {
landing.nav_controller->update_loiter(arc, landing.aparm.loiter_radius, landing_point.flags.loiter_ccw ? -1 : 1);
return false;
}
stage = DEEPSTALL_STAGE_APPROACH;
}
FALLTHROUGH;
case DEEPSTALL_STAGE_APPROACH:
{
Location entry_point;
landing.nav_controller->update_waypoint(arc_exit, extended_approach);
float relative_alt_D;
landing.ahrs.get_relative_position_D_home(relative_alt_D);
const float travel_distance = predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, false);
memcpy(&entry_point, &landing_point, sizeof(Location));
location_update(entry_point, target_heading_deg + 180.0, travel_distance);
if (!location_passed_point(current_loc, arc_exit, entry_point)) {
if (location_passed_point(current_loc, arc_exit, extended_approach)) {
// this should never happen, but prevent against an indefinite fly away
stage = DEEPSTALL_STAGE_FLY_TO_LANDING;
}
return false;
}
predict_travel_distance(landing.ahrs.wind_estimate(), -relative_alt_D, true);
stage = DEEPSTALL_STAGE_LAND;
stall_entry_time = AP_HAL::millis();
const SRV_Channel* elevator = SRV_Channels::get_channel_for(SRV_Channel::k_elevator);
if (elevator != nullptr) {
// take the last used elevator angle as the starting deflection
// don't worry about bailing here if the elevator channel can't be found
// that will be handled within override_servos
initial_elevator_pwm = elevator->get_output_pwm();
}
}
FALLTHROUGH;
case DEEPSTALL_STAGE_LAND:
// while in deepstall the only thing verify needs to keep the extended approach point sufficently far away
landing.nav_controller->update_waypoint(current_loc, extended_approach);
landing.disarm_if_autoland_complete_fn();
return false;
default:
return true;
}
}
/*
steering rate controller. Returns servo out -4500 to 4500 given
desired yaw rate in degrees/sec. Positive yaw rate means clockwise yaw.
*/
int32_t AP_SteerController::get_steering_out_rate(float desired_rate)
{
uint32_t tnow = AP_HAL::millis();
uint32_t dt = tnow - _last_t;
if (_last_t == 0 || dt > 1000) {
dt = 0;
}
_last_t = tnow;
float speed = _ahrs.groundspeed();
if (speed < _minspeed) {
// assume a minimum speed. This stops osciallations when first starting to move
speed = _minspeed;
}
// this is a linear approximation of the inverse steering
// equation for a ground vehicle. It returns steering as an angle from -45 to 45
float scaler = 1.0f / speed;
_pid_info.desired = desired_rate;
// Calculate the steering rate error (deg/sec) and apply gain scaler
// We do this in earth frame to allow for rover leaning over in hard corners
float yaw_rate_earth = ToDeg(_ahrs.get_yaw_rate_earth());
if (_reverse) {
yaw_rate_earth = wrap_180(180 + yaw_rate_earth);
}
float rate_error = (desired_rate - yaw_rate_earth) * scaler;
// Calculate equivalent gains so that values for K_P and K_I can be taken across from the old PID law
// No conversion is required for K_D
float ki_rate = _K_I * _tau * 45.0f;
float kp_ff = MAX((_K_P - _K_I * _tau) * _tau - _K_D , 0) * 45.0f;
float k_ff = _K_FF * 45.0f;
float delta_time = (float)dt * 0.001f;
// Multiply roll rate error by _ki_rate and integrate
// Don't integrate if in stabilise mode as the integrator will wind up against the pilots inputs
if (ki_rate > 0 && speed >= _minspeed) {
// only integrate if gain and time step are positive.
if (dt > 0) {
float integrator_delta = rate_error * ki_rate * delta_time * scaler;
// prevent the integrator from increasing if steering defln demand is above the upper limit
if (_last_out < -45) {
integrator_delta = MAX(integrator_delta , 0);
} else if (_last_out > 45) {
// prevent the integrator from decreasing if steering defln demand is below the lower limit
integrator_delta = MIN(integrator_delta, 0);
}
_pid_info.I += integrator_delta;
}
} else {
_pid_info.I = 0;
}
// Scale the integration limit
float intLimScaled = _imax * 0.01f;
// Constrain the integrator state
_pid_info.I = constrain_float(_pid_info.I, -intLimScaled, intLimScaled);
_pid_info.D = rate_error * _K_D * 4.0f;
_pid_info.P = (ToRad(desired_rate) * kp_ff) * scaler;
_pid_info.FF = (ToRad(desired_rate) * k_ff) * scaler;
// Calculate the demanded control surface deflection
_last_out = _pid_info.D + _pid_info.FF + _pid_info.P + _pid_info.I;
// Convert to centi-degrees and constrain
return constrain_float(_last_out * 100, -4500, 4500);
}
static void getEstimatedAttitude(void)
{
static t_fp_vector EstG, EstM;
static float cms[3] = {0.0f, 0.0f, 0.0f}, Tilt_25deg, AccScaleCMSS;
static float INV_GY_CMPF, INV_GY_CMPFM, ACC_GPS_RC, ACC_ALT_RC, ACC_RC;
static uint32_t UpsDwnTimer, SQ1G;
static bool init = false;
float tmp[3], DeltGyRad[3], rollRAD, pitchRAD;
float Norm, A, B, cr, sr, cp, sp, spcy, spsy, accycp, CmsFac;
uint8_t i;
uint32_t tmpu32 = 0;
if(!init) // Setup variables & constants
{
init = true;
AccScaleCMSS = OneGcmss / (float)cfg.sens_1G; // scale to cm/ss
SQ1G = (int32_t)cfg.sens_1G * (int32_t)cfg.sens_1G;
Tilt_25deg = cosf(25.0f * RADX);
INV_GY_CMPF = 1.0f / (float)(cfg.gy_gcmpf + 1); // Default 400
INV_GY_CMPFM = 1.0f / (float)(cfg.gy_mcmpf + 1); // Default 200
tmp[0] = 0.5f / M_PI;
ACC_RC = tmp[0] / cfg.acc_lpfhz; // Default 0,536 Hz
ACC_ALT_RC = tmp[0] / (float)cfg.acc_altlpfhz; // Default 10 Hz
ACC_GPS_RC = tmp[0] / (float)cfg.acc_gpslpfhz; // Default 5 Hz
for (i = 0; i < 3; i++) // Preset some values to reduce runup time
{
accSmooth[i] = accADC[i];
EstG.A[i] = accSmooth[i];
EstM.A[i] = magADCfloat[i]; // Using /2 for more stability
}
}
CmsFac = ACCDeltaTimeINS * AccScaleCMSS; // We need that factor for INS below
tmp[0] = ACCDeltaTimeINS / (ACC_RC + ACCDeltaTimeINS);
for (i = 0; i < 3; i++)
{
accSmooth[i] += tmp[0] * (accADC[i] - accSmooth[i]); // For Gyrodrift correction
DeltGyRad[i] = (ACCDeltaTimeINS * gyroADC[i]) * 0.0625f; // gyroADC delivered in 16 * rad/s
tmpu32 += (int32_t)accSmooth[i] * (int32_t)accSmooth[i]; // Accumulate ACC magnitude there
}
RotGravAndMag(&EstG.V, &EstM.V, DeltGyRad); // Rotate Grav & Mag together to avoid doublecalculation
tmpu32 = (tmpu32 * 100) / SQ1G; // accMag * 100 / ((int32_t)acc_1G * acc_1G);
if (72 < tmpu32 && tmpu32 < 133) // Gyro drift correct between 0.85G - 1.15G
{
for (i = 0; i < 3; i++) EstG.A[i] = (EstG.A[i] * (float)cfg.gy_gcmpf + accSmooth[i]) * INV_GY_CMPF;
}
tmp[0] = EstG.A[0] * EstG.A[0] + EstG.A[2] * EstG.A[2]; // Start Angle Calculation. tmp[0] is used for heading below
Norm = sqrtf(tmp[0] + EstG.A[1] * EstG.A[1]);
if(!Norm) return; // Should never happen but break here to prevent div-by-zero-evil
Norm = 1.0f / Norm;
rollRAD = atan2f(EstG.A[0] * Norm, EstG.A[2] * Norm); // Norm seems to be obsolete, but testing shows different result :)
pitchRAD = -asinf(constrain(EstG.A[1] * Norm, -1.0f, 1.0f)); // Ensure range, eliminate rounding stuff that might occure.
cr = cosf(rollRAD);
sr = sinf(rollRAD);
cp = cosf(pitchRAD);
sp = sinf(pitchRAD);
TiltValue = cr * cp; // We do this correctly here
angle[ROLL] = (float)((int32_t)( rollRAD * RADtoDEG10 + 0.5f)); // Use rounded values, eliminate jitter for main PID I and D
angle[PITCH] = (float)((int32_t)(-pitchRAD * RADtoDEG10 + 0.5f));
if (TiltValue >= 0) UpsDwnTimer = 0;
else if(!UpsDwnTimer) UpsDwnTimer = currentTime + 20000; // Use 20ms Timer here to make absolutely sure we are upsidedown
if (UpsDwnTimer && currentTime > UpsDwnTimer) UpsideDown = true;
else UpsideDown = false;
if (TiltValue > Tilt_25deg) f.SMALL_ANGLES_25 = 1;
else f.SMALL_ANGLES_25 = 0;
if (cfg.mag_calibrated) // mag_calibrated can just be true if MAG present
{
if(HaveNewMag) // Only do Complementary filter when new MAG data are available
{
HaveNewMag = false;
for (i = 0; i < 3; i++) EstM.A[i] = (EstM.A[i] * (float)cfg.gy_mcmpf + magADCfloat[i]) * INV_GY_CMPFM;
}
A = EstM.A[1] * tmp[0] - (EstM.A[0] * EstG.A[0] + EstM.A[2] * EstG.A[2]) * EstG.A[1];// Mwii method is more precise (less rounding errors)
B = EstM.A[2] * EstG.A[0] - EstM.A[0] * EstG.A[2];
heading = wrap_180(atan2f(B, A * Norm) * RADtoDEG + magneticDeclination);
if (sensors(SENSOR_GPS) && !UpsideDown)
{
tmp[0] = heading * RADX; // Do GPS INS rotate ACC X/Y to earthframe no centrifugal comp. yet
cos_yaw_x = cosf(tmp[0]); // Store for general use
sin_yaw_y = sinf(tmp[0]);
spcy = sp * cos_yaw_x;
spsy = sp * sin_yaw_y;
accycp = cp * accADC[1];
tmp[0] = accycp * cos_yaw_x + (sr * spcy - cr * sin_yaw_y) * accADC[0] + ( sr * sin_yaw_y + cr * spcy) * accADC[2]; // Rotate raw acc here
tmp[1] = accycp * sin_yaw_y + (cr * cos_yaw_x + sr * spsy) * accADC[0] + (-sr * cos_yaw_x + cr * spsy) * accADC[2];
tmp[2] = ACCDeltaTimeINS / (ACC_GPS_RC + ACCDeltaTimeINS);
for (i = 0; i < 2; i++)
{
cms[i] += tmp[2] * (tmp[i] * CmsFac - cms[i]);
ACC_speed[i] -= cms[i]; //cm/s N+ E+
}
}
}
if(GroundAltInitialized && !UpsideDown) // GroundAltInitialized can just be true if baro present
{
tmp[0] = ((-sp) * accADC[1] + sr * cp * accADC[0] + cp * cr * accADC[2]) - (float)cfg.sens_1G;
cms[2] += (ACCDeltaTimeINS / (ACC_ALT_RC + ACCDeltaTimeINS)) * (tmp[0] * CmsFac - cms[2]);
vario += cms[2];
}
}
void loop()
{
static float yaw_target = 0;
// Wait until new orientation data (normally 5ms max)
while (ins.num_samples_available() == 0);
static uint32_t lastPkt = 0;
static int16_t channels[8] = {0,0,0,0,0,0,0,0};
Iriss::Command doSomething = check_for_command();
do_respond(doSomething, channels);
// turn throttle off if no update for 0.5seconds
if(hal.scheduler->millis() - lastPkt > 500) {
channels[2] = 0;
}
long rcthr, rcyaw, rcpit, rcroll; // Variables to store radio in
// Read RC transmitter
rcthr = channels[2];
rcyaw = channels[3];
rcpit = channels[1];
rcroll = channels[0];
// Ask MPU6050 for orientation
ins.update();
float roll,pitch,yaw;
ins.quaternion.to_euler(&roll, &pitch, &yaw);
roll = ToDeg(roll) ;
pitch = ToDeg(pitch) ;
yaw = ToDeg(yaw) ;
// Ask MPU6050 for gyro data
Vector3f gyro = ins.get_gyro();
float gyroPitch = ToDeg(gyro.y), gyroRoll = ToDeg(gyro.x), gyroYaw = ToDeg(gyro.z);
// Do the magic
if(rcthr > RC_THR_MIN + 100) { // Throttle raised, turn on stablisation.
// Stablise PIDS
float pitch_stab_output = constrain(pids[PID_PITCH_STAB].get_pid((float)rcpit - pitch, 1), -250, 250);
float roll_stab_output = constrain(pids[PID_ROLL_STAB].get_pid((float)rcroll - roll, 1), -250, 250);
float yaw_stab_output = constrain(pids[PID_YAW_STAB].get_pid(wrap_180(yaw_target - yaw), 1), -360, 360);
// is pilot asking for yaw change - if so feed directly to rate pid (overwriting yaw stab output)
if(abs(rcyaw ) > 5) {
yaw_stab_output = rcyaw;
yaw_target = yaw; // remember this yaw for when pilot stops
}
// rate PIDS
long pitch_output = (long) constrain(pids[PID_PITCH_RATE].get_pid(pitch_stab_output - gyroPitch, 1), - 500, 500);
long roll_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(roll_stab_output - gyroRoll, 1), -500, 500);
long yaw_output = (long) constrain(pids[PID_ROLL_RATE].get_pid(yaw_stab_output - gyroYaw, 1), -500, 500);
// mix pid outputs and send to the motors.
hal.rcout->write(MOTOR_FL, rcthr + roll_output + pitch_output - yaw_output);
hal.rcout->write(MOTOR_BL, rcthr + roll_output - pitch_output + yaw_output);
hal.rcout->write(MOTOR_FR, rcthr - roll_output + pitch_output + yaw_output);
hal.rcout->write(MOTOR_BR, rcthr - roll_output - pitch_output - yaw_output);
}
else {
// motors off
hal.rcout->write(MOTOR_FL, 1000);
hal.rcout->write(MOTOR_BL, 1000);
hal.rcout->write(MOTOR_FR, 1000);
hal.rcout->write(MOTOR_BR, 1000);
// reset yaw target so we maintain this on takeoff
yaw_target = yaw;
// reset PID integrals whilst on the ground
for(int i=0; i<6; i++) {
pids[i].reset_I();
}
}
}
