MAVLinkSimulationMAV.cc 15.1 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;
80
        servos.time_boot_ms = 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 133
                //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;
                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

        }


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

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

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

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

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

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

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

lm's avatar
lm committed
222 223 224
        if (sys_mode & MAV_MODE_FLAG_DECODE_POSITION_HIL)
        {
            mavlink_hil_controls_t hil;
225 226 227 228
            hil.roll_ailerons = 0.0f;
            hil.pitch_elevator = 0.05f;
            hil.yaw_rudder = 0.05f;
            hil.throttle = 0.6f;
lm's avatar
lm committed
229 230 231 232 233 234
            // 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);
        }

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

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

        // The message container to be used for sending
248
        //mavlink_message_t ret;
pixhawk's avatar
pixhawk committed
249 250 251 252 253 254 255
        // 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
256
        strcpy((char *)val.name, "FLOAT");
pixhawk's avatar
pixhawk committed
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 283
        // 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
284
        strcpy((char *)valint.name, "INTEGER");
pixhawk's avatar
pixhawk committed
285 286 287 288 289 290 291 292 293 294 295 296 297 298
        // 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);

299 300 301 302 303 304
        timer25Hz = 2;
    }

    timer1Hz--;
    timer10Hz--;
    timer25Hz--;
305 306
}

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

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

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

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
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 351
    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
352 353 354 355
}

static void print_field(const mavlink_message_t *msg, const mavlink_field_info_t *f)
{
lm's avatar
lm committed
356 357 358 359 360 361 362 363 364 365
    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
366

lm's avatar
lm committed
367 368 369 370 371 372
        } 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
373
                }
lm's avatar
lm committed
374 375
            }
            qDebug("]");
lm's avatar
lm committed
376
        }
lm's avatar
lm committed
377 378
    }
    qDebug(" ");
lm's avatar
lm committed
379 380 381 382
}

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

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

401
    switch(msg.msgid) {
402 403
    case MAVLINK_MSG_ID_ATTITUDE:
        break;
404 405 406
    case MAVLINK_MSG_ID_SET_MODE: {
        mavlink_set_mode_t mode;
        mavlink_msg_set_mode_decode(&msg, &mode);
407
        if (systemid == mode.target_system) sys_mode = mode.base_mode;
408 409
    }
    break;
lm's avatar
lm committed
410 411 412 413 414 415 416 417 418 419 420 421 422 423 424
    case MAVLINK_MSG_ID_HIL_STATE:
    {
        mavlink_hil_state_t state;
        mavlink_msg_hil_state_decode(&msg, &state);
        roll = state.roll;
        pitch = state.pitch;
        yaw = state.yaw;
        rollspeed = state.rollspeed;
        pitchspeed = state.pitchspeed;
        yawspeed = state.yawspeed;
        latitude = state.lat;
        longitude = state.lon;
        altitude = state.alt;
    }
    break;
lm's avatar
lm committed
425
    // FIXME MAVLINKV10PORTINGNEEDED
lm's avatar
lm committed
426 427 428 429 430 431 432 433 434 435 436 437 438 439 440 441 442 443 444 445 446 447 448 449
    //    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);
    //        }
    //    }
450
    break;
451 452 453
    case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
        mavlink_set_local_position_setpoint_t sp;
        mavlink_msg_set_local_position_setpoint_decode(&msg, &sp);
454
        if (sp.target_system == this->systemid) {
lm's avatar
lm committed
455
            nav_mode = 0;
456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473
            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;
474 475
    }
}