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

#include "MAVLinkSimulationMAV.h"

8
MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid, double lat, double lon, int version) :
9 10 11 12 13 14 15 16 17
    QObject(parent),
    link(parent),
    planner(parent, systemid),
    systemid(systemid),
    timer25Hz(0),
    timer10Hz(0),
    timer1Hz(0),
    latitude(lat),
    longitude(lon),
18
    altitude(550.0),
19 20
    x(lat),
    y(lon),
21
    z(altitude),
22 23 24
    roll(0.0),
    pitch(0.0),
    yaw(0.0),
lm's avatar
lm committed
25 26 27
    rollspeed(0.0),
    pitchspeed(0.0),
    yawspeed(0.0),
28 29
    globalNavigation(true),
    firstWP(false),
30 31 32 33 34 35 36 37 38 39 40 41 42 43
    //    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),
44
    nextSPYaw(0.0),
45
    sys_mode(MAV_MODE_PREFLIGHT),
46
    sys_state(MAV_STATE_STANDBY),
lm's avatar
lm committed
47
    nav_mode(0),
48 49
    flying(false),
    mavlink_version(version)
50
{
51
    // Please note: The waypoint planner is running
52
    connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop()));
53
    connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
54
    connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t)));
55
    mainloopTimer.start(20);
56 57 58 59 60
    mainloop();
}

void MAVLinkSimulationMAV::mainloop()
{
61
    // Calculate new simulator values
62
    //    double maxSpeed = 0.0001; // rad/s in earth coordinate frame
63

64
    //        double xNew = // (nextSPX - previousSPX)
65

66
    if (flying) {
67
        sys_state = MAV_STATE_ACTIVE;
lm's avatar
lm committed
68 69
        sys_mode = MAV_MODE_AUTO_ARMED;
        nav_mode = 0;
70 71
    }

72
    // 1 Hz execution
73
    if (timer1Hz <= 0) {
74
        mavlink_message_t msg;
lm's avatar
lm committed
75
        mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_TYPE_FIXED_WING, MAV_AUTOPILOT_ARDUPILOTMEGA, MAV_MODE_GUIDED_ARMED, 0, MAV_STATE_ACTIVE);
76
        link->sendMAVLinkMessage(&msg);
pixhawk's avatar
pixhawk committed
77
        planner.handleMessage(msg);
78 79

        mavlink_servo_output_raw_t servos;
Lorenz Meier's avatar
Lorenz Meier committed
80
        servos.time_usec = 0;
81 82 83 84 85 86 87 88 89 90 91
        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);
92 93 94 95
        timer1Hz = 50;
    }

    // 10 Hz execution
96
    if (timer10Hz <= 0) {
pixhawk's avatar
pixhawk committed
97 98 99 100 101
        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
102

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

lm's avatar
lm committed
105 106 107 108
        if (!(sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL))
        {
            if (!firstWP) {
                //float trueyaw = atan2f(xm, ym);
pixhawk's avatar
pixhawk committed
109

lm's avatar
lm committed
110
                float newYaw = atan2f(ym, xm);
111

lm's avatar
lm committed
112 113 114 115 116
                if (fabs(yaw - newYaw) < 90) {
                    yaw = yaw*0.7 + 0.3*newYaw;
                } else {
                    yaw = newYaw;
                }
117

lm's avatar
lm committed
118
                //qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw;
119

lm's avatar
lm committed
120 121 122 123 124 125 126 127 128 129 130 131 132
                //if (sqrt(xm*xm+ym*ym) > 0.0000001)
                if (flying) {
                    x += cos(yaw)*radPer100ms;
                    y += sin(yaw)*radPer100ms;
                    z += altPer100ms*zsign;
                }

                //if (xm < 0.001) xm
            } else {
                x = nextSPX;
                y = nextSPY;
                z = nextSPZ;
                firstWP = false;
133
//                qDebug() << "INIT STEP";
pixhawk's avatar
pixhawk committed
134
            }
lm's avatar
lm committed
135 136 137 138
        }
        else
        {
            // FIXME Implement heading and altitude controller
pixhawk's avatar
pixhawk committed
139 140 141

        }

142
        // GLOBAL POSITION
143 144
        mavlink_message_t msg;
        mavlink_global_position_int_t pos;
lm's avatar
lm committed
145 146 147
        pos.alt = altitude*1000.0;
        pos.lat = longitude*1E7;
        pos.lon = longitude*1E7;
148
        pos.vx = sin(yaw)*10.0f*100.0f;
lm's avatar
lm committed
149
        pos.vy = 0;
pixhawk's avatar
pixhawk committed
150
        pos.vz = altPer100ms*10.0f*100.0f*zsign*-1.0f;
151 152
        mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos);
        link->sendMAVLinkMessage(&msg);
pixhawk's avatar
pixhawk committed
153
        planner.handleMessage(msg);
154 155

        // ATTITUDE
pixhawk's avatar
pixhawk committed
156
        mavlink_attitude_t attitude;
157
        attitude.time_boot_ms = 0;
lm's avatar
lm committed
158 159
        attitude.roll = roll;
        attitude.pitch = pitch;
160
        attitude.yaw = yaw;
lm's avatar
lm committed
161 162 163
        attitude.rollspeed = rollspeed;
        attitude.pitchspeed = pitchspeed;
        attitude.yawspeed = yawspeed;
pixhawk's avatar
pixhawk committed
164 165 166

        mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
        link->sendMAVLinkMessage(&msg);
167 168 169 170

        // SYSTEM STATUS
        mavlink_sys_status_t status;
        status.load = 300;
lm's avatar
lm committed
171 172
        //        status.mode = sys_mode;
        //        status.nav_mode = nav_mode;
173
        status.errors_comm = 0;
174
        status.voltage_battery = 10500;
lm's avatar
lm committed
175
        //        status.status = sys_state;
176
        status.battery_remaining = 90;
177
        mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
178
        link->sendMAVLinkMessage(&msg);
179
        timer10Hz = 5;
180 181 182

        // VFR HUD
        mavlink_vfr_hud_t hud;
183 184
        hud.airspeed = pos.vx/100.0f;
        hud.groundspeed = pos.vx/100.0f;
lm's avatar
lm committed
185
        hud.alt = altitude;
186
        hud.heading = static_cast<int>((yaw/M_PI)*180.0f+180.0f) % 360;
187
        hud.climb = pos.vz/100.0f;
188 189 190 191 192 193 194 195 196 197
        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
198 199 200
        nav.wp_dist = 2.0f;
        nav.alt_error = 0.5f;
        nav.xtrack_error = 0.2f;
201
        nav.aspd_error = 0.0f;
202 203
        mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav);
        link->sendMAVLinkMessage(&msg);
204 205 206 207 208 209 210

        // 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
211
        pressure.time_usec = 0; // Works also with zero timestamp
212 213
        mavlink_msg_raw_pressure_encode(systemid, MAV_COMP_ID_IMU, &msg, &pressure);
        link->sendMAVLinkMessage(&msg);
214 215 216
    }

    // 25 Hz execution
217
    if (timer25Hz <= 0) {
218 219 220
        // The message container to be used for sending
        mavlink_message_t ret;

lm's avatar
lm committed
221 222 223
        if (sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
        {
            mavlink_hil_controls_t hil;
224 225 226 227
            hil.roll_ailerons = 0.0f;
            hil.pitch_elevator = 0.05f;
            hil.yaw_rudder = 0.05f;
            hil.throttle = 0.6f;
lm's avatar
lm committed
228 229 230 231 232 233
            // Encode the data (adding header and checksums, etc.)
            mavlink_msg_hil_controls_encode(systemid, MAV_COMP_ID_IMU, &ret, &hil);
            // And send it
            link->sendMAVLinkMessage(&ret);
        }

234 235 236
        // Send actual controller outputs
        // This message just shows the direction
        // and magnitude of the control output
lm's avatar
lm committed
237 238 239 240 241 242
        //        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
243 244 245 246

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

        // The message container to be used for sending
247
        //mavlink_message_t ret;
pixhawk's avatar
pixhawk committed
248 249 250 251 252 253 254
        // 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
255
        strcpy((char *)val.name, "FLOAT");
pixhawk's avatar
pixhawk committed
256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282
        // 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
283
        strcpy((char *)valint.name, "INTEGER");
pixhawk's avatar
pixhawk committed
284 285 286 287 288 289 290 291 292 293 294 295 296 297
        // 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);

298 299 300 301 302 303
        timer25Hz = 2;
    }

    timer1Hz--;
    timer10Hz--;
    timer25Hz--;
304 305
}

306
//static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];
lm's avatar
lm committed
307 308

static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
309
//static unsigned error_count;
lm's avatar
lm committed
310

311
mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;
lm's avatar
lm committed
312 313 314 315

static void print_one_field(const mavlink_message_t *msg, const mavlink_field_info_t *f, int idx)
{
#define PRINT_FORMAT(f, def) (f->print_format?f->print_format:def)
lm's avatar
lm committed
316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350
    switch (f->type) {
    case MAVLINK_TYPE_CHAR:
        qDebug(PRINT_FORMAT(f, "%c"), _MAV_RETURN_char(msg, f->wire_offset+idx*1));
        break;
    case MAVLINK_TYPE_UINT8_T:
        qDebug(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint8_t(msg, f->wire_offset+idx*1));
        break;
    case MAVLINK_TYPE_INT8_T:
        qDebug(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int8_t(msg, f->wire_offset+idx*1));
        break;
    case MAVLINK_TYPE_UINT16_T:
        qDebug(PRINT_FORMAT(f, "%u"), _MAV_RETURN_uint16_t(msg, f->wire_offset+idx*2));
        break;
    case MAVLINK_TYPE_INT16_T:
        qDebug(PRINT_FORMAT(f, "%d"), _MAV_RETURN_int16_t(msg, f->wire_offset+idx*2));
        break;
    case MAVLINK_TYPE_UINT32_T:
        qDebug(PRINT_FORMAT(f, "%lu"), (unsigned long)_MAV_RETURN_uint32_t(msg, f->wire_offset+idx*4));
        break;
    case MAVLINK_TYPE_INT32_T:
        qDebug(PRINT_FORMAT(f, "%ld"), (long)_MAV_RETURN_int32_t(msg, f->wire_offset+idx*4));
        break;
    case MAVLINK_TYPE_UINT64_T:
        qDebug(PRINT_FORMAT(f, "%llu"), (unsigned long long)_MAV_RETURN_uint64_t(msg, f->wire_offset+idx*8));
        break;
    case MAVLINK_TYPE_INT64_T:
        qDebug(PRINT_FORMAT(f, "%lld"), (long long)_MAV_RETURN_int64_t(msg, f->wire_offset+idx*8));
        break;
    case MAVLINK_TYPE_FLOAT:
        qDebug(PRINT_FORMAT(f, "%f"), (double)_MAV_RETURN_float(msg, f->wire_offset+idx*4));
        break;
    case MAVLINK_TYPE_DOUBLE:
        qDebug(PRINT_FORMAT(f, "%f"), _MAV_RETURN_double(msg, f->wire_offset+idx*8));
        break;
    }
lm's avatar
lm committed
351 352 353 354
}

static void print_field(const mavlink_message_t *msg, const mavlink_field_info_t *f)
{
lm's avatar
lm committed
355 356 357 358 359 360 361 362 363 364
    qDebug("%s: ", f->name);
    if (f->array_length == 0) {
        print_one_field(msg, f, 0);
        qDebug(" ");
    } else {
        unsigned i;
        /* print an array */
        if (f->type == MAVLINK_TYPE_CHAR) {
            qDebug("'%.*s'", f->array_length,
                   f->wire_offset+(const char *)_MAV_PAYLOAD(msg));
lm's avatar
lm committed
365

lm's avatar
lm committed
366 367 368 369 370 371
        } else {
            qDebug("[ ");
            for (i=0; i<f->array_length; i++) {
                print_one_field(msg, f, i);
                if (i < f->array_length) {
                    qDebug(", ");
lm's avatar
lm committed
372
                }
lm's avatar
lm committed
373 374
            }
            qDebug("]");
lm's avatar
lm committed
375
        }
lm's avatar
lm committed
376 377
    }
    qDebug(" ");
lm's avatar
lm committed
378 379 380 381
}

static void print_message(const mavlink_message_t *msg)
{
lm's avatar
lm committed
382 383 384
    const mavlink_message_info_t *m = &message_info[msg->msgid];
    const mavlink_field_info_t *f = m->fields;
    unsigned i;
385 386 387 388 389
//    qDebug("%s { ", m->name);
//    for (i=0; i<m->num_fields; i++) {
//        print_field(msg, &f[i]);
//    }
//    qDebug("}\n");
lm's avatar
lm committed
390 391
}

392 393
void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
lm's avatar
lm committed
394 395 396
    if (msg.sysid != systemid)
    {
        print_message(&msg);
397
//        qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
lm's avatar
lm committed
398
    }
399

400
    switch(msg.msgid) {
401 402
    case MAVLINK_MSG_ID_ATTITUDE:
        break;
403 404 405
    case MAVLINK_MSG_ID_SET_MODE: {
        mavlink_set_mode_t mode;
        mavlink_msg_set_mode_decode(&msg, &mode);
406
        if (systemid == mode.target_system) sys_mode = mode.base_mode;
407 408
    }
    break;
409
    case MAVLINK_MSG_ID_HIL_STATE_QUATERNION:
lm's avatar
lm committed
410
    {
411 412
        mavlink_hil_state_quaternion_t state;
        mavlink_msg_hil_state_quaternion_decode(&msg, &state);
413 414 415 416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449 450 451 452 453

        double a = state.attitude_quaternion[0];
        double b = state.attitude_quaternion[1];
        double c = state.attitude_quaternion[2];
        double d = state.attitude_quaternion[3];
        double aSq = a * a;
        double bSq = b * b;
        double cSq = c * c;
        double dSq = d * d;
        float dcm[3][3];
        dcm[0][0] = aSq + bSq - cSq - dSq;
        dcm[0][1] = 2.0 * (b * c - a * d);
        dcm[0][2] = 2.0 * (a * c + b * d);
        dcm[1][0] = 2.0 * (b * c + a * d);
        dcm[1][1] = aSq - bSq + cSq - dSq;
        dcm[1][2] = 2.0 * (c * d - a * b);
        dcm[2][0] = 2.0 * (b * d - a * c);
        dcm[2][1] = 2.0 * (a * b + c * d);
        dcm[2][2] = aSq - bSq - cSq + dSq;

        float phi, theta, psi;
        theta = asin(-dcm[2][0]);

        if (fabs(theta - M_PI_2) < 1.0e-3f) {
            phi = 0.0f;
            psi = (atan2(dcm[1][2] - dcm[0][1],
                    dcm[0][2] + dcm[1][1]) + phi);

        } else if (fabs(theta + M_PI_2) < 1.0e-3f) {
            phi = 0.0f;
            psi = atan2f(dcm[1][2] - dcm[0][1],
                      dcm[0][2] + dcm[1][1] - phi);

        } else {
            phi = atan2f(dcm[2][1], dcm[2][2]);
            psi = atan2f(dcm[1][0], dcm[0][0]);
        }

        roll = phi;
        pitch = theta;
        yaw = psi;
lm's avatar
lm committed
454 455 456 457 458 459 460 461
        rollspeed = state.rollspeed;
        pitchspeed = state.pitchspeed;
        yawspeed = state.yawspeed;
        latitude = state.lat;
        longitude = state.lon;
        altitude = state.alt;
    }
    break;
lm's avatar
lm committed
462
    // FIXME MAVLINKV10PORTINGNEEDED
lm's avatar
lm committed
463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486
    //    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;
    ////            }
    ////            break;
    ////            }

    //            // 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);
    //        }
    //    }
487
    break;
488 489 490
    case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
        mavlink_set_local_position_setpoint_t sp;
        mavlink_msg_set_local_position_setpoint_decode(&msg, &sp);
491
        if (sp.target_system == this->systemid) {
lm's avatar
lm committed
492
            nav_mode = 0;
493 494 495 496 497 498 499 500 501 502 503 504 505 506 507 508 509 510
            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;
511 512
    }
}