px4sim 0.1.6

A wrapper to simplify creating a custom PX4 SITL simulator.
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

use crate::data::DroneData;
use crate::util::time_since_epoch;
use mavlink::{
    common::{
        HilSensorUpdatedFlags, MavAutopilot, MavMessage, MavModeFlag, MavState, MavType,
        HEARTBEAT_DATA, HIL_GPS_DATA, HIL_SENSOR_DATA, HIL_STATE_QUATERNION_DATA, SYSTEM_TIME_DATA,
    },
    MavHeader,
};

pub fn make_header(id: u8) -> MavHeader {
    MavHeader {
        system_id: id,
        component_id: 1,
        sequence: 1,
    }
}

pub fn make_heartbeat() -> MavMessage {
    MavMessage::HEARTBEAT(HEARTBEAT_DATA {
        custom_mode: 0,
        mavtype: MavType::MAV_TYPE_QUADROTOR,
        autopilot: MavAutopilot::MAV_AUTOPILOT_PX4,
        base_mode: MavModeFlag::empty(),
        system_status: MavState::MAV_STATE_STANDBY,
        mavlink_version: 3,
    })
}

pub fn make_system_time(boot_time: u32) -> MavMessage {
    MavMessage::SYSTEM_TIME(SYSTEM_TIME_DATA {
        time_unix_usec: time_since_epoch(),
        time_boot_ms: boot_time,
    })
}

pub fn make_hil_gps(data: &DroneData) -> MavMessage {
    let (lat, lon) = data.coordinates;
    let (vn, ve, vd) = data.gps_velocity;
    let (eph, epv) = data.position_error;
    MavMessage::HIL_GPS(HIL_GPS_DATA {
        lat,
        lon,
        vn,
        ve,
        vd,
        eph,
        epv,
        alt: data.altitude,
        cog: data.course_over_ground,
        vel: data.ground_velocity,
        fix_type: 3,
        time_usec: time_since_epoch(),
        satellites_visible: 10,
    })
}

pub fn make_hil_state_quaternion(data: &DroneData) -> MavMessage {
    let (lat, lon) = data.coordinates;
    let (rollspeed, pitchspeed, yawspeed) = data.angular_velocity;
    let (vx, vy, vz) = data.linear_velocity;
    let (xacc, yacc, zacc) = data.linear_acceleration;
    MavMessage::HIL_STATE_QUATERNION(HIL_STATE_QUATERNION_DATA {
        lat,
        lon,
        rollspeed,
        pitchspeed,
        yawspeed,
        vx,
        vy,
        vz,
        xacc,
        yacc,
        zacc,
        alt: data.altitude,
        attitude_quaternion: data.attitude_quaternion,
        ind_airspeed: data.indicated_airspeed,
        true_airspeed: data.true_airspeed,
        time_usec: time_since_epoch(),
    })
}

pub fn make_hil_sensor(data: &DroneData) -> MavMessage {
    let (xacc, yacc, zacc) = data.measured_acceleration;
    let (xgyro, ygyro, zgyro) = data.measured_angular_velocity;
    let (xmag, ymag, zmag) = data.measured_magnetic_field;
    MavMessage::HIL_SENSOR(HIL_SENSOR_DATA {
        xacc,
        yacc,
        zacc,
        xgyro,
        ygyro,
        zgyro,
        xmag,
        ymag,
        zmag,
        abs_pressure: data.absolute_pressure,
        diff_pressure: data.differential_pressure,
        pressure_alt: data.altitude_from_pressure,
        temperature: data.measured_temperature,
        fields_updated: HilSensorUpdatedFlags::all(),
        time_usec: time_since_epoch(),
    })
}