// convert a set of roll rates from earth frame to body frame
void SITL::convert_body_frame(double rollDeg, double pitchDeg,
double rollRate, double pitchRate, double yawRate,
double *p, double *q, double *r)
{
double phi, theta, phiDot, thetaDot, psiDot;
phi = ToRad(rollDeg);
theta = ToRad(pitchDeg);
phiDot = ToRad(rollRate);
thetaDot = ToRad(pitchRate);
psiDot = ToRad(yawRate);
*p = phiDot - psiDot*sin(theta);
*q = cos(phi)*thetaDot + sin(phi)*psiDot*cos(theta);
*r = cos(phi)*psiDot*cos(theta) - sin(phi)*thetaDot;
}
// add_trim - adjust the roll and pitch trim up to a total of 10 degrees
void AP_AHRS::add_trim(float roll_in_radians, float pitch_in_radians, bool save_to_eeprom)
{
Vector3f trim = _trim.get();
// add new trim
trim.x = constrain_float(trim.x + roll_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
trim.y = constrain_float(trim.y + pitch_in_radians, ToRad(-AP_AHRS_TRIM_LIMIT), ToRad(AP_AHRS_TRIM_LIMIT));
// set new trim values
_trim.set(trim);
// save to eeprom
if( save_to_eeprom ) {
_trim.save();
}
}
static void calc_nav_roll()
{
#define NAV_ROLL_BY_RATE 0
#if NAV_ROLL_BY_RATE
// Scale from centidegrees (PID input) to radians per second. A P gain of 1.0 should result in a
// desired rate of 1 degree per second per degree of error - if you're 15 degrees off, you'll try
// to turn at 15 degrees per second.
float turn_rate = ToRad(g.pidNavRoll.get_pid(bearing_error_cd) * .01);
// Use airspeed_cruise as an analogue for airspeed if we don't have airspeed.
float speed;
if (!ahrs.airspeed_estimate(&speed)) {
speed = g.airspeed_cruise_cm*0.01;
// Floor the speed so that the user can't enter a bad value
if(speed < 6) {
speed = 6;
}
}
// Bank angle = V*R/g, where V is airspeed, R is turn rate, and g is gravity.
nav_roll = ToDeg(atan(speed*turn_rate/GRAVITY_MSS)*100);
#else
// this is the old nav_roll calculation. We will use this for 2.50
// then remove for a future release
float nav_gain_scaler = 0.01 * g_gps->ground_speed / g.scaling_speed;
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
nav_roll_cd = g.pidNavRoll.get_pid(bearing_error_cd, nav_gain_scaler); //returns desired bank angle in degrees*100
#endif
nav_roll_cd = constrain_int32(nav_roll_cd, -g.roll_limit_cd.get(), g.roll_limit_cd.get());
}
void DCM_driftCorrection(float* accelVector, float scaledAccelMag, float magneticHeading)
{
//Compensation the Roll, Pitch and Yaw drift.
float magneticHeading_X;
float magneticHeading_Y;
static float Scaled_Omega_P[3];
static float Scaled_Omega_I[3];
float Accel_weight;
float Integrator_magnitude;
float tempfloat;
//*****Roll and Pitch***************
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = constrain(1 - 2*abs(1 - scaledAccelMag),0,1);
Vector_Cross_Product(&errorRollPitch[0],&accelVector[0],&DCM_Matrix[2][0]); //adjust the ground of reference
Vector_Scale(&Omega_P[0],&errorRollPitch[0],Kp_ROLLPITCH*Accel_weight);
Vector_Scale(&Scaled_Omega_I[0],&errorRollPitch[0],Ki_ROLLPITCH*Accel_weight);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);
//*****YAW***************
//Calculate Heading_X and Heading_Y
magneticHeading_X = cos(magneticHeading);
magneticHeading_Y = sin(magneticHeading);
// We make the gyro YAW drift correction based on compass magnetic heading
errorCourse=(DCM_Matrix[0][0]*magneticHeading_Y) - (DCM_Matrix[1][0]*magneticHeading_X); //Calculating YAW error
Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW);
Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding Proportional.
Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
Integrator_magnitude = sqrt(Vector_Dot_Product(Omega_I,Omega_I));
if (Integrator_magnitude > ToRad(300)) {
Vector_Scale(Omega_I,Omega_I,0.5f*ToRad(300)/Integrator_magnitude);
}
}
// update mount position - should be called periodically
void AP_Mount_MAVLink::update()
{
// exit immediately if not initialised
if (!_initialised) {
return;
}
// update based on mount mode
switch(get_mode()) {
// move mount to a "retracted" position. we do not implement a separate servo based retract mechanism
case MAV_MOUNT_MODE_RETRACT:
break;
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
{
const Vector3f &target = _state._neutral_angles.get();
_angle_ef_target_rad.x = ToRad(target.x);
_angle_ef_target_rad.y = ToRad(target.y);
_angle_ef_target_rad.z = ToRad(target.z);
}
break;
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
// do nothing because earth-frame angle targets (i.e. _angle_ef_target_rad) should have already been set by a MOUNT_CONTROL message from GCS
break;
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
// update targets using pilot's rc inputs
update_targets_from_rc();
break;
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
if(_frontend._ahrs.get_gps().status() >= AP_GPS::GPS_OK_FIX_2D) {
calc_angle_to_location(_state._roi_target, _angle_ef_target_rad, true, true);
}
break;
default:
// we do not know this mode so do nothing
break;
}
}
void
GPS::update(void)
{
bool result;
uint32_t tnow;
// call the GPS driver to process incoming data
result = read();
tnow = hal.scheduler->millis();
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((tnow - _idleTimer) > idleTimeout) {
Debug("gps read timeout %lu %lu", (unsigned long)tnow, (unsigned long)_idleTimer);
_status = NO_GPS;
init(_port, _nav_setting);
// reset the idle timer
_idleTimer = tnow;
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
valid_read = true;
new_data = true;
// reset the idle timer
_idleTimer = tnow;
if (_status == GPS_OK) {
last_fix_time = _idleTimer;
_last_ground_speed_cm = ground_speed;
if (_have_raw_velocity) {
// the GPS is able to give us velocity numbers directly
_velocity_north = _vel_north * 0.01;
_velocity_east = _vel_east * 0.01;
_velocity_down = _vel_down * 0.01;
} else {
float gps_heading = ToRad(ground_course * 0.01);
float gps_speed = ground_speed * 0.01;
float sin_heading, cos_heading;
cos_heading = cos(gps_heading);
sin_heading = sin(gps_heading);
_velocity_north = gps_speed * cos_heading;
_velocity_east = gps_speed * sin_heading;
// no good way to get descent rate
_velocity_down = 0;
}
}
}
}
/*
fill in 3D velocity for a GPS that doesn't give vertical velocity numbers
*/
void AP_GPS_Backend::fill_3d_velocity(void)
{
float gps_heading = ToRad(state.ground_course_cd * 0.01f);
state.velocity.x = state.ground_speed * cosf(gps_heading);
state.velocity.y = state.ground_speed * sinf(gps_heading);
state.velocity.z = 0;
state.have_vertical_velocity = false;
}
/**************************************************************************
* \brief Filter DCM Matrix Multiply
* The predict function. Updates 2 variables:
* our model-state x and the 2x2 matrix P /n
*
* x = [ angle, bias ]'
*
* = F x + B u
*
* = [ 1 -dt, 0 1 ] [ angle, bias ] + [ dt, 0 ] [ dotAngle 0 ]
*
* => angle = angle + dt (dotAngle - bias)
* bias = bias
*
*
* P = F P transpose(F) + Q
*
* = [ 1 -dt, 0 1 ] * P * [ 1 0, -dt 1 ] + Q
*
* P(0,0) = P(0,0) - dt * ( P(1,0) + P(0,1) ) + dt² * P(1,1) + Q(0,0)
* P(0,1) = P(0,1) - dt * P(1,1) + Q(0,1)
* P(1,0) = P(1,0) - dt * P(1,1) + Q(1,0)
* P(1,1) = P(1,1) + Q(1,1)
*
* \param ---
*
* \return ---
***************************************************************************/
static void filter_kalman_ars_predict(void)
{
x_angle += dt * (ToRad(filterdata->xGyr) - x_bias);
P_00 += - dt * (P_10 + P_01) + filterdata->Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + filterdata->Q_gyro * dt;
}
/// accel_to_lean_angles - horizontal desired acceleration to lean angles
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::accel_to_lean_angles(float dt, float ekfNavVelGainScaler, bool use_althold_lean_angle)
{
float accel_total; // total acceleration in cm/s/s
float accel_right, accel_forward;
float lean_angle_max = _attitude_control.lean_angle_max();
float accel_max = POSCONTROL_ACCEL_XY_MAX;
// limit acceleration if necessary
if (use_althold_lean_angle) {
accel_max = MIN(accel_max, GRAVITY_MSS * 100.0f * tanf(ToRad(constrain_float(_attitude_control.get_althold_lean_angle_max(),1000,8000)/100.0f)));
}
// scale desired acceleration if it's beyond acceptable limit
accel_total = norm(_accel_target.x, _accel_target.y);
if (accel_total > accel_max && accel_total > 0.0f) {
_accel_target.x = accel_max * _accel_target.x/accel_total;
_accel_target.y = accel_max * _accel_target.y/accel_total;
_limit.accel_xy = true; // unused
} else {
// reset accel limit flag
_limit.accel_xy = false;
}
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {
_accel_target_jerk_limited.x = _accel_target.x;
_accel_target_jerk_limited.y = _accel_target.y;
_accel_target_filter.reset(Vector2f(_accel_target.x, _accel_target.y));
_flags.reset_accel_to_lean_xy = false;
}
// apply jerk limit of 17 m/s^3 - equates to a worst case of about 100 deg/sec/sec
float max_delta_accel = dt * _jerk_cmsss;
Vector2f accel_in(_accel_target.x, _accel_target.y);
Vector2f accel_change = accel_in-_accel_target_jerk_limited;
float accel_change_length = accel_change.length();
if(accel_change_length > max_delta_accel) {
accel_change *= max_delta_accel/accel_change_length;
}
_accel_target_jerk_limited += accel_change;
// lowpass filter on NE accel
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
Vector2f accel_target_filtered = _accel_target_filter.apply(_accel_target_jerk_limited, dt);
// rotate accelerations into body forward-right frame
accel_forward = accel_target_filtered.x*_ahrs.cos_yaw() + accel_target_filtered.y*_ahrs.sin_yaw();
accel_right = -accel_target_filtered.x*_ahrs.sin_yaw() + accel_target_filtered.y*_ahrs.cos_yaw();
// update angle targets that will be passed to stabilize controller
_pitch_target = constrain_float(atanf(-accel_forward/(GRAVITY_MSS * 100))*(18000/M_PI),-lean_angle_max, lean_angle_max);
float cos_pitch_target = cosf(_pitch_target*M_PI/18000);
_roll_target = constrain_float(atanf(accel_right*cos_pitch_target/(GRAVITY_MSS * 100))*(18000/M_PI), -lean_angle_max, lean_angle_max);
}
/*
given a magnetic heading, and roll, pitch, yaw values,
calculate consistent magnetometer components
All angles are in radians
*/
static Vector3f heading_to_mag(float roll, float pitch, float yaw)
{
Vector3f Bearth, m;
Matrix3f R;
// Bearth is the magnetic field in Canberra. We need to adjust
// it for inclination and declination
Bearth(MAG_FIELD_STRENGTH, 0, 0);
R.from_euler(0, -ToRad(MAG_INCLINATION), ToRad(MAG_DECLINATION));
Bearth = R * Bearth;
// create a rotation matrix for the given attitude
R.from_euler(roll, pitch, yaw);
// convert the earth frame magnetic vector to body frame, and
// apply the offsets
m = R.transposed() * Bearth - Vector3f(MAG_OFS_X, MAG_OFS_Y, MAG_OFS_Z);
return m + (rand_vec3f() * sitl.mag_noise);
}
// setup _Bearth
void Compass::_setup_earth_field(void)
{
// assume a earth field strength of 400
_hil.Bearth(400, 0, 0);
// rotate _Bearth for inclination and declination. -66 degrees
// is the inclination in Canberra, Australia
Matrix3f R;
R.from_euler(0, ToRad(66), get_declination());
_hil.Bearth = R * _hil.Bearth;
}
void
AP_Mount::calc_GPS_target_angle(struct Location *target)
{
float GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)*.00000005))*.01113195;
float GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0*sqrt(GPS_vector_x*GPS_vector_x + GPS_vector_y*GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
_tilt_control_angle = atan2(GPS_vector_z, target_distance);
_pan_control_angle = atan2(GPS_vector_x, GPS_vector_y);
}
void test_frame_transforms(void)
{
Vector3f v, v2;
Quaternion q;
Matrix3f m;
hal.console->println("frame transform tests\n");
q.from_euler(ToRad(45), ToRad(45), ToRad(45));
q.normalize();
m.from_euler(ToRad(45), ToRad(45), ToRad(45));
v2 = v = Vector3f(0, 0, 1);
q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);
v2 = v = Vector3f(0, 1, 0);
q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n\n", v2.x, v2.y, v2.z);
v2 = v = Vector3f(1, 0, 0);
q.earth_to_body(v2);
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
v2 = m * v;
hal.console->printf("%f %f %f\n", v2.x, v2.y, v2.z);
}
void test_quaternion_eulers(void)
{
uint8_t i, j, k;
uint8_t N = ARRAY_SIZE(angles);
hal.console->println("quaternion unit tests\n");
test_quaternion(M_PI/4, 0, 0);
test_quaternion(0, M_PI/4, 0);
test_quaternion(0, 0, M_PI/4);
test_quaternion(-M_PI/4, 0, 0);
test_quaternion(0, -M_PI/4, 0);
test_quaternion(0, 0, -M_PI/4);
test_quaternion(-M_PI/4, 1, 1);
test_quaternion(1, -M_PI/4, 1);
test_quaternion(1, 1, -M_PI/4);
test_quaternion(ToRad(89), 0, 0.1f);
test_quaternion(0, ToRad(89), 0.1f);
test_quaternion(0.1f, 0, ToRad(89));
test_quaternion(ToRad(91), 0, 0.1f);
test_quaternion(0, ToRad(91), 0.1f);
test_quaternion(0.1f, 0, ToRad(91));
for (i=0; i<N; i++)
for (j=0; j<N; j++)
for (k=0; k<N; k++)
test_quaternion(angles[i], angles[j], angles[k]);
hal.console->println("tests done\n");
}
// calc_velocities - calculate angular velocity max and acceleration based on radius and rate
// this should be called whenever the radius or rate are changed
// initialises the yaw and current position around the circle
void AC_Circle::calc_velocities(bool init_velocity)
{
// if we are doing a panorama set the circle_angle to the current heading
if (_radius <= 0) {
_angular_vel_max = ToRad(_rate);
_angular_accel = max(fabsf(_angular_vel_max),ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN)); // reach maximum yaw velocity in 1 second
}else{
// calculate max velocity based on waypoint speed ensuring we do not use more than half our max acceleration for accelerating towards the center of the circle
float velocity_max = min(_pos_control.get_speed_xy(), safe_sqrt(0.5f*_pos_control.get_accel_xy()*_radius));
// angular_velocity in radians per second
_angular_vel_max = velocity_max/_radius;
_angular_vel_max = constrain_float(ToRad(_rate),-_angular_vel_max,_angular_vel_max);
// angular_velocity in radians per second
_angular_accel = max(_pos_control.get_accel_xy()/_radius, ToRad(AC_CIRCLE_ANGULAR_ACCEL_MIN));
}
// initialise angular velocity
if (init_velocity) {
_angular_vel = 0;
}
}
/* report SITL state via MAVLink */
void SITL::simstate_send(mavlink_channel_t chan)
{
float yaw;
// convert to same conventions as DCM
yaw = state.yawDeg;
if (yaw > 180) {
yaw -= 360;
}
mavlink_msg_simstate_send(chan,
ToRad(state.rollDeg),
ToRad(state.pitchDeg),
ToRad(yaw),
state.xAccel,
state.yAccel,
state.zAccel,
radians(state.rollRate),
radians(state.pitchRate),
radians(state.yawRate),
state.latitude*1.0e7,
state.longitude*1.0e7);
}
void setup() {
hal.console->println("Compass library test");
if (!compass.init()) {
hal.console->println("compass initialisation failed!");
while (1) ;
}
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
compass.set_and_save_offsets(0,0,0,0); // set offsets to account for surrounding interference
compass.set_declination(ToRad(0.0f)); // set local difference between magnetic north and true north
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
}
// auto_trim - slightly adjusts the ahrs.roll_trim and ahrs.pitch_trim towards the current stick positions
// meant to be called continuously while the pilot attempts to keep the copter level
void Copter::auto_trim()
{
if (auto_trim_counter > 0) {
auto_trim_counter--;
// flash the leds
AP_Notify::flags.save_trim = true;
// calculate roll trim adjustment
float roll_trim_adjustment = ToRad((float)channel_roll->get_control_in() / 4000.0f);
// calculate pitch trim adjustment
float pitch_trim_adjustment = ToRad((float)channel_pitch->get_control_in() / 4000.0f);
// add trim to ahrs object
// save to eeprom on last iteration
ahrs.add_trim(roll_trim_adjustment, pitch_trim_adjustment, (auto_trim_counter == 0));
// on last iteration restore leds and accel gains to normal
if (auto_trim_counter == 0) {
AP_Notify::flags.save_trim = false;
}
}
}
/**************************************************************************
* \brief Filter DCM Matrix Update
*
* \param ---
*
* \return ---
***************************************************************************/
static void filter_dcm_matrix_update(void)
{
Gyro_Vector[0] = -ToRad(filterdata->xGyr); //+ //+
Gyro_Vector[1] = -ToRad(filterdata->yGyr); //- //+
Gyro_Vector[2] = -ToRad(filterdata->zGyr); //- //+
Accel_Vector[0] = filterdata->xAcc; //-
Accel_Vector[1] = filterdata->yAcc; //-
Accel_Vector[2] = -filterdata->zAcc; //-
Vector_Add(&Omega[0], &Gyro_Vector[0], &Omega_I[0]); //adding proportional term
Vector_Add(&Omega_Vector[0], &Omega[0], &Omega_P[0]); //adding Integrator term
/* Remove centrifugal acceleration. */
//filter_dcm_accel_adjust(); //Not yet used!
Update_Matrix[0][0]=0;
Update_Matrix[0][1]=-dt*Omega_Vector[2];//-z
Update_Matrix[0][2]=dt*Omega_Vector[1];//y
Update_Matrix[1][0]=dt*Omega_Vector[2];//z
Update_Matrix[1][1]=0;
Update_Matrix[1][2]=-dt*Omega_Vector[0];//-x
Update_Matrix[2][0]=-dt*Omega_Vector[1];//-y
Update_Matrix[2][1]=dt*Omega_Vector[0];//x
Update_Matrix[2][2]=0;
filter_dcm_matrix_multiply(DCM_Matrix,Update_Matrix,Temporary_Matrix); //a*b=c
/* Matrix Addition (update) */
for(int x=0; x<3; x++)
{
for(int y=0; y<3; y++)
{
DCM_Matrix[x][y]+=Temporary_Matrix[x][y];
}
}
}
static void check_result(const char *msg,
float roll, float pitch, float yaw,
float roll2, float pitch2, float yaw2)
{
if (isnan(roll2) ||
isnan(pitch2) ||
isnan(yaw2)) {
hal.console->printf("%s NAN eulers roll=%f pitch=%f yaw=%f\n",
msg,
(double)roll,
(double)pitch,
(double)yaw);
}
if (rad_diff(roll2,roll) > ToRad(179)) {
// reverse all 3
roll2 += fmodf(roll2 + M_PI, 2 * M_PI);
pitch2 += fmodf(pitch2 + M_PI, 2 * M_PI);
yaw2 += fmodf(yaw2 + M_PI, 2 * M_PI);
}
if (rad_diff(roll2,roll) > 0.01f ||
rad_diff(pitch2, pitch) > 0.01f ||
rad_diff(yaw2, yaw) > 0.01f) {
if (pitch >= M_PI/2 ||
pitch <= -M_PI/2 ||
ToDeg(rad_diff(pitch, M_PI/2)) < 1 ||
ToDeg(rad_diff(pitch, -M_PI/2)) < 1) {
// we expect breakdown at these poles
#if SHOW_POLES_BREAKDOWN
hal.console->printf(
"%s breakdown eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
msg,
(double)ToDeg(roll), (double)ToDeg(roll2),
(double)ToDeg(pitch), (double)ToDeg(pitch2),
(double)ToDeg(yaw), (double)ToDeg(yaw2));
#endif
} else {
hal.console->printf(
"%s incorrect eulers roll=%f/%f pitch=%f/%f yaw=%f/%f\n",
msg,
(double)ToDeg(roll), (double)ToDeg(roll2),
(double)ToDeg(pitch), (double)ToDeg(pitch2),
(double)ToDeg(yaw), (double)ToDeg(yaw2));
}
}
}
// to be called only once on boot for initializing objects
static void setup()
{
hal.console->printf("Compass library test\n");
board_config.init();
vehicle.ahrs.init();
compass.init();
hal.console->printf("init done - %u compasses detected\n", compass.get_count());
// set offsets to account for surrounding interference
compass.set_and_save_offsets(0, Vector3f(0, 0, 0));
// set local difference between magnetic north and true north
compass.set_declination(ToRad(0.0f));
hal.scheduler->delay(1000);
timer = AP_HAL::micros();
}
void setup(void) {
Serial.begin(115200);
menu();
I2c.begin();
I2c.timeOut(20);
if (!compass.init()) {
Serial.println("compass initialisation failed!");
while (1) ;
}
isr_registry.init();
adc_scheduler.init(&isr_registry);
APM_RC.Init(&isr_registry);
APM_RC.enable_out(CH_1);
APM_RC.enable_out(CH_2);
APM_RC.enable_out(CH_3);
APM_RC.enable_out(CH_4);
APM_RC.enable_out(CH_5);
APM_RC.enable_out(CH_6);
APM_RC.enable_out(CH_7);
APM_RC.enable_out(CH_8);
APM_RC.OutputCh(1, valor1);
APM_RC.OutputCh(2, valor1);
APM_RC.OutputCh(3, valor1);
APM_RC.OutputCh(4, valor1);
APM_RC.OutputCh(5, valor1);
APM_RC.OutputCh(6, valor1);
DataFlash.Init();
DataFlash.StartWrite(1);
imu.init(IMU::COLD_START, delay, flash_leds, &adc_scheduler);
imu.init_accel(delay, flash_leds);Serial.print("\n");
funcion_serial();
fast_loopTimer=micros();timer1=micros();
compass.set_orientation(AP_COMPASS_COMPONENTS_DOWN_PINS_FORWARD);
compass.set_offsets(0,0,0);
compass.set_declination(ToRad(0.23));
}
float Orientation_calcCompassHeading(float* magneticVectors)
{
float Head_X;
float Head_Y;
float cos_roll;
float sin_roll;
float cos_pitch;
float sin_pitch;
float Magnetic_Heading = 0.0;
cos_roll = cos(Orientation_Roll);
sin_roll = sin(Orientation_Roll);
cos_pitch = cos(Orientation_Pitch);
sin_pitch = sin(Orientation_Pitch);
// Tilt compensated Magnetic field X component:
Head_X = magneticVectors[0]*cos_pitch +
magneticVectors[1]*sin_roll*sin_pitch +
magneticVectors[2]*cos_roll*sin_pitch;
// Tilt compensated Magnetic field Y component:
Head_Y = magneticVectors[1]*cos_roll -
magneticVectors[2]*sin_roll;
// Magnetic Heading
Magnetic_Heading = atan2(-Head_Y,Head_X);
// Declination correction (if supplied)
if( MAGNETIC_DECLINATION != 0 )
{
Magnetic_Heading = Magnetic_Heading + ToRad(MAGNETIC_DECLINATION);
if (Magnetic_Heading > M_PI) // Angle normalization (-180 deg, 180 deg)
Magnetic_Heading -= (2.0 * M_PI);
else if (Magnetic_Heading < -M_PI)
Magnetic_Heading += (2.0 * M_PI);
}
return Magnetic_Heading;
//return 0;
}
// Default constructor.
// Note that the Vector/Matrix constructors already implicitly zero
// their values.
//
AC_WPNav::AC_WPNav(const AP_InertialNav& inav, const AP_AHRS_View& ahrs, AC_PosControl& pos_control, const AC_AttitudeControl& attitude_control) :
_inav(inav),
_ahrs(ahrs),
_pos_control(pos_control),
_attitude_control(attitude_control)
{
AP_Param::setup_object_defaults(this, var_info);
// init flags
_flags.reached_destination = false;
_flags.fast_waypoint = false;
_flags.slowing_down = false;
_flags.recalc_wp_leash = false;
_flags.new_wp_destination = false;
_flags.segment_type = SEGMENT_STRAIGHT;
// sanity check some parameters
_wp_accel_cmss = MIN(_wp_accel_cmss, GRAVITY_MSS * 100.0f * tanf(ToRad(_attitude_control.lean_angle_max() * 0.01f)));
_wp_radius_cm = MAX(_wp_radius_cm, WPNAV_WP_RADIUS_MIN);
}
void
AP_Mount::calc_GPS_target_angle(const struct Location *target)
{
float GPS_vector_x = (target->lng-_current_loc->lng)*cosf(ToRad((_current_loc->lat+target->lat)*0.00000005f))*0.01113195f;
float GPS_vector_y = (target->lat-_current_loc->lat)*0.01113195f;
float GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
if (mount_axis_mask & MASK_TILT) {
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
} else {
_tilt_control_angle = 0;
}
if (mount_axis_mask & MASK_YAW) {
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
} else {
_pan_control_angle = 0;
}
}
TEST(Transformation, FitMagAnisotropy)
{
cudaDeviceReset();
//Case 1:
{
HeaderMRC refheader = ReadMRCHeader("E:\\carrie\\Particles\\micro\\sum.mrc");
int3 dims = refheader.dimensions;
uint nrefs = 1;
void* h_refraw;
ReadMRC("E:\\carrie\\Particles\\micro\\sum.mrc", &h_refraw);
tfloat* h_ref = MixedToHostTfloat(h_refraw, refheader.mode, Elements(dims));
float bestdistortion = 0;
float bestangle = 0;
d_FitMagAnisotropy(h_ref, toInt2(dims), 70, 0.03f, 0.0002f, ToRad(2.0f), bestdistortion, bestangle);
bestangle = ToDeg(bestangle);
std::cout << bestangle;
}
cudaDeviceReset();
}
// calc_angle_to_location - calculates the earth-frame roll, tilt and pan angles (and radians) to point at the given target
void AP_Mount_Backend::calc_angle_to_location(const struct Location &target, Vector3f& angles_to_target_rad, bool calc_tilt, bool calc_pan, bool relative_pan)
{
float GPS_vector_x = (target.lng-_frontend._current_loc.lng)*cosf(ToRad((_frontend._current_loc.lat+target.lat)*0.00000005f))*0.01113195f;
float GPS_vector_y = (target.lat-_frontend._current_loc.lat)*0.01113195f;
float GPS_vector_z = (target.alt-_frontend._current_loc.alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0f*norm(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
// initialise all angles to zero
angles_to_target_rad.zero();
// tilt calcs
if (calc_tilt) {
angles_to_target_rad.y = atan2f(GPS_vector_z, target_distance);
}
// pan calcs
if (calc_pan) {
// calc absolute heading and then onvert to vehicle relative yaw
angles_to_target_rad.z = atan2f(GPS_vector_x, GPS_vector_y);
if (relative_pan) {
angles_to_target_rad.z = wrap_PI(angles_to_target_rad.z - _frontend._ahrs.yaw);
}
}
}
void
AP_DCM::drift_correction(void)
{
//Compensation the Roll, Pitch and Yaw drift.
//float mag_heading_x;
//float mag_heading_y;
float error_course;
float accel_magnitude;
float accel_weight;
float integrator_magnitude;
//static float scaled_omega_P[3];
//static float scaled_omega_I[3];
static bool in_motion = false;
Matrix3f rot_mat;
//*****Roll and Pitch***************
// Calculate the magnitude of the accelerometer vector
accel_magnitude = _accel_vector.length() / 9.80665f;
// Dynamic weighting of accelerometer info (reliability filter)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
accel_weight = constrain(1 - 2 * fabs(1 - accel_magnitude), 0, 1); //
// We monitor the amount that the accelerometer based drift correction is deweighted for performance reporting
_health += constrain((0.02 * (accel_weight - .5)), 0, 1);
// adjust the ground of reference
_error_roll_pitch = _dcm_matrix.c % _accel_vector; // Equation 27 *** sign changed from prev implementation???
// error_roll_pitch are in Accel m/s^2 units
// Limit max error_roll_pitch to limit max omega_P and omega_I
_error_roll_pitch.x = constrain(_error_roll_pitch.x, -1.17f, 1.17f);
_error_roll_pitch.y = constrain(_error_roll_pitch.y, -1.17f, 1.17f);
_error_roll_pitch.z = constrain(_error_roll_pitch.z, -1.17f, 1.17f);
_omega_P = _error_roll_pitch * (Kp_ROLLPITCH * accel_weight);
_omega_I += _error_roll_pitch * (Ki_ROLLPITCH * accel_weight);
//*****YAW***************
if (_compass) {
// We make the gyro YAW drift correction based on compass magnetic heading
error_course= (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error
} else {
// Use GPS Ground course to correct yaw gyro drift
if (_gps->ground_speed >= SPEEDFILT) {
_course_over_ground_x = cos(ToRad(_gps->ground_course/100.0));
_course_over_ground_y = sin(ToRad(_gps->ground_course/100.0));
if(in_motion) {
error_course = (_dcm_matrix.a.x * _course_over_ground_y) - (_dcm_matrix.b.x * _course_over_ground_x); // Equation 23, Calculating YAW error
} else {
float cos_psi_err, sin_psi_err;
// This is the case for when we first start moving and reset the DCM so that yaw matches the gps ground course
// This is just to get a reasonable estimate faster
yaw = atan2(_dcm_matrix.b.x, _dcm_matrix.a.x);
cos_psi_err = cos(ToRad(_gps->ground_course/100.0) - yaw);
sin_psi_err = sin(ToRad(_gps->ground_course/100.0) - yaw);
// Rxx = cos psi err, Rxy = - sin psi err, Rxz = 0
// Ryx = sin psi err, Ryy = cos psi err, Ryz = 0
// Rzx = Rzy = 0, Rzz = 1
rot_mat.a.x = cos_psi_err;
rot_mat.a.y = - sin_psi_err;
rot_mat.b.x = sin_psi_err;
rot_mat.b.y = cos_psi_err;
rot_mat.a.z = 0;
rot_mat.b.z = 0;
rot_mat.c.x = 0;
rot_mat.c.y = 0;
rot_mat.c.z = 1.0;
_dcm_matrix = rot_mat * _dcm_matrix;
in_motion = true;
error_course = 0;
}
} else {
error_course = 0;
in_motion = false;
}
}
_error_yaw = _dcm_matrix.c * error_course; // Equation 24, Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
_omega_P += _error_yaw * Kp_YAW; // Adding yaw correction to proportional correction vector.
_omega_I += _error_yaw * Ki_YAW; // adding yaw correction to integrator correction vector.
// Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros
integrator_magnitude = _omega_I.length();
if (integrator_magnitude > radians(300)) {
_omega_I *= (0.5f * radians(300) / integrator_magnitude); // Why do we have this discontinuous? EG, why the 0.5?
}
//Serial.print("*");
}
/// run horizontal position controller correcting position and velocity
/// converts position (_pos_target) to target velocity (_vel_target)
/// desired velocity (_vel_desired) is combined into final target velocity
/// converts desired velocities in lat/lon directions to accelerations in lat/lon frame
/// converts desired accelerations provided in lat/lon frame to roll/pitch angles
void AC_PosControl::run_xy_controller(float dt)
{
float ekfGndSpdLimit, ekfNavVelGainScaler;
AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);
Vector3f curr_pos = _inav.get_position();
float kP = ekfNavVelGainScaler * _p_pos_xy.kP(); // scale gains to compensate for noisy optical flow measurement in the EKF
// avoid divide by zero
if (kP <= 0.0f) {
_vel_target.x = 0.0f;
_vel_target.y = 0.0f;
}else{
// calculate distance error
_pos_error.x = _pos_target.x - curr_pos.x;
_pos_error.y = _pos_target.y - curr_pos.y;
// Constrain _pos_error and target position
// Constrain the maximum length of _vel_target to the maximum position correction velocity
// TODO: replace the leash length with a user definable maximum position correction
if (limit_vector_length(_pos_error.x, _pos_error.y, _leash))
{
_pos_target.x = curr_pos.x + _pos_error.x;
_pos_target.y = curr_pos.y + _pos_error.y;
}
_vel_target = sqrt_controller(_pos_error, kP, _accel_cms);
}
// add velocity feed-forward
_vel_target.x += _vel_desired.x;
_vel_target.y += _vel_desired.y;
// the following section converts desired velocities in lat/lon directions to accelerations in lat/lon frame
Vector2f accel_target, vel_xy_p, vel_xy_i, vel_xy_d;
// check if vehicle velocity is being overridden
if (_flags.vehicle_horiz_vel_override) {
_flags.vehicle_horiz_vel_override = false;
} else {
_vehicle_horiz_vel.x = _inav.get_velocity().x;
_vehicle_horiz_vel.y = _inav.get_velocity().y;
}
// calculate velocity error
_vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;
_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;
// TODO: constrain velocity error and velocity target
// call pi controller
_pid_vel_xy.set_input(_vel_error);
// get p
vel_xy_p = _pid_vel_xy.get_p();
// update i term if we have not hit the accel or throttle limits OR the i term will reduce
// TODO: move limit handling into the PI and PID controller
if (!_limit.accel_xy && !_motors.limit.throttle_upper) {
vel_xy_i = _pid_vel_xy.get_i();
} else {
vel_xy_i = _pid_vel_xy.get_i_shrink();
}
// get d
vel_xy_d = _pid_vel_xy.get_d();
// acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise
accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;
accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;
// reset accel to current desired acceleration
if (_flags.reset_accel_to_lean_xy) {
_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));
_flags.reset_accel_to_lean_xy = false;
}
// filter correction acceleration
_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f*ekfNavVelGainScaler));
_accel_target_filter.apply(accel_target, dt);
// pass the correction acceleration to the target acceleration output
_accel_target.x = _accel_target_filter.get().x;
_accel_target.y = _accel_target_filter.get().y;
// Add feed forward into the target acceleration output
_accel_target.x += _accel_desired.x;
_accel_target.y += _accel_desired.y;
// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles
// limit acceleration using maximum lean angles
float angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());
float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);
_limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);
// update angle targets that will be passed to stabilize controller
accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);
}
float AP_InertialSensor_HIL::get_gyro_drift_rate(void) {
// 0.5 degrees/second/minute
return ToRad(0.5/60);
}
