SlugsMAV.cc 7.04 KB
Newer Older
1
2
#include "SlugsMAV.h"

3
4
#include <QDebug>

5
SlugsMAV::SlugsMAV(MAVLinkProtocol* mavlink, int id) :
6
    UAS(mavlink, id)
7
{
8
9
10
11
12
13
14
    widgetTimer = new QTimer (this);
    widgetTimer->setInterval(SLUGS_UPDATE_RATE);

    connect (widgetTimer, SIGNAL(timeout()), this, SLOT(emitSignals()));
    widgetTimer->start();
    memset(&mlRawImuData ,0, sizeof(mavlink_raw_imu_t));// clear all the state structures

15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
#ifdef MAVLINK_ENABLED_SLUGS


    memset(&mlGpsData, 0, sizeof(mavlink_gps_raw_t));
    memset(&mlCpuLoadData, 0, sizeof(mavlink_cpu_load_t));
    memset(&mlAirData, 0, sizeof(mavlink_air_data_t));
    memset(&mlSensorBiasData, 0, sizeof(mavlink_sensor_bias_t));
    memset(&mlDiagnosticData, 0, sizeof(mavlink_diagnostic_t));
    memset(&mlBoot ,0, sizeof(mavlink_boot_t));
    memset(&mlGpsDateTime ,0, sizeof(mavlink_gps_date_time_t));
    memset(&mlApMode ,0, sizeof(mavlink_set_mode_t));
    memset(&mlNavigation ,0, sizeof(mavlink_slugs_navigation_t));
    memset(&mlDataLog ,0, sizeof(mavlink_data_log_t));
    memset(&mlPassthrough ,0, sizeof(mavlink_ctrl_srfc_pt_t));
    memset(&mlActionAck,0, sizeof(mavlink_action_ack_t));
    memset(&mlAction ,0, sizeof(mavlink_slugs_action_t));

    memset(&mlScaled ,0, sizeof(mavlink_scaled_imu_t));
    memset(&mlServo ,0, sizeof(mavlink_servo_output_raw_t));
    memset(&mlChannels ,0, sizeof(mavlink_rc_channels_raw_t));

    updateRoundRobin = 0;
    uasId = id;
#endif
39
40
}

41
42
43
44
45
46
47
48
/**
 * This function is called by MAVLink once a complete, uncorrupted (CRC check valid)
 * mavlink packet is received.
 *
 * @param link Hardware link the message came from (e.g. /dev/ttyUSB0 or UDP port).
 *             messages can be sent back to the system via this link
 * @param message MAVLink message, as received from the MAVLink protocol stack
 */
49
50
void SlugsMAV::receiveMessage(LinkInterface* link, mavlink_message_t message)
{
51
    UAS::receiveMessage(link, message);// Let UAS handle the default message set
52

53
54
    if (message.sysid == uasId) {
#ifdef MAVLINK_ENABLED_SLUGS// Handle your special messages mavlink_message_t* msg = &message;
55

56
57
58
59
        switch (message.msgid) {
        case MAVLINK_MSG_ID_RAW_IMU:
            mavlink_msg_raw_imu_decode(&message, &mlRawImuData);
            break;
60

61
62
63
64
        case MAVLINK_MSG_ID_BOOT:
            mavlink_msg_boot_decode(&message,&mlBoot);
            emit slugsBootMsg(uasId, mlBoot);
            break;
65

66
67
68
        case MAVLINK_MSG_ID_ATTITUDE:
            mavlink_msg_attitude_decode(&message, &mlAttitude);
            break;
69

70
71
72
        case MAVLINK_MSG_ID_GPS_RAW:
            mavlink_msg_gps_raw_decode(&message, &mlGpsData);
            break;
73

74
75
76
        case MAVLINK_MSG_ID_CPU_LOAD:       //170
            mavlink_msg_cpu_load_decode(&message,&mlCpuLoadData);
            break;
77

78
79
80
        case MAVLINK_MSG_ID_AIR_DATA:       //171
            mavlink_msg_air_data_decode(&message,&mlAirData);
            break;
81

82
83
84
        case MAVLINK_MSG_ID_SENSOR_BIAS:    //172
            mavlink_msg_sensor_bias_decode(&message,&mlSensorBiasData);
            break;
85

86
87
88
        case MAVLINK_MSG_ID_DIAGNOSTIC:     //173
            mavlink_msg_diagnostic_decode(&message,&mlDiagnosticData);
            break;
89

90
91
92
        case MAVLINK_MSG_ID_SLUGS_NAVIGATION://176
            mavlink_msg_slugs_navigation_decode(&message,&mlNavigation);
            break;
93

94
95
96
        case MAVLINK_MSG_ID_DATA_LOG:       //177
            mavlink_msg_data_log_decode(&message,&mlDataLog);
            break;
97

98
99
100
        case MAVLINK_MSG_ID_GPS_DATE_TIME:    //179
            mavlink_msg_gps_date_time_decode(&message,&mlGpsDateTime);
            break;
101

102
103
104
        case MAVLINK_MSG_ID_MID_LVL_CMDS:     //180
            mavlink_msg_mid_lvl_cmds_decode(&message, &mlMidLevelCommands);
            break;
105

106
107
108
        case MAVLINK_MSG_ID_CTRL_SRFC_PT:     //181
            mavlink_msg_ctrl_srfc_pt_decode(&message, &mlPassthrough);
            break;
109

110
111
112
        case MAVLINK_MSG_ID_SLUGS_ACTION:     //183
            mavlink_msg_slugs_action_decode(&message, &mlAction);
            break;
113

114
115
116
        case MAVLINK_MSG_ID_SCALED_IMU:
            mavlink_msg_scaled_imu_decode(&message, &mlScaled);
            break;
117

118
119
120
        case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW:
            mavlink_msg_servo_output_raw_decode(&message, &mlServo);
            break;
121

122
123
124
        case MAVLINK_MSG_ID_RC_CHANNELS_RAW:
            mavlink_msg_rc_channels_raw_decode(&message, &mlChannels);
            break;
125

126
127
128
            switch (mlAction.actionId) {
            case SLUGS_ACTION_EEPROM:
                if (mlAction.actionVal == SLUGS_ACTION_FAIL) {
129
                    emit textMessageReceived(message.sysid, message.compid, 255, "EEPROM Write Fail, Data was not saved in Memory!");
130
131
                }
                break;
132

133
134
            case SLUGS_ACTION_PT_CHANGE:
                if (mlAction.actionVal == SLUGS_ACTION_SUCCESS) {
135
                    emit textMessageReceived(message.sysid, message.compid, 0, "Passthrough Succesfully Changed");
136
                }
137
                break;
138

139
140
            case SLUGS_ACTION_MLC_CHANGE:
                if (mlAction.actionVal == SLUGS_ACTION_SUCCESS) {
141
                    emit textMessageReceived(message.sysid, message.compid, 0, "Mid-level Commands Succesfully Changed");
142
                }
143
144
                break;
            }
tecnosapiens's avatar
tecnosapiens committed
145

146
            //break;
tecnosapiens's avatar
tecnosapiens committed
147

148
        default:
149
            //        qDebug() << "\nSLUGS RECEIVED MESSAGE WITH ID" << message.msgid;
150
151
            break;
        }
152
#endif
153
    }
154
}
tecnosapiens's avatar
tecnosapiens committed
155
156
157



158
159
void SlugsMAV::emitSignals (void)
{
160
161
#ifdef MAVLINK_ENABLED_SLUGS
    switch(updateRoundRobin) {
162
    case 1:
163
164
165
        emit slugsCPULoad(uasId, mlCpuLoadData);
        emit slugsSensorBias(uasId,mlSensorBiasData);
        break;
166
167

    case 2:
168
169
        emit slugsAirData(uasId, mlAirData);
        emit slugsDiagnostic(uasId,mlDiagnosticData);
170

171
        break;
172
173

    case 3:
174
175
176
        emit slugsNavegation(uasId, mlNavigation);
        emit slugsDataLog(uasId, mlDataLog);
        break;
177

178
    case 4:
179
        emit slugsGPSDateTime(uasId, mlGpsDateTime);
180

181
        break;
182

183
    case 5:
184
185
186
        emit slugsActionAck(uasId,mlActionAck);
        emit emitGpsSignals();
        break;
187

188
189
190
191
    case 6:
        emit slugsChannels(uasId, mlChannels);
        emit slugsServo(uasId, mlServo);
        emit slugsScaled(uasId, mlScaled);
192

193
194
        break;
    }
195

196
197
198
199
200
201
    emit slugsAttitude(uasId, mlAttitude);
    emit attitudeChanged(this,
                         mlAttitude.roll,
                         mlAttitude.pitch,
                         mlAttitude.yaw,
                         0.0);
202
#endif
tecnosapiens's avatar
tecnosapiens committed
203

204
    emit slugsRawImu(uasId, mlRawImuData);
tecnosapiens's avatar
tecnosapiens committed
205
206


207
208
    // wrap around
    updateRoundRobin = updateRoundRobin > 10? 1: updateRoundRobin + 1;
tecnosapiens's avatar
tecnosapiens committed
209
210


211
}
212

213
#ifdef MAVLINK_ENABLED_SLUGS
214
215
void SlugsMAV::emitGpsSignals (void)
{
216

217
    // qDebug()<<"After Emit GPS Signal"<<mlGpsData.fix_type;
218
219
220
221


    //ToDo Uncomment if. it was comment only to test

222
// if (mlGpsData.fix_type > 0){
223
224
225
226
227
228
    emit globalPositionChanged(this,
                               mlGpsData.lon,
                               mlGpsData.lat,
                               mlGpsData.alt,
                               0.0);

229
    emit slugsGPSCogSog(uasId,mlGpsData.hdg, mlGpsData.v);
230

231
}
232

233

Alejandro's avatar
Alejandro committed
234

235
#endif // MAVLINK_ENABLED_SLUGS