diff --git a/msg/vehicle_attitude.msg b/msg/vehicle_attitude.msg index 2c38af5c0a..40a3b03c0a 100644 --- a/msg/vehicle_attitude.msg +++ b/msg/vehicle_attitude.msg @@ -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 diff --git a/src/drivers/bst/bst.cpp b/src/drivers/bst/bst.cpp index 9a8d55ac40..d131ab2723 100644 --- a/src/drivers/bst/bst.cpp +++ b/src/drivers/bst/bst.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #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 q(&att.q[0]); + matrix::Euler euler(q); BSTPacket 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); } diff --git a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index b09912c7a5..ee2c882578 100644 --- a/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/examples/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -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 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 q(_ekf->states[0], _ekf->states[1], _ekf->states[2], _ekf->states[3]); + matrix::Euler euler(q); + _local_pos.yaw = euler(2); if (!PX4_ISFINITE(_local_pos.x) || !PX4_ISFINITE(_local_pos.y) || diff --git a/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp index 5db8f77ac4..f5697fda03 100644 --- a/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp +++ b/src/examples/mc_att_control_multiplatform/mc_att_control_base.cpp @@ -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 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 q(&_v_att->data().q[0]); + R.set(&q._data[0][0]); /* all input data is ready, run controller itself */ diff --git a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp index df7240ac4c..3785bd33df 100644 --- a/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp +++ b/src/examples/mc_pos_control_multiplatform/mc_pos_control.cpp @@ -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 q(&_att->data().q[0]); + matrix::Euler 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 q(&_att->data().q[0]); + matrix::Euler euler(q); + matrix::Dcm 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); } } diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c index 6ca3afb618..ce0718839e 100644 --- a/src/examples/px4_simple_app/px4_simple_app.c +++ b/src/examples/px4_simple_app/px4_simple_app.c @@ -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); } diff --git a/src/lib/terrain_estimation/terrain_estimator.cpp b/src/lib/terrain_estimation/terrain_estimator.cpp index 1b2f075a5a..9d83c07efc 100644 --- a/src/lib/terrain_estimation/terrain_estimator.cpp +++ b/src/lib/terrain_estimation/terrain_estimator.cpp @@ -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 R_att(attitude->R); - matrix::Vector a(sensor->accelerometer_m_s2); + if (attitude->q_valid) { + matrix::Quaternion q(&attitude->q[0]); + matrix::Dcm R_att(q); + matrix::Vector a(&sensor->accelerometer_m_s2[0]); matrix::Vector 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 q(&attitude->q[0]); + matrix::Euler euler(q); float d = distance->current_distance; matrix::Matrix C; @@ -124,7 +126,7 @@ void TerrainEstimator::measurement_update(uint64_t time_ref, const struct vehicl float R = 0.009f; matrix::Vector y; - y(0) = d * cosf(attitude->roll) * cosf(attitude->pitch); + y(0) = d * cosf(euler(0)) * cosf(euler(1)); // residual matrix::Matrix S_I = (C * _P * C.transpose()); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 281551832a..7c4d90480e 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -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; diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 523287fef2..1cf0d027d4 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -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 q(&att.q[0]); + matrix::Euler euler(q); + roll_mean += euler(0); + pitch_mean += euler(1); counter++; } diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 634d799d56..5856405b5b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -78,6 +78,15 @@ #include #include #include + +#include +#include +#include +#include +#include +#include + +#include #include #include #include @@ -106,7 +115,6 @@ #include #include #include -#include 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 q(&attitude.q[0]); + matrix::Euler euler(q); + home.yaw = euler(2); PX4_INFO("home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 1d55434ab0..a4b25bfecb 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -721,10 +721,10 @@ void Ekf2::task_main() // generate remaining vehicle attitude data att.timestamp = hrt_absolute_time(); - matrix::Euler euler(q); - att.roll = euler(0); - att.pitch = euler(1); - att.yaw = euler(2); + //matrix::Euler 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 euler(q); + lpos.yaw = euler(2); float terrain_vpos; lpos.dist_bottom_valid = _ekf.get_terrain_vert_pos(&terrain_vpos); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a37a307d1f..afc47a16bc 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -841,11 +841,12 @@ protected: if (_att_sub->update(&_att_time, &att)) { mavlink_attitude_t msg; - + matrix::Quaternion q(&att.q[0]); + matrix::Euler 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 q(&att.q[0]); + matrix::Euler 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) { diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7a7f74f207..43c97dc71b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 q(&hil_attitude.q[0]); + matrix::Euler 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 q(&hil_attitude.q[0]); + matrix::Euler euler(q); + hil_local_pos.yaw = euler(2); hil_local_pos.xy_global = true; hil_local_pos.z_global = true; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 70959bcf22..a7b9f53f5e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp @@ -500,11 +500,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* sensor combined */ orb_check(sensor_combined_sub, &updated); + matrix::Quaternion q(&att.q[0]); + matrix::Dcm 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 q(&att.q[0]); + matrix::Dcm 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 euler(R); + local_pos.yaw = euler(0); local_pos.dist_bottom_valid = dist_bottom_valid; local_pos.eph = eph; local_pos.epv = epv; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 118330a1e7..e88df4d7df 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -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); } diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index ba31477541..8d0af17f83 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -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 q(&_v_att->q[0]); + matrix::Euler 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(); }