MAVLinkSimulationMAV.cc 11.2 KB
Newer Older
1
#include <QDebug>
2
#include <cmath>
pixhawk's avatar
pixhawk committed
3
#include <qmath.h>
4 5 6

#include "MAVLinkSimulationMAV.h"

7
MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon, int version) :
8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25
    QObject(parent),
    link(parent),
    planner(parent, systemid),
    systemid(systemid),
    timer25Hz(0),
    timer10Hz(0),
    timer1Hz(0),
    latitude(lat),
    longitude(lon),
    altitude(0.0),
    x(lat),
    y(lon),
    z(550),
    roll(0.0),
    pitch(0.0),
    yaw(0.0),
    globalNavigation(true),
    firstWP(false),
LM's avatar
LM committed
26 27 28 29 30 31 32 33 34 35 36 37 38 39
//    previousSPX(8.548056),
//    previousSPY(47.376389),
//    previousSPZ(550),
//    previousSPYaw(0.0),
//    nextSPX(8.548056),
//    nextSPY(47.376389),
//    nextSPZ(550),
        previousSPX(37.480391),
        previousSPY(122.282883),
        previousSPZ(550),
        previousSPYaw(0.0),
        nextSPX(37.480391),
        nextSPY(122.282883),
        nextSPZ(550),
40 41 42 43 44 45
    nextSPYaw(0.0),
    sys_mode(MAV_MODE_READY),
    sys_state(MAV_STATE_STANDBY),
    nav_mode(MAV_NAV_GROUNDED),
    flying(false),
    mavlink_version(version)
46
{
47
    // Please note: The waypoint planner is running
48
    connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
49
    connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
50
    connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
51
    mainloopTimer.start(20);
52 53 54 55 56
    mainloop();
}

void MAVLinkSimulationMAV::mainloop()
{
57
    // Calculate new simulator values
58
    //    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
59

60
    //        double xNew = // (nextSPX - previousSPX)
61

62
    if (flying) {
63 64 65 66 67
        sys_state = MAV_STATE_ACTIVE;
        sys_mode = MAV_MODE_AUTO;
        nav_mode = MAV_NAV_WAYPOINT;
    }

68
    // 1 Hz execution
69
    if (timer1Hz <= 0) {
70
        mavlink_message_t msg;
71
        mavlink_msg_heartbeat_pack_version_free(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK, mavlink_version);
72
        link->sendMAVLinkMessage(&msg);
pixhawk's avatar
pixhawk committed
73
        planner.handleMessage(msg);
74 75 76 77 78 79 80 81 82 83 84 85 86

        mavlink_servo_output_raw_t servos;
        servos.servo1_raw = 1000;
        servos.servo2_raw = 1250;
        servos.servo3_raw = 1400;
        servos.servo4_raw = 1500;
        servos.servo5_raw = 1800;
        servos.servo6_raw = 1500;
        servos.servo7_raw = 1500;
        servos.servo8_raw = 2000;

        mavlink_msg_servo_output_raw_encode(systemid, MAV_COMP_ID_IMU, &msg, &servos);
        link->sendMAVLinkMessage(&msg);
87 88 89 90
        timer1Hz = 50;
    }

    // 10 Hz execution
91
    if (timer10Hz <= 0) {
pixhawk's avatar
pixhawk committed
92 93 94 95 96
        double radPer100ms = 0.00006;
        double altPer100ms = 0.4;
        double xm = (nextSPX - x);
        double ym = (nextSPY - y);
        double zm = (nextSPZ - z);
pixhawk's avatar
pixhawk committed
97

pixhawk's avatar
pixhawk committed
98
        float zsign = (zm < 0) ? -1.0f : 1.0f;
pixhawk's avatar
pixhawk committed
99

100
        if (!firstWP) {
pixhawk's avatar
pixhawk committed
101 102
            //float trueyaw = atan2f(xm, ym);

103
            float newYaw = atan2f(ym, xm);
104

105
            if (fabs(yaw - newYaw) < 90) {
106
                yaw = yaw*0.7 + 0.3*newYaw;
107
            } else {
108 109 110 111 112 113
                yaw = newYaw;
            }

            //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;

            //if (sqrt(xm*xm+ym*ym) > 0.0000001)
114
            if (flying) {
115 116
                x += cos(yaw)*radPer100ms;
                y += sin(yaw)*radPer100ms;
pixhawk's avatar
pixhawk committed
117 118 119 120
                z += altPer100ms*zsign;
            }

            //if (xm < 0.001) xm
121
        } else {
pixhawk's avatar
pixhawk committed
122 123 124 125 126 127 128 129
            x = nextSPX;
            y = nextSPY;
            z = nextSPZ;
            firstWP = false;
            qDebug() << "INIT STEP";
        }


130
        // GLOBAL POSITION
131 132 133
        mavlink_message_t msg;
        mavlink_global_position_int_t pos;
        pos.alt = z*1000.0;
134 135
        pos.lat = x*1E7;
        pos.lon = y*1E7;
136
        pos.vx = sin(yaw)*10.0f*100.0f;
lm's avatar
lm committed
137
        pos.vy = 0;
pixhawk's avatar
pixhawk committed
138
        pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
139 140
        mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
        link->sendMAVLinkMessage(&msg);
pixhawk's avatar
pixhawk committed
141
        planner.handleMessage(msg);
142 143

        // ATTITUDE
pixhawk's avatar
pixhawk committed
144
        mavlink_attitude_t attitude;
145
        attitude.usec = 0;
pixhawk's avatar
pixhawk committed
146 147
        attitude.roll = 0.0f;
        attitude.pitch = 0.0f;
148
        attitude.yaw = yaw;
149 150 151
        attitude.rollspeed = 0.0f;
        attitude.pitchspeed = 0.0f;
        attitude.yawspeed = 0.0f;
pixhawk's avatar
pixhawk committed
152 153 154

        mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
        link->sendMAVLinkMessage(&msg);
155 156 157 158 159 160 161 162 163

        // SYSTEM STATUS
        mavlink_sys_status_t status;
        status.load = 300;
        status.mode = sys_mode;
        status.nav_mode = nav_mode;
        status.packet_drop = 0;
        status.vbat = 10500;
        status.status = sys_state;
164
        status.battery_remaining = 912;
165
        mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
166
        link->sendMAVLinkMessage(&msg);
167
        timer10Hz = 5;
168 169 170

        // VFR HUD
        mavlink_vfr_hud_t hud;
171 172 173
        hud.airspeed = pos.vx/100.0f;
        hud.groundspeed = pos.vx/100.0f;
        hud.alt = z;
174
        hud.heading = static_cast<int>((yaw/M_PI)*180.0f+180.0f) % 360;
175
        hud.climb = pos.vz/100.0f;
176 177 178 179 180 181 182 183 184 185
        hud.throttle = 90;
        mavlink_msg_vfr_hud_encode(systemid, MAV_COMP_ID_IMU, &msg, &hud);
        link->sendMAVLinkMessage(&msg);

        // NAV CONTROLLER
        mavlink_nav_controller_output_t nav;
        nav.nav_roll = roll;
        nav.nav_pitch = pitch;
        nav.nav_bearing = yaw;
        nav.target_bearing = yaw;
lm's avatar
lm committed
186 187 188
        nav.wp_dist = 2.0f;
        nav.alt_error = 0.5f;
        nav.xtrack_error = 0.2f;
189
        nav.aspd_error = 0.0f;
190 191
        mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav);
        link->sendMAVLinkMessage(&msg);
192 193 194 195 196 197 198 199 200 201

        // RAW PRESSURE
        mavlink_raw_pressure_t pressure;
        pressure.press_abs = 1000;
        pressure.press_diff1 = 2000;
        pressure.press_diff2 = 5000;
        pressure.temperature = 18150; // 18.15 deg Celsius
        pressure.usec = 0; // Works also with zero timestamp
        mavlink_msg_raw_pressure_encode(systemid, MAV_COMP_ID_IMU, &msg, &pressure);
        link->sendMAVLinkMessage(&msg);
202 203 204
    }

    // 25 Hz execution
205
    if (timer25Hz <= 0) {
206 207 208
        // The message container to be used for sending
        mavlink_message_t ret;

209
#ifdef MAVLINK_ENABLED_PIXHAWK
210 211 212 213 214 215
        // Send which controllers are active
        mavlink_control_status_t control_status;
        control_status.control_att = 1;
        control_status.control_pos_xy = 1;
        control_status.control_pos_yaw = 1;
        control_status.control_pos_z = 1;
216 217 218
        control_status.gps_fix = 2;        // 2D GPS fix
        control_status.position_fix = 3;   // 3D fix from GPS + barometric pressure
        control_status.vision_fix = 0;     // no fix from vision system
219
        control_status.ahrs_health = 230;
220 221
        mavlink_msg_control_status_encode(systemid, MAV_COMP_ID_IMU, &ret, &control_status);
        link->sendMAVLinkMessage(&ret);
222
#endif //MAVLINK_ENABLED_PIXHAWK
223 224 225 226

        // Send actual controller outputs
        // This message just shows the direction
        // and magnitude of the control output
227 228 229 230 231 232
//        mavlink_position_controller_output_t pos;
//        pos.x = sin(yaw)*127.0f;
//        pos.y = cos(yaw)*127.0f;
//        pos.z = 0;
//        mavlink_msg_position_controller_output_encode(systemid, MAV_COMP_ID_IMU, &ret, &pos);
//        link->sendMAVLinkMessage(&ret);
pixhawk's avatar
pixhawk committed
233 234 235 236

        // Send a named value with name "FLOAT" and 0.5f as value

        // The message container to be used for sending
237
        //mavlink_message_t ret;
pixhawk's avatar
pixhawk committed
238 239 240 241 242 243 244
        // The C-struct holding the data to be sent
        mavlink_named_value_float_t val;

        // Fill in the data
        // Name of the value, maximum 10 characters
        // see full message specs at:
        // http://pixhawk.ethz.ch/wiki/mavlink/
Andrew Tridgell's avatar
Andrew Tridgell committed
245
        strcpy((char *)val.name, "FLOAT");
pixhawk's avatar
pixhawk committed
246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272
        // Value, in this case 0.5
        val.value = 0.5f;

        // Encode the data (adding header and checksums, etc.)
        mavlink_msg_named_value_float_encode(systemid, MAV_COMP_ID_IMU, &ret, &val);
        // And send it
        link->sendMAVLinkMessage(&ret);

        // MICROCONTROLLER SEND CODE:

        // uint8_t buf[MAVLINK_MAX_PACKET_LEN];
        // int16_t len = mavlink_msg_to_send_buffer(buf, &ret);
        // uart0_transmit(buf, len);


        // SEND INTEGER VALUE

        // We are reusing the "mavlink_message_t ret"
        // message buffer

        // The C-struct holding the data to be sent
        mavlink_named_value_int_t valint;

        // Fill in the data
        // Name of the value, maximum 10 characters
        // see full message specs at:
        // http://pixhawk.ethz.ch/wiki/mavlink/
Andrew Tridgell's avatar
Andrew Tridgell committed
273
        strcpy((char *)valint.name, "INTEGER");
pixhawk's avatar
pixhawk committed
274 275 276 277 278 279 280 281 282 283 284 285 286 287
        // Value, in this case 18000
        valint.value = 18000;

        // Encode the data (adding header and checksums, etc.)
        mavlink_msg_named_value_int_encode(systemid, MAV_COMP_ID_IMU, &ret, &valint);
        // And send it
        link->sendMAVLinkMessage(&ret);

        // MICROCONTROLLER SEND CODE:

        // uint8_t buf[MAVLINK_MAX_PACKET_LEN];
        // int16_t len = mavlink_msg_to_send_buffer(buf, &ret);
        // uart0_transmit(buf, len);

288 289 290 291 292 293
        timer25Hz = 2;
    }

    timer1Hz--;
    timer10Hz--;
    timer25Hz--;
294 295 296 297
}

void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
298
    //qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
299

300
    switch(msg.msgid) {
301 302
    case MAVLINK_MSG_ID_ATTITUDE:
        break;
303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322
    case MAVLINK_MSG_ID_SET_MODE: {
        mavlink_set_mode_t mode;
        mavlink_msg_set_mode_decode(&msg, &mode);
        if (systemid == mode.target) sys_mode = mode.mode;
    }
    break;
    case MAVLINK_MSG_ID_ACTION: {
        mavlink_action_t action;
        mavlink_msg_action_decode(&msg, &action);
        if (systemid == action.target && (action.target_component == 0 || action.target_component == MAV_COMP_ID_IMU)) {
            mavlink_action_ack_t ack;
            ack.action = action.action;
            switch (action.action) {
            case MAV_ACTION_TAKEOFF:
                flying = true;
                nav_mode = MAV_NAV_LIFTOFF;
                ack.result = 1;
                break;
            default: {
                ack.result = 0;
323
            }
324
            break;
325
            }
326 327 328 329 330

            // Give feedback about action
            mavlink_message_t r_msg;
            mavlink_msg_action_ack_encode(systemid, MAV_COMP_ID_IMU, &r_msg, &ack);
            link->sendMAVLinkMessage(&r_msg);
331
        }
332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356
    }
    break;
    case MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_SET: {
        mavlink_local_position_setpoint_set_t sp;
        mavlink_msg_local_position_setpoint_set_decode(&msg, &sp);
        if (sp.target_system == this->systemid) {
            nav_mode = MAV_NAV_WAYPOINT;
            previousSPX = nextSPX;
            previousSPY = nextSPY;
            previousSPZ = nextSPZ;
            nextSPX = sp.x;
            nextSPY = sp.y;
            nextSPZ = sp.z;

            // Rotary wing
            //nextSPYaw = sp.yaw;

            // Airplane
            //yaw = atan2(previousSPX-nextSPX, previousSPY-nextSPY);

            //if (!firstWP) firstWP = true;
        }
        //qDebug() << "UPDATED SP:" << "X" << nextSPX << "Y" << nextSPY << "Z" << nextSPZ;
    }
    break;
357 358
    }
}