fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-05 19:40:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

add matrix copyTo

Browse Source
This commit is contained in:
Daniel Agar
2017-02-24 13:49:45 -05:00
committed by Lorenz Meier
parent f43f3baa02
commit ecb2511a7b
6 changed files with 37 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/ekf2_main.cpp
+2 -8
View File
@@ -730,10 +730,7 @@ void Ekf2::task_main()
ctrl_state.z_pos = position[2];
// Attitude quaternion
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);
q.copyTo(ctrl_state.q);
_ekf.get_quat_reset(&ctrl_state.delta_q_reset[0], &ctrl_state.quat_reset_counter);
@@ -789,10 +786,7 @@ void Ekf2::task_main()
struct vehicle_attitude_s att = {};
att.timestamp = hrt_absolute_time();
att.q[0] = q(0);
att.q[1] = q(1);
att.q[2] = q(2);
att.q[3] = q(3);
q.copyTo(att.q);
att.rollspeed = gyro_rad[0];
att.pitchspeed = gyro_rad[1];
src/modules/fw_att_control/fw_att_control_main.cpp
+1 -4
View File
@@ -1001,10 +1001,7 @@ FixedwingAttitudeControl::task_main()
_att_sp.thrust = _manual.z;
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
_att_sp.q_d[0] = q(0);
_att_sp.q_d[1] = q(1);
_att_sp.q_d[2] = q(2);
_att_sp.q_d[3] = q(3);
q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
int instance;
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+1 -4
View File
@@ -2340,10 +2340,7 @@ FixedwingPositionControl::task_main()
}
Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body));
_att_sp.q_d[0] = q(0);
_att_sp.q_d[1] = q(1);
_att_sp.q_d[2] = q(2);
_att_sp.q_d[3] = q(3);
q.copyTo(_att_sp.q_d);
_att_sp.q_d_valid = true;
if (!_control_mode.flag_control_offboard_enabled ||
src/modules/mavlink/mavlink_receiver.cpp
+30 -52
View File
@@ -1166,19 +1166,12 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
vision_attitude.timestamp = sync_stamp(pos.usec);
math::Quaternion q;
q.from_euler(pos.roll, pos.pitch, pos.yaw);
matrix::Quatf q(matrix::Eulerf(pos.roll, pos.pitch, pos.yaw));
q.copyTo(vision_attitude.q);
vision_attitude.q[0] = q(0);
vision_attitude.q[1] = q(1);
vision_attitude.q[2] = q(2);
vision_attitude.q[3] = q(3);
int instance_id = 0;
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &instance_id,
ORB_PRIO_DEFAULT);
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &instance_id,
ORB_PRIO_DEFAULT);
int inst = 0;
orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &inst, ORB_PRIO_DEFAULT);
orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &inst, ORB_PRIO_DEFAULT);
}
void
@@ -1690,7 +1683,7 @@ MavlinkReceiver::get_message_interval(int msgId)
{
unsigned interval = 0;
MavlinkStream *stream;
MavlinkStream *stream = nullptr;
LL_FOREACH(_mavlink->get_streams(), stream) {
if (stream->get_id() == msgId) {
interval = stream->get_interval();
@@ -1708,7 +1701,7 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
mavlink_system_time_t time;
mavlink_msg_system_time_decode(msg, &time);
timespec tv;
timespec tv = {};
px4_clock_gettime(CLOCK_REALTIME, &tv);
// date -d @1234567890: Sat Feb 14 02:31:30 MSK 2009
@@ -1732,11 +1725,10 @@ MavlinkReceiver::handle_message_system_time(mavlink_message_t *msg)
void
MavlinkReceiver::handle_message_timesync(mavlink_message_t *msg)
{
mavlink_timesync_t tsync;
mavlink_timesync_t tsync = {};
mavlink_msg_timesync_decode(msg, &tsync);
struct time_offset_s tsync_offset;
memset(&tsync_offset, 0, sizeof(tsync_offset));
struct time_offset_s tsync_offset = {};
uint64_t now_ns = hrt_absolute_time() * 1000LL ;
@@ -1981,8 +1973,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
uint64_t timestamp = hrt_absolute_time();
struct vehicle_gps_position_s hil_gps;
memset(&hil_gps, 0, sizeof(hil_gps));
struct vehicle_gps_position_s hil_gps = {};
hil_gps.timestamp_time_relative = 0;
hil_gps.time_utc_usec = gps.time_usec;
@@ -2017,7 +2008,7 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
{
mavlink_follow_target_t follow_target_msg;
follow_target_s follow_target_topic = { };
follow_target_s follow_target_topic = {};
mavlink_msg_follow_target_decode(msg, &follow_target_msg);
@@ -2038,7 +2029,7 @@ void MavlinkReceiver::handle_message_follow_target(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
{
mavlink_adsb_vehicle_t adsb;
transponder_report_s t = { };
transponder_report_s t = {};
mavlink_msg_adsb_vehicle_decode(msg, &adsb);
@@ -2071,7 +2062,7 @@ void MavlinkReceiver::handle_message_adsb_vehicle(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
{
mavlink_collision_t collision;
collision_report_s t = { };
collision_report_s t = {};
mavlink_msg_collision_decode(msg, &collision);
@@ -2094,11 +2085,10 @@ void MavlinkReceiver::handle_message_collision(mavlink_message_t *msg)
void MavlinkReceiver::handle_message_gps_rtcm_data(mavlink_message_t *msg)
{
mavlink_gps_rtcm_data_t gps_rtcm_data_msg;
gps_inject_data_s gps_inject_data_topic;
mavlink_gps_rtcm_data_t gps_rtcm_data_msg = {};
mavlink_msg_gps_rtcm_data_decode(msg, &gps_rtcm_data_msg);
gps_inject_data_s gps_inject_data_topic = {};
gps_inject_data_topic.len = math::min((int)sizeof(gps_rtcm_data_msg.data),
(int)sizeof(uint8_t) * gps_rtcm_data_msg.len);
gps_inject_data_topic.flags = gps_rtcm_data_msg.flags;
@@ -2127,8 +2117,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* airspeed */
{
struct airspeed_s airspeed;
memset(&airspeed, 0, sizeof(airspeed));
struct airspeed_s airspeed = {};
airspeed.timestamp = timestamp;
airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
@@ -2145,17 +2134,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* attitude */
struct vehicle_attitude_s hil_attitude;
{
memset(&hil_attitude, 0, sizeof(hil_attitude));
math::Quaternion q(hil_state.attitude_quaternion);
math::Matrix<3, 3> C_nb = q.to_dcm();
math::Vector<3> euler = C_nb.to_euler();
hil_attitude = {};
hil_attitude.timestamp = timestamp;
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
matrix::Quatf q(hil_state.attitude_quaternion);
q.copyTo(hil_attitude.q);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;
@@ -2171,9 +2154,9 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* global position */
{
struct vehicle_global_position_s hil_global_pos;
memset(&hil_global_pos, 0, sizeof(hil_global_pos));
struct vehicle_global_position_s hil_global_pos = {};
matrix::Eulerf euler = matrix::Quatf(hil_attitude.q);
hil_global_pos.timestamp = timestamp;
hil_global_pos.lat = hil_state.lat / ((double)1e7);
hil_global_pos.lon = hil_state.lon / ((double)1e7);
@@ -2208,8 +2191,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
hil_local_pos.ref_alt = _hil_local_alt0;
}
float x;
float y;
float x = 0.0f;
float y = 0.0f;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
hil_local_pos.timestamp = timestamp;
hil_local_pos.xy_valid = true;
@@ -2254,8 +2237,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* accelerometer */
{
struct accel_report accel;
memset(&accel, 0, sizeof(accel));
struct accel_report accel = {};
accel.timestamp = timestamp;
accel.x_raw = hil_state.xacc / CONSTANTS_ONE_G * 1e3f;
@@ -2276,8 +2258,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* battery status */
{
struct battery_status_s hil_battery_status;
memset(&hil_battery_status, 0, sizeof(hil_battery_status));
struct battery_status_s hil_battery_status = {};
hil_battery_status.timestamp = timestamp;
hil_battery_status.voltage_v = 11.1f;
@@ -2295,8 +2276,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
/* control state */
control_state_s ctrl_state = {};
matrix::Quaternion<float> q(hil_state.attitude_quaternion);
matrix::Dcm<float> R_to_body(q.inversed());
matrix::Quatf q(hil_state.attitude_quaternion);
matrix::Dcmf R_to_body(q.inversed());
//Time
ctrl_state.timestamp = hrt_absolute_time();
@@ -2311,8 +2292,8 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
// Local Position NED:
//ctrl_state: position in local earth frame
//hil_state : Latitude/Longitude expressed as * 1E7
float x;
float y;
float x = 0.0f;
float y = 0.0f;
double lat = hil_state.lat * 1e-7;
double lon = hil_state.lon * 1e-7;
map_projection_project(&_hil_local_proj_ref, lat, lon, &x, &y);
@@ -2321,10 +2302,7 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
ctrl_state.z_pos = hil_state.alt / 1000.0f;
// Attitude quaternion
ctrl_state.q[0] = q(0);
ctrl_state.q[1] = q(1);
ctrl_state.q[2] = q(2);
ctrl_state.q[3] = q(3);
q.copyTo(ctrl_state.q);
// Velocity
//ctrl_state: velocity in body frame (x forward/y right/z down)
src/modules/simulator/simulator_mavlink.cpp
+2 -6
View File
@@ -386,12 +386,8 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
{
hil_attitude.timestamp = timestamp;
math::Quaternion q(hil_state.attitude_quaternion);
hil_attitude.q[0] = q(0);
hil_attitude.q[1] = q(1);
hil_attitude.q[2] = q(2);
hil_attitude.q[3] = q(3);
matrix::Quatf q(hil_state.attitude_quaternion);
q.copyTo(hil_attitude.q);
hil_attitude.rollspeed = hil_state.rollspeed;
hil_attitude.pitchspeed = hil_state.pitchspeed;