diff --git a/src/lib/matrix b/src/lib/matrix index e2211c5867..cf924956d7 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit e2211c586714d0c3175d5d38c28d7f1f677d64a3 +Subproject commit cf924956d7d62ce18bfc4f8441e9177ddb69c0dc diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 39bb0d4992..88d05d8dc1 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -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]; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index b3ca530d08..eba48450e4 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -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; 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 2f6e9ba17a..2be826c46b 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 @@ -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 || diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bc2fc9b324..cda6c48abc 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -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 q(hil_state.attitude_quaternion); - matrix::Dcm 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) diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 449f0866ce..2435c5fe83 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -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;