aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.cpp113
-rw-r--r--src/modules/att_pos_estimator_ekf/KalmanNav.hpp20
-rw-r--r--src/modules/att_pos_estimator_ekf/kalman_main.cpp2
-rwxr-xr-xsrc/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp20
-rw-r--r--src/modules/commander/accelerometer_calibration.cpp14
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp36
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp10
-rw-r--r--src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp128
-rw-r--r--src/modules/navigator/navigator_main.cpp16
-rw-r--r--src/modules/sensors/sensors.cpp12
10 files changed, 181 insertions, 190 deletions
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index ecca04dd7..aca3fe7b6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -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)) {
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index a69bde1a6..46ee4b6c8 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -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. */
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 372b2d162..3d20d4d2d 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -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);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 983ac7c8d..48a11e7fc 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -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)) {
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 5eeca5a1a..36b75dd58 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -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);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index a9648b207..5e32ac32f 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -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 &current_position, const math::Vector2f &ground_speed,
+FixedwingPositionControl::control_position(const math::Vector<2> &current_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 &current_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 &current_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 &current_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 &current_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),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7b6fad658..b4f7f2dfe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -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;
diff --git a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
index 76f053372..30c073c29 100644
--- a/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
+++ b/src/modules/mc_att_control_vector/mc_att_control_vector_main.cpp
@@ -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 */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index f6c44444a..04307d96b 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -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;
}
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index f99312f8c..9e2eeafa4 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -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;
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-11 00:26:08 +0000