MAVLinkSimulationMAV.cc 14.1 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),
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
    nextSPYaw(0.0),
41
    sys_mode(MAV_MODE_PREFLIGHT),
42
    sys_state(MAV_STATE_STANDBY),
lm's avatar
lm committed
43
    nav_mode(0),
44 45
    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
        sys_state = MAV_STATE_ACTIVE;
lm's avatar
lm committed
64 65
        sys_mode = MAV_MODE_AUTO_ARMED;
        nav_mode = 0;
66 67
    }

68
    // 1 Hz execution
69
    if (timer1Hz <= 0) {
70
        mavlink_message_t msg;
lm's avatar
lm committed
71
        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);
72
        link->sendMAVLinkMessage(&msg);
pixhawk's avatar
pixhawk committed
73
        planner.handleMessage(msg);
74 75

        mavlink_servo_output_raw_t servos;
lm's avatar
lm committed
76
        servos.time_usec = 0;
77 78 79 80 81 82 83 84 85 86 87
        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);
88 89 90 91
        timer1Hz = 50;
    }

    // 10 Hz execution
92
    if (timer10Hz <= 0) {
pixhawk's avatar
pixhawk committed
93 94 95 96 97
        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
98

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

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

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

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

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

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

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


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

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

        mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude);
        link->sendMAVLinkMessage(&msg);
156 157 158 159

        // SYSTEM STATUS
        mavlink_sys_status_t status;
        status.load = 300;
160 161
//        status.mode = sys_mode;
//        status.nav_mode = nav_mode;
162
        status.errors_comm = 0;
163 164
        status.voltage_battery = 10500;
//        status.status = sys_state;
165
        status.battery_remaining = 90;
166
        mavlink_msg_sys_status_encode(systemid, MAV_COMP_ID_IMU, &msg, &status);
167
        link->sendMAVLinkMessage(&msg);
168
        timer10Hz = 5;
169 170 171

        // VFR HUD
        mavlink_vfr_hud_t hud;
172 173 174
        hud.airspeed = pos.vx/100.0f;
        hud.groundspeed = pos.vx/100.0f;
        hud.alt = z;
175
        hud.heading = static_cast<int>((yaw/M_PI)*180.0f+180.0f) % 360;
176
        hud.climb = pos.vz/100.0f;
177 178 179 180 181 182 183 184 185 186
        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
187 188 189
        nav.wp_dist = 2.0f;
        nav.alt_error = 0.5f;
        nav.xtrack_error = 0.2f;
190
        nav.aspd_error = 0.0f;
191 192
        mavlink_msg_nav_controller_output_encode(systemid, MAV_COMP_ID_IMU, &msg, &nav);
        link->sendMAVLinkMessage(&msg);
193 194 195 196 197 198 199

        // 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
200
        pressure.time_usec = 0; // Works also with zero timestamp
201 202
        mavlink_msg_raw_pressure_encode(systemid, MAV_COMP_ID_IMU, &msg, &pressure);
        link->sendMAVLinkMessage(&msg);
203 204 205
    }

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

        // Send actual controller outputs
        // This message just shows the direction
        // and magnitude of the control output
213 214 215 216 217 218
//        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
219 220 221 222

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

        // The message container to be used for sending
223
        //mavlink_message_t ret;
pixhawk's avatar
pixhawk committed
224 225 226 227 228 229 230
        // 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
231
        strcpy((char *)val.name, "FLOAT");
pixhawk's avatar
pixhawk committed
232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258
        // 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
259
        strcpy((char *)valint.name, "INTEGER");
pixhawk's avatar
pixhawk committed
260 261 262 263 264 265 266 267 268 269 270 271 272 273
        // 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);

274 275 276 277 278 279
        timer25Hz = 2;
    }

    timer1Hz--;
    timer10Hz--;
    timer25Hz--;
280 281
}

lm's avatar
lm committed
282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 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 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367
static unsigned chan_counts[MAVLINK_COMM_NUM_BUFFERS];

static const unsigned message_lengths[] = MAVLINK_MESSAGE_LENGTHS;
static unsigned error_count;

static const mavlink_message_info_t message_info[256] = MAVLINK_MESSAGE_INFO;

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)
        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;
        }
}

static void print_field(const mavlink_message_t *msg, const mavlink_field_info_t *f)
{
        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));

                } else {
                        qDebug("[ ");
                        for (i=0; i<f->array_length; i++) {
                                print_one_field(msg, f, i);
                                if (i < f->array_length) {
                                        qDebug(", ");
                                }
                        }
                        qDebug("]");
                }
        }
        qDebug(" ");
}

static void print_message(const mavlink_message_t *msg)
{
        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");
}

368 369
void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg)
{
lm's avatar
lm committed
370 371 372 373 374
    if (msg.sysid != systemid)
    {
        print_message(&msg);
        qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid;
    }
375

376
    switch(msg.msgid) {
377 378
    case MAVLINK_MSG_ID_ATTITUDE:
        break;
379 380 381
    case MAVLINK_MSG_ID_SET_MODE: {
        mavlink_set_mode_t mode;
        mavlink_msg_set_mode_decode(&msg, &mode);
382
        if (systemid == mode.target_system) sys_mode = mode.base_mode;
383 384
    }
    break;
lm's avatar
lm committed
385 386 387 388 389 390 391 392 393 394 395 396 397 398 399 400 401 402 403 404 405 406 407 408 409
    // FIXME MAVLINKV10PORTINGNEEDED
//    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);
//        }
//    }
410
    break;
411 412 413
    case MAVLINK_MSG_ID_SET_LOCAL_POSITION_SETPOINT: {
        mavlink_set_local_position_setpoint_t sp;
        mavlink_msg_set_local_position_setpoint_decode(&msg, &sp);
414
        if (sp.target_system == this->systemid) {
lm's avatar
lm committed
415
            nav_mode = 0;
416 417 418 419 420 421 422 423 424 425 426 427 428 429 430 431 432 433
            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;
434 435
    }
}