mirror of
https://github.com/okalachev/flix.git
synced 2025-07-28 20:08:53 +00:00
Minor code simplifications
This commit is contained in:
parent
253f2fe3dd
commit
fe98a5bf97
@ -47,9 +47,9 @@ void sendMavlink() {
|
|||||||
lastFast = t;
|
lastFast = t;
|
||||||
|
|
||||||
const float zeroQuat[] = {0, 0, 0, 0};
|
const float zeroQuat[] = {0, 0, 0, 0};
|
||||||
Quaternion attitudeFRD = fluToFrd(attitude); // MAVLink uses FRD coordinate system
|
Quaternion att = fluToFrd(attitude); // MAVLink uses FRD coordinate system
|
||||||
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
mavlink_msg_attitude_quaternion_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg,
|
||||||
time, attitudeFRD.w, attitudeFRD.x, attitudeFRD.y, attitudeFRD.z, rates.x, rates.y, rates.z, zeroQuat);
|
time, att.w, att.x, att.y, att.z, rates.x, rates.y, rates.z, zeroQuat);
|
||||||
sendMessage(&msg);
|
sendMessage(&msg);
|
||||||
|
|
||||||
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
|
mavlink_msg_rc_channels_scaled_pack(SYSTEM_ID, MAV_COMP_ID_AUTOPILOT1, &msg, controlsTime * 1000, 0,
|
||||||
|
@ -1,6 +1,7 @@
|
|||||||
<?xml version="1.0"?>
|
<?xml version="1.0"?>
|
||||||
<sdf version="1.5">
|
<sdf version="1.5">
|
||||||
<model name="flix">
|
<model name="flix">
|
||||||
|
<plugin name="flix" filename="libflix.so"/>
|
||||||
<link name="body">
|
<link name="body">
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>0.065</mass>
|
<mass>0.065</mass>
|
||||||
@ -23,38 +24,14 @@
|
|||||||
<update_rate>1000</update_rate>
|
<update_rate>1000</update_rate>
|
||||||
<imu>
|
<imu>
|
||||||
<angular_velocity>
|
<angular_velocity>
|
||||||
<x>
|
<x><noise type="gaussian"><stddev>0.00174533</stddev></noise></x><!-- 0.1 degrees per second -->
|
||||||
<noise type="gaussian">
|
<y><noise type="gaussian"><stddev>0.00174533</stddev></noise></y>
|
||||||
<stddev>0.00174533</stddev><!-- 0.1 degrees per second -->
|
<z><noise type="gaussian"><stddev>0.00174533</stddev></noise></z>
|
||||||
</noise>
|
|
||||||
</x>
|
|
||||||
<y>
|
|
||||||
<noise type="gaussian">
|
|
||||||
<stddev>0.00174533</stddev>
|
|
||||||
</noise>
|
|
||||||
</y>
|
|
||||||
<z>
|
|
||||||
<noise type="gaussian">
|
|
||||||
<stddev>0.00174533</stddev>
|
|
||||||
</noise>
|
|
||||||
</z>
|
|
||||||
</angular_velocity>
|
</angular_velocity>
|
||||||
<linear_acceleration>
|
<linear_acceleration>
|
||||||
<x>
|
<x><noise type="gaussian"><stddev>0.0784</stddev></noise></x><!-- 8 mg -->
|
||||||
<noise type="gaussian">
|
<y><noise type="gaussian"><stddev>0.0784</stddev></noise></y>
|
||||||
<stddev>0.0784</stddev><!-- 8 mg -->
|
<z><noise type="gaussian"><stddev>0.0784</stddev></noise></z>
|
||||||
</noise>
|
|
||||||
</x>
|
|
||||||
<y>
|
|
||||||
<noise type="gaussian">
|
|
||||||
<stddev>0.0784</stddev>
|
|
||||||
</noise>
|
|
||||||
</y>
|
|
||||||
<z>
|
|
||||||
<noise type="gaussian">
|
|
||||||
<stddev>0.0784</stddev>
|
|
||||||
</noise>
|
|
||||||
</z>
|
|
||||||
</linear_acceleration>
|
</linear_acceleration>
|
||||||
</imu>
|
</imu>
|
||||||
</sensor>
|
</sensor>
|
||||||
@ -90,6 +67,5 @@
|
|||||||
<material><ambient>1 1 1 0.5</ambient><diffuse>1 1 1 0.5</diffuse></material>
|
<material><ambient>1 1 1 0.5</ambient><diffuse>1 1 1 0.5</diffuse></material>
|
||||||
</visual>
|
</visual>
|
||||||
</link>
|
</link>
|
||||||
<plugin name="flix" filename="libflix.so"/>
|
|
||||||
</model>
|
</model>
|
||||||
</sdf>
|
</sdf>
|
||||||
|
Loading…
x
Reference in New Issue
Block a user