yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

adapted to new vehicle attitude message

Browse Source
This commit is contained in:
tumbili
2016-04-20 17:51:47 +02:00
committed by Lorenz Meier
parent 215bfaa377
commit 5e0e522903
16 changed files with 142 additions and 94 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
msg/vehicle_attitude.msg
View File

@@ -4,5 +4,6 @@ float32 rollspeed # Angular velocity about body north axis (x) in rad/s
float32 pitchspeed # Angular velocity about body east axis (y) in rad/s
float32 yawspeed # Angular velocity about body down axis (z) in rad/s
float32[4] q # Quaternion (NED)
bool q_valid # Quaternion valid
# TOPICS vehicle_attitude vehicle_attitude_groundtruth

9
src/drivers/bst/bst.cpp
View File

@@ -49,6 +49,7 @@
#include <math.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/battery_status.h>
#include <mathlib/math/Quaternion.hpp>
#define BST_DEVICE_PATH "/dev/bst0"
@@ -289,11 +290,13 @@ void BST::cycle()
if (updated) {
vehicle_attitude_s att;
orb_copy(ORB_ID(vehicle_attitude), _attitude_sub, &att);
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
BSTPacket<BSTAttitude> bst_att = {};
bst_att.type = 0x1E;
bst_att.payload.roll = swap_int32(att.roll * 10000);
bst_att.payload.pitch = swap_int32(att.pitch * 10000);
bst_att.payload.yaw = swap_int32(att.yaw * 10000);
bst_att.payload.roll = swap_int32(euler(0) * 10000);
bst_att.payload.pitch = swap_int32(euler(1) * 10000);
bst_att.payload.yaw = swap_int32(euler(2) * 10000);
send_packet(bst_att);
}

30
src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@@ -820,15 +820,15 @@ void AttitudePositionEstimatorEKF::initializeGPS()
void AttitudePositionEstimatorEKF::publishAttitude()
{
// Output results
math::Quaternion q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
math::Matrix<3, 3> R = q.to_dcm();
math::Vector<3> euler = R.to_euler();
matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
//math::Matrix<3, 3> R = q.to_dcm();
//math::Vector<3> euler = R.to_euler();
for (int i = 0; i < 3; i++) {
/*for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
PX4_R(_att.R, i, j) = R(i, j);
}
}
}*/
_att.timestamp = _last_sensor_timestamp;
_att.q[0] = _ekf->states[0];
@@ -836,21 +836,21 @@ void AttitudePositionEstimatorEKF::publishAttitude()
_att.q[2] = _ekf->states[2];
_att.q[3] = _ekf->states[3];
_att.q_valid = true;
_att.R_valid = true;
//_att.R_valid = true;
_att.timestamp = _last_sensor_timestamp;
_att.roll = euler(0);
_att.pitch = euler(1);
_att.yaw = euler(2);
//_att.timestamp = _last_sensor_timestamp;
//_att.roll = euler(0);
//_att.pitch = euler(1);
//_att.yaw = euler(2);
_att.rollspeed = _ekf->dAngIMU.x / _ekf->dtIMU - _ekf->states[10] / _ekf->dtIMUfilt;
_att.pitchspeed = _ekf->dAngIMU.y / _ekf->dtIMU - _ekf->states[11] / _ekf->dtIMUfilt;
_att.yawspeed = _ekf->dAngIMU.z / _ekf->dtIMU - _ekf->states[12] / _ekf->dtIMUfilt;
// gyro offsets
_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
//_att.rate_offsets[0] = _ekf->states[10] / _ekf->dtIMUfilt;
//_att.rate_offsets[1] = _ekf->states[11] / _ekf->dtIMUfilt;
//_att.rate_offsets[2] = _ekf->states[12] / _ekf->dtIMUfilt;
/* lazily publish the attitude only once available */
if (_att_pub != nullptr) {
@@ -973,7 +973,9 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
_local_pos.xy_global = _gps_initialized; //TODO: Handle optical flow mode here
_local_pos.z_global = false;
_local_pos.yaw = _att.yaw;
matrix::Quaternion<float> q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]);
matrix::Euler<float> euler(q);
_local_pos.yaw = euler(2);
if (!PX4_ISFINITE(_local_pos.x) ||
!PX4_ISFINITE(_local_pos.y) ||

6
src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp
View File

@@ -87,11 +87,13 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
/* construct attitude setpoint rotation matrix */
math::Matrix<3, 3> R_sp;
R_sp.set(_v_att_sp->data().R_body);
matrix::Quaternion<float> q_sp(&_v_att_sp->data().q_d[0]);
R_sp.set(&q_sp._data[0][0]);
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att->data().R);
matrix::Quaternion<float> q(&_v_att->data().q[0]);
R.set(&q._data[0][0]);
/* all input data is ready, run controller itself */

26
src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp
View File

@@ -250,7 +250,9 @@ MulticopterPositionControlMultiplatform::reset_alt_sp()
//XXX hack until #1741 is in/ported
/* reset yaw sp */
_att_sp_msg.data().yaw_body = _att->data().yaw;
matrix::Quaternion<float> q(&_att->data().q[0]);
matrix::Euler<float> euler(q);
_att_sp_msg.data().yaw_body = euler(2);
//XXX: port this once a mavlink like interface is available
// mavlink_log_info(&_mavlink_log_pub, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
@@ -582,6 +584,10 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
static bool was_armed = false;
static uint64_t t_prev = 0;
matrix::Quaternion<float> q(&_att->data().q[0]);
matrix::Euler<float> euler(q);
matrix::Dcm<float> R(q);
uint64_t t = get_time_micros();
float dt = t_prev != 0 ? (t - t_prev) * 0.000001f : 0.005f;
t_prev = t;
@@ -641,7 +647,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
_att_sp_msg.data().roll_body = 0.0f;
_att_sp_msg.data().pitch_body = 0.0f;
_att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().yaw_body = euler(2);
_att_sp_msg.data().thrust = 0.0f;
_att_sp_msg.data().timestamp = get_time_micros();
@@ -815,11 +821,11 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
/* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att->data().R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att->data().R, 2, 2);
if (R(2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / R(2, 2);
} else if (PX4_R(_att->data().R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att->data().R, 2, 2) + 1.0f;
} else if (R(2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * R(2, 2) + 1.0f;
saturation_z = true;
} else {
@@ -1005,7 +1011,7 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
/* reset yaw setpoint to current position if needed */
if (reset_yaw_sp) {
reset_yaw_sp = false;
_att_sp_msg.data().yaw_body = _att->data().yaw;
_att_sp_msg.data().yaw_body = euler(2);
}
/* do not move yaw while arming */
@@ -1014,13 +1020,13 @@ void MulticopterPositionControlMultiplatform::handle_vehicle_attitude(const px4
_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - euler(2));
if (yaw_offs < - YAW_OFFSET_MAX) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) - YAW_OFFSET_MAX);
} else if (yaw_offs > YAW_OFFSET_MAX) {
_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
_att_sp_msg.data().yaw_body = _wrap_pi(euler(2) + YAW_OFFSET_MAX);
}
}

10
src/examples/px4_simple_app/px4_simple_app.c
View File

@@ -107,9 +107,13 @@ int px4_simple_app_main(int argc, char *argv[])
(double)raw.accelerometer_m_s2[2]);
/* set att and publish this information for other apps */
att.roll = raw.accelerometer_m_s2[0];
att.pitch = raw.accelerometer_m_s2[1];
att.yaw = raw.accelerometer_m_s2[2];
//att.roll = raw.accelerometer_m_s2[0];
//att.pitch = raw.accelerometer_m_s2[1];
//att.yaw = raw.accelerometer_m_s2[2];
att.q[0] = raw.accelerometer_m_s2[0];
att.q[1] = raw.accelerometer_m_s2[1];
att.q[2] = raw.accelerometer_m_s2[2];
orb_publish(ORB_ID(vehicle_attitude), att_pub, &att);
}

12
src/lib/terrain_estimation/terrain_estimator.cpp
View File

@@ -65,9 +65,10 @@ void TerrainEstimator::predict(float dt, const struct vehicle_attitude_s *attitu
const struct sensor_combined_s *sensor,
const struct distance_sensor_s *distance)
{
if (attitude->R_valid) {
matrix::Matrix<float, 3, 3> R_att(attitude->R);
matrix::Vector<float, 3> a(sensor->accelerometer_m_s2);
if (attitude->q_valid) {
matrix::Quaternion<float> q(&attitude->q[0]);
matrix::Dcm<float> R_att(q);
matrix::Vector<float, 3> a(&sensor->accelerometer_m_s2[0]);
matrix::Vector<float, 3> u;
u = R_att * a;
_u_z = u(2) + 9.81f; // compensate for gravity
@@ -115,7 +116,8 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
}
if (distance->timestamp > _time_last_distance) {
matrix::Quaternion<float> q(&attitude->q[0]);
matrix::Euler<float> euler(q);
float d = distance->current_distance;
matrix::Matrix<float, 1, n_x> C;
@@ -124,7 +126,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl
float R = 0.009f;
matrix::Vector<float, 1> y;
y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch);
y(0) = d * cosf(euler(0)) * cosf(euler(1));
// residual
matrix::Matrix<float, 1, 1> S_I = (C * _P * C.transpose());

24
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@@ -468,26 +468,26 @@ void AttitudeEstimatorQ::task_main()
struct vehicle_attitude_s att = {};
att.timestamp = sensors.timestamp;
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
//att.roll = euler(0);
//att.pitch = euler(1);
//att.yaw = euler(2);
att.rollspeed = _rates(0);
att.pitchspeed = _rates(1);
att.yawspeed = _rates(2);
for (int i = 0; i < 3; i++) {
att.g_comp[i] = _accel(i) - _pos_acc(i);
}
//for (int i = 0; i < 3; i++) {
// att.g_comp[i] = _accel(i) - _pos_acc(i);
//}
/* copy offsets */
memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
///* copy offsets */
//memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
Matrix<3, 3> R = _q.to_dcm();
//Matrix<3, 3> R = _q.to_dcm();
/* copy rotation matrix */
memcpy(&att.R[0], R.data, sizeof(att.R));
att.R_valid = true;
///* copy rotation matrix */
//memcpy(&att.R[0], R.data, sizeof(att.R));
//att.R_valid = true;
memcpy(&att.q[0], _q.data, sizeof(att.q));
att.q_valid = true;

6
src/modules/commander/accelerometer_calibration.cpp
View File

@@ -687,8 +687,10 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub) {
}
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
roll_mean += att.roll;
pitch_mean += att.pitch;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
roll_mean += euler(0);
pitch_mean += euler(1);
counter++;
}

14
src/modules/commander/commander.cpp
View File

@@ -78,6 +78,15 @@
#include <systemlib/state_table.h>
#include <systemlib/systemlib.h>
#include <systemlib/hysteresis/hysteresis.h>
#include <sys/stat.h>
#include <string.h>
#include <math.h>
#include <poll.h>
#include <float.h>
#include <matrix/math.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/battery_status.h>
@@ -106,7 +115,6 @@
#include <uORB/topics/vehicle_land_detected.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/uORB.h>
typedef enum VEHICLE_MODE_FLAG
{
@@ -1160,7 +1168,9 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
home.y = localPosition.y;
home.z = localPosition.z;
home.yaw = attitude.yaw;
matrix::Quaternion<float> q(&attitude.q[0]);
matrix::Euler<float> euler(q);
home.yaw = euler(2);
PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);

11
src/modules/ekf2/ekf2_main.cpp
View File

@@ -721,10 +721,10 @@ void Ekf2::task_main()
// generate remaining vehicle attitude data
att.timestamp = hrt_absolute_time();
matrix::Euler<float> euler(q);
att.roll = euler(0);
att.pitch = euler(1);
att.yaw = euler(2);
//matrix::Euler<float> euler(q);
//att.roll = euler(0);
//att.pitch = euler(1);
//att.yaw = euler(2);
att.q[0] = q(0);
att.q[1] = q(1);
@@ -777,7 +777,8 @@ void Ekf2::task_main()
lpos.ref_lon = ekf_origin.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees
// The rotation of the tangent plane vs. geographical north
lpos.yaw = att.yaw;
matrix::Euler<float> euler(q);
lpos.yaw = euler(2);
float terrain_vpos;
lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos);

14
src/modules/mavlink/mavlink_messages.cpp
View File

@@ -841,11 +841,12 @@ protected:
if (_att_sub->update(&_att_time, &att)) {
mavlink_attitude_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
msg.time_boot_ms = att.timestamp / 1000;
msg.roll = att.roll;
msg.pitch = att.pitch;
msg.yaw = att.yaw;
msg.roll = euler(0);
msg.pitch = euler(1);
msg.yaw = euler(2);
msg.rollspeed = att.rollspeed;
msg.pitchspeed = att.pitchspeed;
msg.yawspeed = att.yawspeed;
@@ -1014,10 +1015,11 @@ protected:
if (updated) {
mavlink_vfr_hud_t msg;
matrix::Quaternion<float> q(&att.q[0]);
matrix::Euler<float> euler(q);
msg.airspeed = airspeed.indicated_airspeed_m_s;
msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e);
msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F;
msg.heading = _wrap_2pi(euler(2)) * M_RAD_TO_DEG_F;
msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f;
if (_pos_time > 0) {

19
src/modules/mavlink/mavlink_receiver.cpp
View File

@@ -1920,8 +1920,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
math::Vector<3> euler = C_nb.to_euler();
hil_attitude.timestamp = timestamp;
memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
hil_attitude.R_valid = true;
//memcpy(hil_attitude.R, C_nb.data, sizeof(hil_attitude.R));
//hil_attitude.R_valid = true;
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
@@ -1929,9 +1929,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_attitude.q[3] = q(3);
hil_attitude.q_valid = true;
hil_attitude.roll = euler(0);
hil_attitude.pitch = euler(1);
hil_attitude.yaw = euler(2);
//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;
@@ -1948,7 +1948,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
{
struct vehicle_global_position_s hil_global_pos;
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
matrix::Quaternion<float> q(&hil_attitude.q[0]);
matrix::Euler<float> euler(q);
hil_global_pos.timestamp = timestamp;
hil_global_pos.lat = hil_state.lat / ((double)1e7);
hil_global_pos.lon = hil_state.lon / ((double)1e7);
@@ -1956,7 +1957,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_global_pos.vel_n = hil_state.vx / 100.0f;
hil_global_pos.vel_e = hil_state.vy / 100.0f;
hil_global_pos.vel_d = hil_state.vz / 100.0f;
hil_global_pos.yaw = hil_attitude.yaw;
hil_global_pos.yaw = euler(1);
hil_global_pos.eph = 2.0f;
hil_global_pos.epv = 4.0f;
@@ -1997,7 +1998,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_local_pos.vx = hil_state.vx / 100.0f;
hil_local_pos.vy = hil_state.vy / 100.0f;
hil_local_pos.vz = hil_state.vz / 100.0f;
hil_local_pos.yaw = hil_attitude.yaw;
matrix::Quaternion<float> q(&hil_attitude.q[0]);
matrix::Euler<float> euler(q);
hil_local_pos.yaw = euler(2);
hil_local_pos.xy_global = true;
hil_local_pos.z_global = true;

34
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
View File

@@ -500,11 +500,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* sensor combined */
orb_check(sensor_combined_sub, &updated);
matrix::Quaternion<float> q(&att.q[0]);
matrix::Dcm<float> R(q);
if (updated) {
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
if (sensor.timestamp + sensor.accelerometer_timestamp_relative != accel_timestamp) {
if (att.R_valid) {
if (att.q_valid) {
/* correct accel bias */
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
@@ -515,7 +518,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
acc[i] = 0.0f;
for (int j = 0; j < 3; j++) {
acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j];
acc[i] += R(i, j) * sensor.accelerometer_m_s2[j];
}
}
@@ -548,9 +551,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance
&& lidar.current_distance < lidar.max_distance
&& (PX4_R(att.R, 2, 2) > 0.7f)) {
if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
&& (R(2, 2) > 0.7f)) {
if (!use_lidar_prev && use_lidar) {
lidar_first = true;
@@ -559,7 +561,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
use_lidar_prev = use_lidar;
lidar_time = t;
dist_ground = lidar.current_distance * PX4_R(att.R, 2, 2); //vertical distance
dist_ground = lidar.current_distance * R(2, 2); //vertical distance
if (lidar_first) {
lidar_first = false;
@@ -602,7 +604,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float flow_q = flow.quality / 255.0f;
float dist_bottom = lidar.current_distance;
if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && PX4_R(att.R, 2, 2) > 0.7f) {
if (dist_bottom > flow_min_dist && flow_q > params.flow_q_min && R(2, 2) > 0.7f) {
/* distance to surface */
//float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar
float flow_dist = dist_bottom; //use this if using lidar
@@ -612,7 +614,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float body_v_est[2] = { 0.0f, 0.0f };
for (int i = 0; i < 2; i++) {
body_v_est[i] = PX4_R(att.R, 0, i) * x_est[1] + PX4_R(att.R, 1, i) * y_est[1] + PX4_R(att.R, 2, i) * z_est[1];
body_v_est[i] = R( 0, i) * x_est[1] + R(1, i) * y_est[1] + R(2, i) * z_est[1];
}
/* set this flag if flow should be accurate according to current velocity and attitude rate estimate */
@@ -706,7 +708,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* project measurements vector to NED basis, skip Z component */
for (int i = 0; i < 2; i++) {
for (int j = 0; j < 3; j++) {
flow_v[i] += PX4_R(att.R, i, j) * flow_m[j];
flow_v[i] += R(i, j) * flow_m[j];
}
}
@@ -715,7 +717,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
w_flow = R(2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
@@ -946,6 +948,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
matrix::Quaternion<float> q(&att.q[0]);
matrix::Dcm<float> R(q);
/* check for timeout on FLOW topic */
if ((flow_valid || lidar_valid) && t > (flow_time + flow_topic_timeout)) {
flow_valid = false;
@@ -1110,7 +1115,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
c += R(j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
@@ -1140,7 +1145,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float c = 0.0f;
for (int j = 0; j < 3; j++) {
c += PX4_R(att.R, j, i) * accel_bias_corr[j];
c += R(j, i) * accel_bias_corr[j];
}
if (PX4_ISFINITE(c)) {
@@ -1311,7 +1316,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
est_buf[buf_ptr][2][1] = z_est[1];
/* push current rotation matrix to buffer */
memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
memcpy(R_buf[buf_ptr], &R._data[0][0], sizeof(R._data));
buf_ptr++;
@@ -1331,7 +1336,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
local_pos.vy = y_est[1];
local_pos.z = z_est[0];
local_pos.vz = z_est[1];
local_pos.yaw = att.yaw;
matrix::Euler<float> euler(R);
local_pos.yaw = euler(0);
local_pos.dist_bottom_valid = dist_bottom_valid;
local_pos.eph = eph;
local_pos.epv = epv;

12
src/modules/sdlog2/sdlog2.c
View File

@@ -2269,15 +2269,15 @@ int sdlog2_thread_main(int argc, char *argv[])
log_msg.body.log_ATT.q_x = buf.att.q[1];
log_msg.body.log_ATT.q_y = buf.att.q[2];
log_msg.body.log_ATT.q_z = buf.att.q[3];
log_msg.body.log_ATT.roll = buf.att.roll;
log_msg.body.log_ATT.pitch = buf.att.pitch;
log_msg.body.log_ATT.yaw = buf.att.yaw;
log_msg.body.log_ATT.roll = 0;
log_msg.body.log_ATT.pitch = 0;
log_msg.body.log_ATT.yaw = 0;
log_msg.body.log_ATT.roll_rate = buf.att.rollspeed;
log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed;
log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed;
log_msg.body.log_ATT.gx = buf.att.g_comp[0];
log_msg.body.log_ATT.gy = buf.att.g_comp[1];
log_msg.body.log_ATT.gz = buf.att.g_comp[2];
log_msg.body.log_ATT.gx = 0;
log_msg.body.log_ATT.gy = 0;
log_msg.body.log_ATT.gz = 0;
LOGBUFFER_WRITE_AND_COUNT(ATT);
}

8
src/modules/vtol_att_control/tailsitter.cpp
View File

@@ -131,6 +131,10 @@ void Tailsitter::update_vtol_state()
* For the backtransition the pitch is controlled in MC mode again and switches to full MC control reaching the sufficient pitch angle.
*/
matrix::Quaternion<float> q(&_v_att->q[0]);
matrix::Euler<float> euler(q);
float pitch = euler(1);
if (!_attc->is_fixed_wing_requested()) {
@@ -157,7 +161,7 @@ void Tailsitter::update_vtol_state()
case TRANSITION_BACK:
// check if we have reached pitch angle to switch to MC mode
if (_v_att->pitch >= PITCH_TRANSITION_BACK) {
if (pitch >= PITCH_TRANSITION_BACK) {
_vtol_schedule.flight_mode = MC_MODE;
}
@@ -180,7 +184,7 @@ void Tailsitter::update_vtol_state()
// check if we have reached airspeed and pitch angle to switch to TRANSITION P2 mode
if ((_airspeed->indicated_airspeed_m_s >= _params_tailsitter.airspeed_trans
&& _v_att->pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
&& pitch <= PITCH_TRANSITION_FRONT_P1) || can_transition_on_ground()) {
_vtol_schedule.flight_mode = FW_MODE;
//_vtol_schedule.transition_start = hrt_absolute_time();
}