mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-18 16:53:33 +08:00
Merge branch 'mathlib_new' into vector_control2
This commit is contained in:
Anton Babushkin
@@ -53,21 +53,6 @@ static const int8_t ret_error = -1; // error occurred
|
||||
|
||||
KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
SuperBlock(parent, name),
|
||||
// ekf matrices
|
||||
F(9, 9),
|
||||
G(9, 6),
|
||||
P(9, 9),
|
||||
P0(9, 9),
|
||||
V(6, 6),
|
||||
// attitude measurement ekf matrices
|
||||
HAtt(4, 9),
|
||||
RAtt(4, 4),
|
||||
// position measurement ekf matrices
|
||||
HPos(6, 9),
|
||||
RPos(6, 6),
|
||||
// attitude representations
|
||||
C_nb(),
|
||||
q(),
|
||||
// subscriptions
|
||||
_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
|
||||
_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
|
||||
@@ -112,8 +97,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
{
|
||||
using namespace math;
|
||||
|
||||
F.zero();
|
||||
G.zero();
|
||||
V.zero();
|
||||
HAtt.zero();
|
||||
RAtt.zero();
|
||||
HPos.zero();
|
||||
RPos.zero();
|
||||
|
||||
// initial state covariance matrix
|
||||
P0 = Matrix::identity(9) * 0.01f;
|
||||
P0.identity();
|
||||
P0 *= 0.01f;
|
||||
P = P0;
|
||||
|
||||
// initial state
|
||||
@@ -138,7 +132,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||
_sensors.magnetometer_ga[2]);
|
||||
|
||||
// initialize dcm
|
||||
C_nb = Dcm(q);
|
||||
C_nb = q.to_dcm();
|
||||
|
||||
// HPos is constant
|
||||
HPos(0, 3) = 1.0f;
|
||||
@@ -228,8 +222,8 @@ void KalmanNav::update()
|
||||
if (correctAtt() == ret_ok) _attitudeInitCounter++;
|
||||
|
||||
if (_attitudeInitCounter > 100) {
|
||||
warnx("initialized EKF attitude\n");
|
||||
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
|
||||
warnx("initialized EKF attitude");
|
||||
warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f",
|
||||
double(phi), double(theta), double(psi));
|
||||
_attitudeInitialized = true;
|
||||
}
|
||||
@@ -259,8 +253,8 @@ void KalmanNav::update()
|
||||
// lat/lon and not have init
|
||||
map_projection_init(lat0, lon0);
|
||||
_positionInitialized = true;
|
||||
warnx("initialized EKF state with GPS\n");
|
||||
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
|
||||
warnx("initialized EKF state with GPS");
|
||||
warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
|
||||
double(vN), double(vE), double(vD),
|
||||
lat, lon, double(alt));
|
||||
}
|
||||
@@ -404,28 +398,28 @@ int KalmanNav::predictState(float dt)
|
||||
|
||||
// attitude prediction
|
||||
if (_attitudeInitialized) {
|
||||
Vector3 w(_sensors.gyro_rad_s);
|
||||
Vector<3> w(_sensors.gyro_rad_s);
|
||||
|
||||
// attitude
|
||||
q = q + q.derivative(w) * dt;
|
||||
|
||||
// renormalize quaternion if needed
|
||||
if (fabsf(q.norm() - 1.0f) > 1e-4f) {
|
||||
q = q.unit();
|
||||
if (fabsf(q.length() - 1.0f) > 1e-4f) {
|
||||
q.normalize();
|
||||
}
|
||||
|
||||
// C_nb update
|
||||
C_nb = Dcm(q);
|
||||
C_nb = q.to_dcm();
|
||||
|
||||
// euler update
|
||||
EulerAngles euler(C_nb);
|
||||
phi = euler.getPhi();
|
||||
theta = euler.getTheta();
|
||||
psi = euler.getPsi();
|
||||
Vector<3> euler = C_nb.to_euler();
|
||||
phi = euler.data[0];
|
||||
theta = euler.data[1];
|
||||
psi = euler.data[2];
|
||||
|
||||
// specific acceleration in nav frame
|
||||
Vector3 accelB(_sensors.accelerometer_m_s2);
|
||||
Vector3 accelN = C_nb * accelB;
|
||||
Vector<3> accelB(_sensors.accelerometer_m_s2);
|
||||
Vector<3> accelN = C_nb * accelB;
|
||||
fN = accelN(0);
|
||||
fE = accelN(1);
|
||||
fD = accelN(2);
|
||||
@@ -549,10 +543,10 @@ int KalmanNav::predictStateCovariance(float dt)
|
||||
G(5, 4) = C_nb(2, 1);
|
||||
G(5, 5) = C_nb(2, 2);
|
||||
|
||||
// continuous predictioon equations
|
||||
// for discrte time EKF
|
||||
// continuous prediction equations
|
||||
// for discrete time EKF
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
P = P + (F * P + P * F.transpose() + G * V * G.transpose()) * dt;
|
||||
P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt;
|
||||
|
||||
return ret_ok;
|
||||
}
|
||||
@@ -577,13 +571,14 @@ int KalmanNav::correctAtt()
|
||||
|
||||
// compensate roll and pitch, but not yaw
|
||||
// XXX take the vectors out of the C_nb matrix to avoid singularities
|
||||
math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
|
||||
math::Matrix<3,3> C_rp;
|
||||
C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed();
|
||||
|
||||
// mag measurement
|
||||
Vector3 magBody(_sensors.magnetometer_ga);
|
||||
Vector<3> magBody(_sensors.magnetometer_ga);
|
||||
|
||||
// transform to earth frame
|
||||
Vector3 magNav = C_rp * magBody;
|
||||
Vector<3> magNav = C_rp * magBody;
|
||||
|
||||
// calculate error between estimate and measurement
|
||||
// apply declination correction for true heading as well.
|
||||
@@ -592,12 +587,12 @@ int KalmanNav::correctAtt()
|
||||
if (yMag < -M_PI_F) yMag += 2*M_PI_F;
|
||||
|
||||
// accel measurement
|
||||
Vector3 zAccel(_sensors.accelerometer_m_s2);
|
||||
float accelMag = zAccel.norm();
|
||||
zAccel = zAccel.unit();
|
||||
Vector<3> zAccel(_sensors.accelerometer_m_s2);
|
||||
float accelMag = zAccel.length();
|
||||
zAccel.normalize();
|
||||
|
||||
// ignore accel correction when accel mag not close to g
|
||||
Matrix RAttAdjust = RAtt;
|
||||
Matrix<4,4> RAttAdjust = RAtt;
|
||||
|
||||
bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
|
||||
|
||||
@@ -611,14 +606,10 @@ int KalmanNav::correctAtt()
|
||||
}
|
||||
|
||||
// accel predicted measurement
|
||||
Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
|
||||
Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized();
|
||||
|
||||
// calculate residual
|
||||
Vector y(4);
|
||||
y(0) = yMag;
|
||||
y(1) = zAccel(0) - zAccelHat(0);
|
||||
y(2) = zAccel(1) - zAccelHat(1);
|
||||
y(3) = zAccel(2) - zAccelHat(2);
|
||||
Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2));
|
||||
|
||||
// HMag
|
||||
HAtt(0, 2) = 1;
|
||||
@@ -632,17 +623,17 @@ int KalmanNav::correctAtt()
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
|
||||
Matrix K = P * HAtt.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance
|
||||
Matrix<9, 4> K = P * HAtt.transposed() * S.inversed();
|
||||
Vector<9> xCorrect = K * y;
|
||||
|
||||
// check correciton is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
for (size_t i = 0; i < xCorrect.get_size(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (isnan(val) || isinf(val)) {
|
||||
// abort correction and return
|
||||
warnx("numerical failure in att correction\n");
|
||||
warnx("numerical failure in att correction");
|
||||
// reset P matrix to P0
|
||||
P = P0;
|
||||
return ret_error;
|
||||
@@ -669,7 +660,7 @@ int KalmanNav::correctAtt()
|
||||
P = P - K * HAtt * P;
|
||||
|
||||
// fault detection
|
||||
float beta = y.dot(S.inverse() * y);
|
||||
float beta = y * (S.inversed() * y);
|
||||
|
||||
if (beta > _faultAtt.get()) {
|
||||
warnx("fault in attitude: beta = %8.4f", (double)beta);
|
||||
@@ -678,7 +669,7 @@ int KalmanNav::correctAtt()
|
||||
|
||||
// update quaternions from euler
|
||||
// angle correction
|
||||
q = Quaternion(EulerAngles(phi, theta, psi));
|
||||
q.from_euler(phi, theta, psi);
|
||||
|
||||
return ret_ok;
|
||||
}
|
||||
@@ -688,7 +679,7 @@ int KalmanNav::correctPos()
|
||||
using namespace math;
|
||||
|
||||
// residual
|
||||
Vector y(6);
|
||||
Vector<6> y;
|
||||
y(0) = _gps.vel_n_m_s - vN;
|
||||
y(1) = _gps.vel_e_m_s - vE;
|
||||
y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
|
||||
@@ -698,17 +689,17 @@ int KalmanNav::correctPos()
|
||||
|
||||
// compute correction
|
||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||
Matrix S = HPos * P * HPos.transpose() + RPos; // residual covariance
|
||||
Matrix K = P * HPos.transpose() * S.inverse();
|
||||
Vector xCorrect = K * y;
|
||||
Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance
|
||||
Matrix<9,6> K = P * HPos.transposed() * S.inversed();
|
||||
Vector<9> xCorrect = K * y;
|
||||
|
||||
// check correction is sane
|
||||
for (size_t i = 0; i < xCorrect.getRows(); i++) {
|
||||
for (size_t i = 0; i < xCorrect.get_size(); i++) {
|
||||
float val = xCorrect(i);
|
||||
|
||||
if (!isfinite(val)) {
|
||||
// abort correction and return
|
||||
warnx("numerical failure in gps correction\n");
|
||||
warnx("numerical failure in gps correction");
|
||||
// fallback to GPS
|
||||
vN = _gps.vel_n_m_s;
|
||||
vE = _gps.vel_e_m_s;
|
||||
@@ -735,7 +726,7 @@ int KalmanNav::correctPos()
|
||||
P = P - K * HPos * P;
|
||||
|
||||
// fault detetcion
|
||||
float beta = y.dot(S.inverse() * y);
|
||||
float beta = y * (S.inversed() * y);
|
||||
|
||||
static int counter = 0;
|
||||
if (beta > _faultPos.get() && (counter % 10 == 0)) {
|
||||
|
||||
@@ -125,17 +125,17 @@ public:
|
||||
virtual void updateParams();
|
||||
protected:
|
||||
// kalman filter
|
||||
math::Matrix F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix P; /**< state covariance matrix */
|
||||
math::Matrix P0; /**< initial state covariance matrix */
|
||||
math::Matrix V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix RPos; /**< position measurement noise matrix */
|
||||
math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */
|
||||
math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */
|
||||
math::Matrix<9,9> P; /**< state covariance matrix */
|
||||
math::Matrix<9,9> P0; /**< initial state covariance matrix */
|
||||
math::Matrix<6,6> V; /**< gyro/ accel noise matrix */
|
||||
math::Matrix<4,9> HAtt; /**< attitude measurement matrix */
|
||||
math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */
|
||||
math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */
|
||||
math::Matrix<6,6> RPos; /**< position measurement noise matrix */
|
||||
// attitude
|
||||
math::Dcm C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */
|
||||
math::Quaternion q; /**< quaternion from body to nav frame */
|
||||
// subscriptions
|
||||
control::UOrbSubscription<sensor_combined_s> _sensors; /**< sensors sub. */
|
||||
|
||||
@@ -107,7 +107,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
|
||||
daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
|
||||
SCHED_DEFAULT,
|
||||
SCHED_PRIORITY_MAX - 30,
|
||||
4096,
|
||||
8192,
|
||||
kalman_demo_thread_main,
|
||||
(argv) ? (const char **)&argv[2] : (const char **)NULL);
|
||||
exit(0);
|
||||
|
||||
@@ -267,6 +267,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
|
||||
unsigned offset_count = 0;
|
||||
|
||||
/* rotation matrix for magnetic declination */
|
||||
math::Matrix<3, 3> R_decl;
|
||||
R_decl.identity();
|
||||
|
||||
/* register the perf counter */
|
||||
perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf");
|
||||
|
||||
@@ -301,6 +305,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
|
||||
/* update parameters */
|
||||
parameters_update(&ekf_param_handles, &ekf_params);
|
||||
|
||||
/* update mag declination rotation matrix */
|
||||
R_decl.from_euler(0.0f, 0.0f, ekf_params.mag_decl);
|
||||
}
|
||||
|
||||
/* only run filter if sensor values changed */
|
||||
@@ -450,17 +457,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds
|
||||
memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
|
||||
|
||||
/* magnetic declination */
|
||||
math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl);
|
||||
math::Dcm R(eulerMagDecl);
|
||||
math::Dcm R_body(&Rot_matrix[0]);
|
||||
R = R * R_body;
|
||||
|
||||
math::Matrix<3, 3> R_body = (&Rot_matrix[0]);
|
||||
math::Matrix<3, 3> R = R_decl * R_body;
|
||||
|
||||
/* copy rotation matrix */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
att.R[i][j] = R(i, j);
|
||||
}
|
||||
}
|
||||
memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R));
|
||||
att.R_valid = true;
|
||||
|
||||
if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
|
||||
|
||||
@@ -194,15 +194,13 @@ int do_accel_calibration(int mavlink_fd)
|
||||
int32_t board_rotation_int;
|
||||
param_get(board_rotation_h, &(board_rotation_int));
|
||||
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
|
||||
math::Matrix board_rotation(3, 3);
|
||||
math::Matrix<3,3> board_rotation;
|
||||
get_rot_matrix(board_rotation_id, &board_rotation);
|
||||
math::Matrix board_rotation_t = board_rotation.transpose();
|
||||
math::Vector3 accel_offs_vec;
|
||||
accel_offs_vec.set(&accel_offs[0]);
|
||||
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix accel_T_mat(3, 3);
|
||||
accel_T_mat.set(&accel_T[0][0]);
|
||||
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
|
||||
math::Vector<3> accel_offs_vec(&accel_offs[0]);
|
||||
math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
|
||||
math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
|
||||
@@ -170,7 +170,7 @@ private:
|
||||
uint64_t _airspeed_last_valid; ///< last time airspeed was valid. Used to detect sensor failures
|
||||
float _groundspeed_undershoot; ///< ground speed error to min. speed in m/s
|
||||
bool _global_pos_valid; ///< global position is valid
|
||||
math::Dcm _R_nb; ///< current attitude
|
||||
math::Matrix<3, 3> _R_nb; ///< current attitude
|
||||
|
||||
ECL_L1_Pos_Controller _l1_control;
|
||||
TECS _tecs;
|
||||
@@ -282,7 +282,7 @@ private:
|
||||
/**
|
||||
* Control position.
|
||||
*/
|
||||
bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed,
|
||||
bool control_position(const math::Vector<2> &global_pos, const math::Vector<2> &ground_speed,
|
||||
const struct vehicle_global_position_set_triplet_s &global_triplet);
|
||||
|
||||
float calculate_target_airspeed(float airspeed_demand);
|
||||
@@ -600,10 +600,10 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
|
||||
|
||||
if (_global_pos_valid) {
|
||||
/* get ground speed vector */
|
||||
math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy);
|
||||
math::Vector<2> ground_speed_vector(_global_pos.vx, _global_pos.vy);
|
||||
|
||||
/* rotate with current attitude */
|
||||
math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
|
||||
math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
|
||||
yaw_vector.normalize();
|
||||
float ground_speed_body = yaw_vector * ground_speed_vector;
|
||||
|
||||
@@ -624,7 +624,7 @@ FixedwingPositionControl::calculate_gndspeed_undershoot()
|
||||
}
|
||||
|
||||
bool
|
||||
FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed,
|
||||
FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed,
|
||||
const struct vehicle_global_position_set_triplet_s &global_triplet)
|
||||
{
|
||||
bool setpoint = true;
|
||||
@@ -637,8 +637,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
||||
float baro_altitude = _global_pos.alt;
|
||||
|
||||
/* filter speed and altitude for controller */
|
||||
math::Vector3 accel_body(_accel.x, _accel.y, _accel.z);
|
||||
math::Vector3 accel_earth = _R_nb.transpose() * accel_body;
|
||||
math::Vector<3> accel_body(_accel.x, _accel.y, _accel.z);
|
||||
math::Vector<3> accel_earth = _R_nb.transposed() * accel_body;
|
||||
|
||||
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
|
||||
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
|
||||
@@ -662,29 +662,29 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
||||
if (_setpoint_valid) {
|
||||
|
||||
/* current waypoint (the one currently heading for) */
|
||||
math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
|
||||
math::Vector<2> next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
|
||||
|
||||
/* previous waypoint */
|
||||
math::Vector2f prev_wp;
|
||||
math::Vector<2> prev_wp;
|
||||
|
||||
if (global_triplet.previous_valid) {
|
||||
prev_wp.setX(global_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(global_triplet.previous.lon / 1e7f);
|
||||
prev_wp(0) = global_triplet.previous.lat / 1e7f;
|
||||
prev_wp(1) = global_triplet.previous.lon / 1e7f;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid previous waypoint, go for the current wp.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp.setX(global_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(global_triplet.current.lon / 1e7f);
|
||||
prev_wp(0) = global_triplet.current.lat / 1e7f;
|
||||
prev_wp(1) = global_triplet.current.lon / 1e7f;
|
||||
|
||||
}
|
||||
|
||||
// XXX add RTL switch
|
||||
if (global_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH && _launch_valid) {
|
||||
|
||||
math::Vector2f rtl_pos(_launch_lat, _launch_lon);
|
||||
math::Vector<2> rtl_pos(_launch_lat, _launch_lon);
|
||||
|
||||
_l1_control.navigate_waypoints(rtl_pos, rtl_pos, current_position, ground_speed);
|
||||
_att_sp.roll_body = _l1_control.nav_roll();
|
||||
@@ -730,7 +730,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
||||
|
||||
/* switch to heading hold for the last meters, continue heading hold after */
|
||||
|
||||
float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
|
||||
float wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), current_position(0), current_position(1));
|
||||
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
|
||||
if (wp_distance < 15.0f || land_noreturn) {
|
||||
|
||||
@@ -869,7 +869,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio
|
||||
|
||||
altitude_error = _loiter_hold_alt - _global_pos.alt;
|
||||
|
||||
math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
|
||||
math::Vector<2> loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
|
||||
|
||||
/* loiter around current position */
|
||||
_l1_control.navigate_loiter(loiter_hold_pos, current_position, _parameters.loiter_hold_radius,
|
||||
@@ -1101,8 +1101,8 @@ FixedwingPositionControl::task_main()
|
||||
vehicle_airspeed_poll();
|
||||
// vehicle_baro_poll();
|
||||
|
||||
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
|
||||
/*
|
||||
* Attempt to control position, on success (= sensors present and not in manual mode),
|
||||
|
||||
@@ -683,8 +683,8 @@ handle_message(mavlink_message_t *msg)
|
||||
|
||||
/* Calculate Rotation Matrix */
|
||||
math::Quaternion q(hil_state.attitude_quaternion);
|
||||
math::Dcm C_nb(q);
|
||||
math::EulerAngles euler(C_nb);
|
||||
math::Matrix<3,3> C_nb = q.to_dcm();
|
||||
math::Vector<3> euler = C_nb.to_euler();
|
||||
|
||||
/* set rotation matrix */
|
||||
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
|
||||
@@ -699,9 +699,9 @@ handle_message(mavlink_message_t *msg)
|
||||
hil_attitude.q[3] = q(3);
|
||||
hil_attitude.q_valid = true;
|
||||
|
||||
hil_attitude.roll = euler.getPhi();
|
||||
hil_attitude.pitch = euler.getTheta();
|
||||
hil_attitude.yaw = euler.getPsi();
|
||||
hil_attitude.roll = euler(0);
|
||||
hil_attitude.pitch = euler(1);
|
||||
hil_attitude.yaw = euler(2);
|
||||
hil_attitude.rollspeed = hil_state.rollspeed;
|
||||
hil_attitude.pitchspeed = hil_state.pitchspeed;
|
||||
hil_attitude.yawspeed = hil_state.yawspeed;
|
||||
|
||||
@@ -133,13 +133,11 @@ private:
|
||||
|
||||
perf_counter_t _loop_perf; /**< loop performance counter */
|
||||
|
||||
bool _att_sp_valid; /**< flag if the attitude setpoint is valid */
|
||||
math::Matrix<3, 3> _K; /**< diagonal gain matrix for position error */
|
||||
math::Matrix<3, 3> _K_rate_p; /**< diagonal gain matrix for angular rate error */
|
||||
math::Matrix<3, 3> _K_rate_d; /**< diagonal gain matrix for angular rate derivative */
|
||||
|
||||
math::Matrix _K; /**< diagonal gain matrix for position error */
|
||||
math::Matrix _K_rate_p; /**< diagonal gain matrix for angular rate error */
|
||||
math::Matrix _K_rate_d; /**< diagonal gain matrix for angular rate derivative */
|
||||
|
||||
math::Vector3 _rates_prev; /**< angular rates on previous step */
|
||||
math::Vector<3> _rates_prev; /**< angular rates on previous step */
|
||||
|
||||
struct {
|
||||
param_t att_p;
|
||||
@@ -222,17 +220,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
_actuators_0_pub(-1),
|
||||
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
|
||||
|
||||
/* states */
|
||||
_att_sp_valid(false),
|
||||
|
||||
/* gain matrices */
|
||||
_K(3, 3),
|
||||
_K_rate_p(3, 3),
|
||||
_K_rate_d(3, 3),
|
||||
|
||||
_rates_prev(0.0f, 0.0f, 0.0f)
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "fw att control"))
|
||||
|
||||
{
|
||||
memset(&_att, 0, sizeof(_att));
|
||||
@@ -241,6 +229,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
|
||||
memset(&_control_mode, 0, sizeof(_control_mode));
|
||||
memset(&_arming, 0, sizeof(_arming));
|
||||
|
||||
_K.zero();
|
||||
_K_rate_p.zero();
|
||||
_K_rate_d.zero();
|
||||
|
||||
_rates_prev.zero();
|
||||
|
||||
_parameter_handles.att_p = param_find("MC_ATT_P");
|
||||
_parameter_handles.att_rate_p = param_find("MC_ATTRATE_P");
|
||||
_parameter_handles.att_rate_d = param_find("MC_ATTRATE_D");
|
||||
@@ -293,17 +287,14 @@ MulticopterAttitudeControl::parameters_update()
|
||||
param_get(_parameter_handles.yaw_rate_p, &yaw_rate_p);
|
||||
param_get(_parameter_handles.yaw_rate_d, &yaw_rate_d);
|
||||
|
||||
_K.setAll(0.0f);
|
||||
_K(0, 0) = att_p;
|
||||
_K(1, 1) = att_p;
|
||||
_K(2, 2) = yaw_p;
|
||||
|
||||
_K_rate_p.setAll(0.0f);
|
||||
_K_rate_p(0, 0) = att_rate_p;
|
||||
_K_rate_p(1, 1) = att_rate_p;
|
||||
_K_rate_p(2, 2) = yaw_rate_p;
|
||||
|
||||
_K_rate_d.setAll(0.0f);
|
||||
_K_rate_d(0, 0) = att_rate_d;
|
||||
_K_rate_d(1, 1) = att_rate_d;
|
||||
_K_rate_d(2, 2) = yaw_rate_d;
|
||||
@@ -348,7 +339,6 @@ MulticopterAttitudeControl::vehicle_setpoint_poll()
|
||||
|
||||
if (att_sp_updated) {
|
||||
orb_copy(ORB_ID(vehicle_attitude_setpoint), _att_sp_sub, &_att_sp);
|
||||
_att_sp_valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -401,6 +391,24 @@ MulticopterAttitudeControl::task_main()
|
||||
vehicle_manual_poll();
|
||||
arming_status_poll();
|
||||
|
||||
/* setpoint rotation matrix */
|
||||
math::Matrix<3, 3> R_sp;
|
||||
R_sp.identity();
|
||||
|
||||
/* rotation matrix for current state */
|
||||
math::Matrix<3, 3> R;
|
||||
R.identity();
|
||||
|
||||
/* current angular rates */
|
||||
math::Vector<3> rates;
|
||||
rates.zero();
|
||||
|
||||
/* identity matrix */
|
||||
math::Matrix<3, 3> I;
|
||||
I.identity();
|
||||
|
||||
math::Quaternion q;
|
||||
|
||||
bool reset_yaw_sp = true;
|
||||
|
||||
/* wakeup source(s) */
|
||||
@@ -486,6 +494,7 @@ MulticopterAttitudeControl::task_main()
|
||||
/* move yaw setpoint */
|
||||
yaw_sp_move_rate = _manual.yaw;
|
||||
_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
|
||||
_att_sp.R_valid = false;
|
||||
publish_att_sp = true;
|
||||
}
|
||||
}
|
||||
@@ -494,6 +503,7 @@ MulticopterAttitudeControl::task_main()
|
||||
if (reset_yaw_sp) {
|
||||
reset_yaw_sp = false;
|
||||
_att_sp.yaw_body = _att.yaw;
|
||||
_att_sp.R_valid = false;
|
||||
publish_att_sp = true;
|
||||
}
|
||||
|
||||
@@ -501,11 +511,10 @@ MulticopterAttitudeControl::task_main()
|
||||
/* update attitude setpoint if not in position control mode */
|
||||
_att_sp.roll_body = _manual.roll;
|
||||
_att_sp.pitch_body = _manual.pitch;
|
||||
_att_sp.R_valid = false;
|
||||
publish_att_sp = true;
|
||||
}
|
||||
|
||||
_att_sp_valid = true;
|
||||
|
||||
} else {
|
||||
/* manual rate inputs (ACRO) */
|
||||
// TODO
|
||||
@@ -528,17 +537,15 @@ MulticopterAttitudeControl::task_main()
|
||||
reset_yaw_sp = true;
|
||||
}
|
||||
|
||||
if (publish_att_sp || (_att_sp_valid && !_att_sp.R_valid)) {
|
||||
/* controller uses rotation matrix, not euler angles, convert if necessary */
|
||||
math::EulerAngles euler_sp(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
|
||||
math::Dcm R_sp(euler_sp);
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
_att_sp.R_body[i][j] = R_sp(i, j);
|
||||
}
|
||||
}
|
||||
if (_att_sp.R_valid) {
|
||||
/* rotation matrix in _att_sp is valid, use it */
|
||||
R_sp.set(&_att_sp.R_body[0][0]);
|
||||
} else {
|
||||
/* rotation matrix in _att_sp is not valid, use euler angles instead */
|
||||
R_sp.from_euler(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body);
|
||||
|
||||
/* copy rotation matrix back to setpoint struct */
|
||||
memcpy(&_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_att_sp.R_body));
|
||||
_att_sp.R_valid = true;
|
||||
}
|
||||
|
||||
@@ -554,41 +561,41 @@ MulticopterAttitudeControl::task_main()
|
||||
}
|
||||
}
|
||||
|
||||
/* desired rotation matrix */
|
||||
math::Dcm R_des(_att_sp.R_body);
|
||||
|
||||
/* rotation matrix for current state */
|
||||
math::Dcm R(_att.R);
|
||||
R.set(_att.R);
|
||||
|
||||
/* current body angular rates */
|
||||
math::Vector3 rates(_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||
rates(0) = _att.rollspeed;
|
||||
rates(1) = _att.pitchspeed;
|
||||
rates(2) = _att.yawspeed;
|
||||
|
||||
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
|
||||
math::Vector3 R_z(R(0, 2), R(1, 2), R(2, 2));
|
||||
math::Vector3 R_des_z(R_des(0, 2), R_des(1, 2), R_des(2, 2));
|
||||
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
|
||||
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
|
||||
|
||||
/* axis and sin(angle) of desired rotation */
|
||||
math::Vector3 e_R = R.transpose() * R_z.cross(R_des_z);
|
||||
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
|
||||
|
||||
/* calculate angle error */
|
||||
float e_R_z_sin = e_R.norm();
|
||||
float e_R_z_cos = R_z * R_des_z;
|
||||
float e_R_z_sin = e_R.length();
|
||||
float e_R_z_cos = R_z * R_sp_z;
|
||||
|
||||
/* calculate weight for yaw control */
|
||||
float yaw_w = R_des(2, 2) * R_des(2, 2);
|
||||
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
|
||||
|
||||
/* calculate rotation matrix after roll/pitch only rotation */
|
||||
math::Matrix R_rp(3, 3);
|
||||
math::Matrix<3, 3> R_rp;
|
||||
|
||||
if (e_R_z_sin > 0.0f) {
|
||||
/* get axis-angle representation */
|
||||
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
|
||||
math::Vector3 e_R_z_axis = e_R / e_R_z_sin;
|
||||
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
|
||||
|
||||
e_R = e_R_z_axis * e_R_z_angle;
|
||||
|
||||
/* cross product matrix for e_R_axis */
|
||||
math::Matrix e_R_cp(3, 3);
|
||||
e_R_cp.setAll(0.0f);
|
||||
math::Matrix<3, 3> e_R_cp;
|
||||
e_R_cp.zero();
|
||||
e_R_cp(0, 1) = -e_R_z_axis(2);
|
||||
e_R_cp(0, 2) = e_R_z_axis(1);
|
||||
e_R_cp(1, 0) = e_R_z_axis(2);
|
||||
@@ -597,11 +604,6 @@ MulticopterAttitudeControl::task_main()
|
||||
e_R_cp(2, 1) = e_R_z_axis(0);
|
||||
|
||||
/* rotation matrix for roll/pitch only rotation */
|
||||
math::Matrix I(3, 3);
|
||||
I.setAll(0.0f);
|
||||
I(0, 0) = 1.0f;
|
||||
I(1, 1) = 1.0f;
|
||||
I(2, 2) = 1.0f;
|
||||
R_rp = R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
|
||||
|
||||
} else {
|
||||
@@ -609,17 +611,17 @@ MulticopterAttitudeControl::task_main()
|
||||
R_rp = R;
|
||||
}
|
||||
|
||||
/* R_rp and R_des has the same Z axis, calculate yaw error */
|
||||
math::Vector3 R_des_x(R_des(0, 0), R_des(1, 0), R_des(2, 0));
|
||||
math::Vector3 R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
|
||||
e_R(2) = atan2f(R_rp_x.cross(R_des_x) * R_des_z, R_rp_x * R_des_x) * yaw_w;
|
||||
/* R_rp and R_sp has the same Z axis, calculate yaw error */
|
||||
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
|
||||
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
|
||||
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
|
||||
|
||||
if (e_R_z_cos < 0.0) {
|
||||
/* for large thrust vector rotations use another rotation method:
|
||||
* calculate angle and axis for R -> R_des rotation directly */
|
||||
math::Quaternion q(R.transpose() * R_des);
|
||||
float angle_d = 2.0f * atan2f(sqrtf(q.getB() * q.getB() + q.getC() * q.getC() + q.getD() * q.getD()), q.getA());
|
||||
math::Vector3 e_R_d(q.getB(), q.getC(), q.getD());
|
||||
* calculate angle and axis for R -> R_sp rotation directly */
|
||||
q.from_dcm(R.transposed() * R_sp);
|
||||
math::Vector<3> e_R_d = q.imag();
|
||||
float angle_d = 2.0f * atan2f(e_R_d.length(), q(0));
|
||||
|
||||
/* use fusion of Z axis based rotation and direct rotation */
|
||||
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
|
||||
@@ -627,11 +629,11 @@ MulticopterAttitudeControl::task_main()
|
||||
}
|
||||
|
||||
/* angular rates setpoint*/
|
||||
math::Vector3 rates_sp = _K * e_R;
|
||||
math::Vector<3> rates_sp = _K * e_R;
|
||||
|
||||
/* feed forward yaw setpoint rate */
|
||||
rates_sp(2) += yaw_sp_move_rate * yaw_w;
|
||||
math::Vector3 control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
|
||||
math::Vector<3> control = _K_rate_p * (rates_sp - rates) + _K_rate_d * (_rates_prev - rates) / fmaxf(dt, 0.003f);
|
||||
_rates_prev = rates;
|
||||
|
||||
/* publish the attitude rates setpoint */
|
||||
|
||||
@@ -422,11 +422,11 @@ Navigator::task_main()
|
||||
|
||||
mission_poll();
|
||||
|
||||
math::Vector2f ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
math::Vector<2> ground_speed(_global_pos.vx, _global_pos.vy);
|
||||
// Current waypoint
|
||||
math::Vector2f next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
|
||||
math::Vector<2> next_wp(_global_triplet.current.lat / 1e7f, _global_triplet.current.lon / 1e7f);
|
||||
// Global position
|
||||
math::Vector2f current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
math::Vector<2> current_position(_global_pos.lat / 1e7f, _global_pos.lon / 1e7f);
|
||||
|
||||
/* AUTONOMOUS FLIGHT */
|
||||
|
||||
@@ -436,19 +436,19 @@ Navigator::task_main()
|
||||
if (_mission_valid) {
|
||||
|
||||
// Next waypoint
|
||||
math::Vector2f prev_wp;
|
||||
math::Vector<2> prev_wp;
|
||||
|
||||
if (_global_triplet.previous_valid) {
|
||||
prev_wp.setX(_global_triplet.previous.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.previous.lon / 1e7f);
|
||||
prev_wp(0) = _global_triplet.previous.lat / 1e7f;
|
||||
prev_wp(1) = _global_triplet.previous.lon / 1e7f;
|
||||
|
||||
} else {
|
||||
/*
|
||||
* No valid next waypoint, go for heading hold.
|
||||
* This is automatically handled by the L1 library.
|
||||
*/
|
||||
prev_wp.setX(_global_triplet.current.lat / 1e7f);
|
||||
prev_wp.setY(_global_triplet.current.lon / 1e7f);
|
||||
prev_wp(0) = _global_triplet.current.lat / 1e7f;
|
||||
prev_wp(1) = _global_triplet.current.lon / 1e7f;
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -211,8 +211,8 @@ private:
|
||||
struct differential_pressure_s _diff_pres;
|
||||
struct airspeed_s _airspeed;
|
||||
|
||||
math::Matrix _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
|
||||
math::Matrix<3,3> _board_rotation; /**< rotation matrix for the orientation that the board is mounted */
|
||||
math::Matrix<3,3> _external_mag_rotation; /**< rotation matrix for the orientation that an external mag is mounted */
|
||||
bool _mag_is_external; /**< true if the active mag is on an external board */
|
||||
|
||||
struct {
|
||||
@@ -465,8 +465,6 @@ Sensors::Sensors() :
|
||||
/* performance counters */
|
||||
_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
|
||||
|
||||
_board_rotation(3, 3),
|
||||
_external_mag_rotation(3, 3),
|
||||
_mag_is_external(false)
|
||||
{
|
||||
|
||||
@@ -920,7 +918,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
|
||||
|
||||
math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
|
||||
math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.accelerometer_m_s2[0] = vect(0);
|
||||
@@ -946,7 +944,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
|
||||
|
||||
math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
|
||||
math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z);
|
||||
vect = _board_rotation * vect;
|
||||
|
||||
raw.gyro_rad_s[0] = vect(0);
|
||||
@@ -972,7 +970,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
|
||||
|
||||
orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
|
||||
|
||||
math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
|
||||
math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);
|
||||
|
||||
if (_mag_is_external)
|
||||
vect = _external_mag_rotation * vect;
|
||||
|
||||
Reference in New Issue
Block a user