fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-08 04:20:35 +08: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
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
+12 -12
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;
src/modules/commander/accelerometer_calibration.cpp
+4 -2
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++;
}
src/modules/commander/commander.cpp
+12 -2
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);
src/modules/ekf2/ekf2_main.cpp
+6 -5
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);
src/modules/mavlink/mavlink_messages.cpp
+8 -6
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) {
src/modules/mavlink/mavlink_receiver.cpp
+11 -8
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;
src/modules/position_estimator_inav/position_estimator_inav_main.cpp
+20 -14
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;
src/modules/sdlog2/sdlog2.c
+6 -6
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);
}
src/modules/vtol_att_control/tailsitter.cpp
+6 -2
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();
}