#include #include #include "MAVLinkSimulationMAV.h" MAVLinkSimulationMAV::MAVLinkSimulationMAV(MAVLinkSimulationLink *parent, int systemid) : QObject(parent), link(parent), planner(parent, systemid), systemid(systemid), timer25Hz(0), timer10Hz(0), timer1Hz(0), latitude(47.376389), longitude(8.548056), altitude(0.0), x(8.548056), y(47.376389), z(550), roll(0.0), pitch(0.0), yaw(0.0), globalNavigation(true), firstWP(false), previousSPX(8.548056), previousSPY(47.376389), previousSPZ(550), previousSPYaw(0.0), nextSPX(8.548056), nextSPY(47.376389), nextSPZ(550), nextSPYaw(0.0) { // Please note: The waypoint planner is running connect(&mainloopTimer, SIGNAL(timeout()), this, SLOT(mainloop())); connect(&planner, SIGNAL(messageSent(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); connect(link, SIGNAL(messageReceived(mavlink_message_t)), this, SLOT(handleMessage(mavlink_message_t))); mainloopTimer.start(20); mainloop(); } void MAVLinkSimulationMAV::mainloop() { // Calculate new simulator values // double maxSpeed = 0.0001; // rad/s in earth coordinate frame // double xNew = // (nextSPX - previousSPX) // 1 Hz execution if (timer1Hz <= 0) { mavlink_message_t msg; mavlink_msg_heartbeat_pack(systemid, MAV_COMP_ID_IMU, &msg, MAV_FIXED_WING, MAV_AUTOPILOT_PIXHAWK); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); timer1Hz = 50; } // 10 Hz execution if (timer10Hz <= 0) { if (!firstWP) { double radPer100ms = 0.0002; double altPer100ms = 0.1; double xm = (nextSPX - x); double ym = (nextSPY - y); double zm = (nextSPZ - z); float zsign = (zm < 0) ? -1.0f : 1.0f; //float trueyaw = atan2f(xm, ym); yaw = yaw*0.9 + 0.1*atan2f(xm, ym); qDebug() << "SIMULATION MAV: x:" << xm << "y:" << ym << "z:" << zm << "yaw:" << yaw; if (sqrt(xm*xm+ym*ym) > 0.0001) { 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"; } mavlink_message_t msg; mavlink_global_position_int_t pos; pos.alt = z*1000.0; pos.lat = y*1E7; pos.lon = x*1E7; pos.vx = 0; pos.vy = 0; pos.vz = 0; mavlink_msg_global_position_int_encode(systemid, MAV_COMP_ID_IMU, &msg, &pos); link->sendMAVLinkMessage(&msg); planner.handleMessage(msg); mavlink_attitude_t attitude; attitude.roll = 0.0f; attitude.pitch = 0.0f; attitude.yaw = yaw; qDebug() << "YAW" << yaw; mavlink_msg_attitude_encode(systemid, MAV_COMP_ID_IMU, &msg, &attitude); link->sendMAVLinkMessage(&msg); timer10Hz = 5; } // 25 Hz execution if (timer25Hz <= 0) { timer25Hz = 2; } timer1Hz--; timer10Hz--; timer25Hz--; } void MAVLinkSimulationMAV::handleMessage(const mavlink_message_t& msg) { //qDebug() << "MAV:" << systemid << "RECEIVED MESSAGE FROM" << msg.sysid << "COMP" << msg.compid; switch(msg.msgid) { case MAVLINK_MSG_ID_ATTITUDE: 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) { 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; } }