void receive(double * y) {
// receive messages
mavlink_message_t msg;
while(_comm->available())
{
uint8_t c = 0;
if (!_comm->read((char*)&c,1)) return;
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&_status))
{
switch(msg.msgid)
{
// this packet seems to me more constrictive so I
// recommend using rc channels scaled instead
case MAVLINK_MSG_ID_HIL_CONTROLS:
{
//std::cout << "receiving hil controls packet" << std::endl;
mavlink_hil_controls_t packet;
mavlink_msg_hil_controls_decode(&msg,&packet);
y[0] = packet.roll_ailerons;
y[1] = packet.pitch_elevator;
y[2] = packet.yaw_rudder;
y[3] = packet.throttle;
y[4] = packet.mode;
y[5] = packet.nav_mode;
y[6] = 0;
y[7] = 0;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving rc channels scaled packet" << std::endl;
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(&msg,&packet);
y[0] = packet.chan1_scaled/1.0e4;
y[1] = packet.chan2_scaled/1.0e4;
y[2] = packet.chan3_scaled/1.0e4;
y[3] = packet.chan4_scaled/1.0e4;
y[4] = packet.chan5_scaled/1.0e4;
y[5] = packet.chan6_scaled/1.0e4;
y[6] = packet.chan7_scaled/1.0e4;
y[7] = packet.chan8_scaled/1.0e4;
break;
}
default:
{
//std::cout << "received message: " << uint32_t(msg.msgid) << std::endl;
}
}
}
}
}
void sci_mavlinkHilState(scicos_block *block, scicos::enumScicosFlags flag)
{
// data
double * u=GetRealInPortPtrs(block,1);
double * y=GetRealOutPortPtrs(block,1);
int * ipar=block->ipar;
static char * device;
static int baudRate;
static char ** stringArray;
static int * intArray;
static int count = 0;
static uint16_t packet_drops = 0;
//handle flags
if (flag==scicos::initialize || flag==scicos::reinitialize)
{
if (mavlink_comm_0_port == NULL)
{
getIpars(1,1,ipar,&stringArray,&intArray);
device = stringArray[0];
baudRate = intArray[0];
try
{
mavlink_comm_0_port = new BufferedAsyncSerial(device,baudRate);
}
catch(const boost::system::system_error & e)
{
Coserror((char *)e.what());
}
}
}
else if (flag==scicos::terminate)
{
if (mavlink_comm_0_port)
{
delete mavlink_comm_0_port;
mavlink_comm_0_port = NULL;
}
}
else if (flag==scicos::updateState)
{
}
else if (flag==scicos::computeDeriv)
{
}
else if (flag==scicos::computeOutput)
{
// channel
mavlink_channel_t chan = MAVLINK_COMM_0;
// loop rates
// TODO: clean this up to use scicos events w/ timers
static int attitudeRate = 50;
static int positionRate = 10;
static int airspeedRate = 1;
// initial times
double scicosTime = get_scicos_time();
static double attitudeTimeStamp = scicosTime;
static double positionTimeStamp = scicosTime;
static double airspeedTimeStamp = scicosTime;
// send airspeed message
if (scicosTime - airspeedTimeStamp > 1.0/airspeedRate)
{
airspeedTimeStamp = scicosTime;
// airspeed (true velocity m/s)
float Vt = u[0];
//double rawPress = 1;
//double airspeed = 1;
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - airspeedTimeStamp < 0)
airspeedTimeStamp = scicosTime;
// send attitude message
if (scicosTime - attitudeTimeStamp > 1.0/attitudeRate)
{
attitudeTimeStamp = scicosTime;
// attitude states (rad)
float roll = u[1];
float pitch = u[2];
float yaw = u[3];
// body rates
float rollRate = u[4];
float pitchRate = u[5];
float yawRate = u[6];
mavlink_msg_attitude_send(chan,attitudeTimeStamp,roll,pitch,yaw,
rollRate,pitchRate,yawRate);
}
else if (scicosTime - attitudeTimeStamp < 0)
attitudeTimeStamp = scicosTime;
// send gps mesage
if (scicosTime - positionTimeStamp > 1.0/positionRate)
{
positionTimeStamp = scicosTime;
// gps
double cog = u[7];
double sog = u[8];
double lat = u[9]*rad2deg;
double lon = u[10]*rad2deg;
double alt = u[11];
//double rawPress = 1;
//double airspeed = 1;
mavlink_msg_gps_raw_send(chan,positionTimeStamp,1,lat,lon,alt,2,10,sog,cog);
//mavlink_msg_raw_pressure_send(chan,timeStamp,airspeed,rawPress,0);
}
else if (scicosTime - positionTimeStamp < 0)
positionTimeStamp = scicosTime;
// receive messages
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(MAVLINK_COMM_0))
{
uint8_t c = comm_receive_ch(MAVLINK_COMM_0);
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_0,c,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving messages" << std::endl;
mavlink_rc_channels_scaled_t rc_channels;
mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels);
y[0] = rc_channels.chan1_scaled/10000.0f;
y[1] = rc_channels.chan2_scaled/10000.0f;
y[2] = rc_channels.chan3_scaled/10000.0f;
y[3] = rc_channels.chan4_scaled/10000.0f;
y[4] = rc_channels.chan5_scaled/10000.0f;
y[5] = rc_channels.chan6_scaled/10000.0f;
y[6] = rc_channels.chan7_scaled/10000.0f;
y[7] = rc_channels.chan8_scaled/10000.0f;
break;
}
}
// update packet drop counter
packet_drops += status.packet_rx_drop_count;
}
}
}
}
void sci_mavlinkHilTracker(scicos_block *block, scicos::enumScicosFlags flag)
{
// data
double * u=GetRealInPortPtrs(block,1);
double * y=GetRealOutPortPtrs(block,1);
int * ipar=block->ipar;
static char * device;
static int baudRate;
static char ** stringArray;
static int * intArray;
static int count = 0;
static uint16_t packet_drops = 0;
//handle flags
if (flag==scicos::initialize || flag==scicos::reinitialize)
{
if (mavlink_comm_2_port == NULL)
{
getIpars(1,1,ipar,&stringArray,&intArray);
device = stringArray[0];
baudRate = intArray[0];
try
{
mavlink_comm_2_port = new BufferedAsyncSerial(device,baudRate);
}
catch(const boost::system::system_error & e)
{
Coserror((char *)e.what());
}
}
}
else if (flag==scicos::terminate)
{
if (mavlink_comm_2_port)
{
delete mavlink_comm_2_port;
mavlink_comm_2_port = NULL;
}
}
else if (flag==scicos::updateState)
{
}
else if (flag==scicos::computeDeriv)
{
}
else if (flag==scicos::computeOutput)
{
// channel
mavlink_channel_t chan = MAVLINK_COMM_2;
// loop rates
// TODO: clean this up to use scicos events w/ timers
static int positionRate = 10;
// initial times
double scicosTime = get_scicos_time();
static double positionTimeStamp = scicosTime;
// send global position
if (scicosTime - positionTimeStamp > 1.0/positionRate)
{
positionTimeStamp = scicosTime;
// gps
double lat = u[0]*rad2deg;
double lon = u[1]*rad2deg;
double alt = u[2];
double vN = u[3];
double vE = u[4];
double vD = u[5];
mavlink_msg_global_position_send(chan,positionTimeStamp,lat,lon,alt,vN,vE,vD);
//std::cout << "sending global position" << std::endl;
}
else if (scicosTime - positionTimeStamp < 0)
positionTimeStamp = scicosTime;
// receive messages
mavlink_message_t msg;
mavlink_status_t status;
while(comm_get_available(MAVLINK_COMM_2))
{
//std::cout << "serial available" << std::endl;
uint8_t c = comm_receive_ch(MAVLINK_COMM_2);
// try to get new message
if(mavlink_parse_char(MAVLINK_COMM_2,c,&msg,&status))
{
switch(msg.msgid)
{
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED:
{
//std::cout << "receiving messages" << std::endl;
mavlink_rc_channels_scaled_t rc_channels;
mavlink_msg_rc_channels_scaled_decode(&msg,&rc_channels);
y[0] = rc_channels.chan1_scaled/10000.0f;
y[1] = rc_channels.chan2_scaled/10000.0f;
y[2] = rc_channels.chan3_scaled/10000.0f;
y[3] = rc_channels.chan4_scaled/10000.0f;
y[4] = rc_channels.chan5_scaled/10000.0f;
y[5] = rc_channels.chan6_scaled/10000.0f;
y[6] = rc_channels.chan7_scaled/10000.0f;
y[7] = rc_channels.chan8_scaled/10000.0f;
break;
}
}
// update packet drop counter
packet_drops += status.packet_rx_drop_count;
}
}
}
}
void mavlink_handleMessage(mavlink_message_t* msg) {
mavlink_message_t msg2;
char sysmsg_str[1024];
switch (msg->msgid) {
case MAVLINK_MSG_ID_HEARTBEAT: {
mavlink_heartbeat_t packet;
mavlink_msg_heartbeat_decode(msg, &packet);
droneType = packet.type;
autoPilot = packet.autopilot;
if (packet.base_mode == MAV_MODE_MANUAL_ARMED) {
ModelData.mode = MODEL_MODE_MANUAL;
} else if (packet.base_mode == 128 + 64 + 16) {
ModelData.mode = MODEL_MODE_RTL;
} else if (packet.base_mode == 128 + 16) {
ModelData.mode = MODEL_MODE_POSHOLD;
} else if (packet.base_mode == 128 + 4) {
ModelData.mode = MODEL_MODE_MISSION;
}
if (packet.system_status == MAV_STATE_ACTIVE) {
ModelData.armed = MODEL_ARMED;
} else {
ModelData.armed = MODEL_DISARMED;
}
// SDL_Log("Heartbeat: %i, %i, %i\n", ModelData.armed, ModelData.mode, ModelData.status);
ModelData.heartbeat = 100;
// sprintf(sysmsg_str, "Heartbeat: %i", (int)time(0));
if ((*msg).sysid != 0xff) {
ModelData.sysid = (*msg).sysid;
ModelData.compid = (*msg).compid;
if (mavlink_maxparam == 0) {
mavlink_start_feeds();
}
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_SCALED: {
mavlink_rc_channels_scaled_t packet;
mavlink_msg_rc_channels_scaled_decode(msg, &packet);
// SDL_Log("Radio: %i,%i,%i\n", packet.chan1_scaled, packet.chan2_scaled, packet.chan3_scaled);
/* if ((int)packet.chan6_scaled > 1000) {
mode = MODE_MISSION;
} else if ((int)packet.chan6_scaled < -1000) {
mode = MODE_MANUEL;
} else {
mode = MODE_POSHOLD;
}
if ((int)packet.chan7_scaled > 1000) {
mode = MODE_RTL;
} else if ((int)packet.chan7_scaled < -1000) {
mode = MODE_SETHOME;
}
*/
ModelData.radio[0] = (int)packet.chan1_scaled / 100.0;
ModelData.radio[1] = (int)packet.chan2_scaled / 100.0;
ModelData.radio[2] = (int)packet.chan3_scaled / 100.0;
ModelData.radio[3] = (int)packet.chan4_scaled / 100.0;
ModelData.radio[4] = (int)packet.chan5_scaled / 100.0;
ModelData.radio[5] = (int)packet.chan6_scaled / 100.0;
ModelData.radio[6] = (int)packet.chan7_scaled / 100.0;
ModelData.radio[7] = (int)packet.chan8_scaled / 100.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_PRESSURE: {
mavlink_scaled_pressure_t packet;
mavlink_msg_scaled_pressure_decode(msg, &packet);
// SDL_Log("BAR;%i;%0.2f;%0.2f;%0.2f\n", time(0), packet.press_abs, packet.press_diff, packet.temperature / 100.0);
// redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_ATTITUDE: {
mavlink_attitude_t packet;
mavlink_msg_attitude_decode(msg, &packet);
ModelData.roll = toDeg(packet.roll);
ModelData.pitch = toDeg(packet.pitch);
if (toDeg(packet.yaw) < 0.0) {
ModelData.yaw = 360.0 + toDeg(packet.yaw);
} else {
ModelData.yaw = toDeg(packet.yaw);
}
mavlink_update_yaw = 1;
// SDL_Log("ATT;%i;%0.2f;%0.2f;%0.2f\n", time(0), toDeg(packet.roll), toDeg(packet.pitch), toDeg(packet.yaw));
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_SCALED_IMU: {
// SDL_Log("SCALED_IMU\n");
break;
}
case MAVLINK_MSG_ID_GPS_RAW_INT: {
mavlink_gps_raw_int_t packet;
mavlink_msg_gps_raw_int_decode(msg, &packet);
if (packet.lat != 0.0) {
GPS_found = 1;
ModelData.p_lat = (float)packet.lat / 10000000.0;
ModelData.p_long = (float)packet.lon / 10000000.0;
ModelData.p_alt = (float)packet.alt / 1000.0;
ModelData.speed = (float)packet.vel / 100.0;
ModelData.numSat = packet.satellites_visible;
ModelData.gpsfix = packet.fix_type;
redraw_flag = 1;
}
break;
}
case MAVLINK_MSG_ID_RC_CHANNELS_RAW: {
// SDL_Log("RC_CHANNELS_RAW\n");
break;
}
case MAVLINK_MSG_ID_SERVO_OUTPUT_RAW: {
// SDL_Log("SERVO_OUTPUT_RAW\n");
break;
}
case MAVLINK_MSG_ID_SYS_STATUS: {
mavlink_sys_status_t packet;
mavlink_msg_sys_status_decode(msg, &packet);
// SDL_Log("%0.1f %%, %0.3f V)\n", packet.load / 10.0, packet.voltage_battery / 1000.0);
ModelData.voltage = packet.voltage_battery / 1000.0;
ModelData.load = packet.load / 10.0;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_STATUSTEXT: {
mavlink_statustext_t packet;
mavlink_msg_statustext_decode(msg, &packet);
SDL_Log("mavlink: ## %s ##\n", packet.text);
sys_message((char *)packet.text);
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_PARAM_VALUE: {
mavlink_param_value_t packet;
mavlink_msg_param_value_decode(msg, &packet);
char var[101];
uint16_t n1 = 0;
uint16_t n2 = 0;
for (n1 = 0; n1 < strlen(packet.param_id); n1++) {
if (packet.param_id[n1] != 9 && packet.param_id[n1] != ' ' && packet.param_id[n1] != '\t') {
var[n2++] = packet.param_id[n1];
}
}
var[n2++] = 0;
// MAV_VAR_FLOAT=0, /* 32 bit float | */
// MAV_VAR_UINT8=1, /* 8 bit unsigned integer | */
// MAV_VAR_INT8=2, /* 8 bit signed integer | */
// MAV_VAR_UINT16=3, /* 16 bit unsigned integer | */
// MAV_VAR_INT16=4, /* 16 bit signed integer | */
// MAV_VAR_UINT32=5, /* 32 bit unsigned integer | */
// MAV_VAR_INT32=6, /* 32 bit signed integer | */
sprintf(sysmsg_str, "PARAM_VALUE (%i/%i): #%s# = %f (Type: %i)", packet.param_index + 1, packet.param_count, var, packet.param_value, packet.param_type);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
mavlink_maxparam = packet.param_count;
mavlink_timeout = 0;
mavlink_set_value(var, packet.param_value, packet.param_type, packet.param_index);
if (packet.param_index + 1 == packet.param_count || packet.param_index % 10 == 0) {
mavlink_param_xml_meta_load();
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_COUNT: {
mavlink_mission_count_t packet;
mavlink_msg_mission_count_decode(msg, &packet);
sprintf(sysmsg_str, "MISSION_COUNT: %i\n", packet.count);
sys_message(sysmsg_str);
mission_max = packet.count;
if (mission_max > 0) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 0);
mavlink_send_message(&msg2);
}
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ACK: {
SDL_Log("mavlink: Mission-Transfer ACK\n");
break;
}
case MAVLINK_MSG_ID_MISSION_REQUEST: {
mavlink_mission_request_t packet;
mavlink_msg_mission_request_decode(msg, &packet);
uint16_t id = packet.seq;
uint16_t id2 = packet.seq;
uint16_t type = 0;
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (id2 > 0) {
id2 = id2 - 1;
} else {
SDL_Log("mavlink: WORKAROUND: first WP == HOME ?\n");
}
}
sprintf(sysmsg_str, "sending Waypoint (%i): %s\n", id, WayPoints[1 + id2].name);
sys_message(sysmsg_str);
if (strcmp(WayPoints[1 + id2].command, "WAYPOINT") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_WAYPOINT\n");
type = MAV_CMD_NAV_WAYPOINT;
} else if (strcmp(WayPoints[1 + id2].command, "RTL") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_RETURN_TO_LAUNCH\n");
type = MAV_CMD_NAV_RETURN_TO_LAUNCH;
} else if (strcmp(WayPoints[1 + id2].command, "LAND") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_LAND\n");
type = MAV_CMD_NAV_LAND;
} else if (strcmp(WayPoints[1 + id2].command, "TAKEOFF") == 0) {
SDL_Log("mavlink: Type: MAV_CMD_NAV_TAKEOFF\n");
type = MAV_CMD_NAV_TAKEOFF;
} else {
SDL_Log("mavlink: Type: UNKNOWN\n");
type = MAV_CMD_NAV_WAYPOINT;
}
sprintf(sysmsg_str, "SENDING MISSION_ITEM: %i: %f, %f, %f\n", id, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
SDL_Log("mavlink: %s\n", sysmsg_str);
mavlink_msg_mission_item_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, id, 0, type, 0.0, 0.0, WayPoints[1 + id2].radius, WayPoints[1 + id2].wait, WayPoints[1 + id2].orbit, WayPoints[1 + id2].yaw, WayPoints[1 + id2].p_lat, WayPoints[1 + id2].p_long, WayPoints[1 + id2].p_alt);
mavlink_send_message(&msg2);
/*
mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z );
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_ITEM: {
mavlink_mission_item_t packet;
mavlink_msg_mission_item_decode(msg, &packet);
sprintf(sysmsg_str, "RECEIVED MISSION_ITEM: %i/%i: %f, %f, %f (%i)\n", packet.seq, mission_max, packet.x, packet.y, packet.z, packet.frame);
SDL_Log("mavlink: %s\n", sysmsg_str);
sys_message(sysmsg_str);
if (packet.seq < mission_max - 1) {
mavlink_msg_mission_request_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, packet.seq + 1);
mavlink_send_message(&msg2);
} else {
mavlink_msg_mission_ack_pack(127, 0, &msg2, ModelData.sysid, ModelData.compid, 15);
mavlink_send_message(&msg2);
}
if (ModelData.teletype == TELETYPE_MEGAPIRATE_NG || ModelData.teletype == TELETYPE_ARDUPILOT) {
if (packet.seq > 0) {
packet.seq = packet.seq - 1;
} else {
SDL_Log("mavlink: WORKAROUND: ignore first WP\n");
break;
}
}
SDL_Log("mavlink: getting WP(%i): %f, %f\n", packet.seq, packet.x, packet.y);
switch (packet.command) {
case MAV_CMD_NAV_WAYPOINT: {
strcpy(WayPoints[1 + packet.seq].command, "WAYPOINT");
break;
}
case MAV_CMD_NAV_LOITER_UNLIM: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_UNLIM");
break;
}
case MAV_CMD_NAV_LOITER_TURNS: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TURNS");
break;
}
case MAV_CMD_NAV_LOITER_TIME: {
strcpy(WayPoints[1 + packet.seq].command, "LOITER_TIME");
break;
}
case MAV_CMD_NAV_RETURN_TO_LAUNCH: {
strcpy(WayPoints[1 + packet.seq].command, "RTL");
break;
}
case MAV_CMD_NAV_LAND: {
strcpy(WayPoints[1 + packet.seq].command, "LAND");
break;
}
case MAV_CMD_NAV_TAKEOFF: {
strcpy(WayPoints[1 + packet.seq].command, "TAKEOFF");
break;
}
default: {
sprintf(WayPoints[1 + packet.seq].command, "CMD:%i", packet.command);
break;
}
}
if (packet.x == 0.0) {
packet.x = 0.00001;
}
if (packet.y == 0.0) {
packet.y = 0.00001;
}
if (packet.z == 0.0) {
packet.z = 0.00001;
}
WayPoints[1 + packet.seq].p_lat = packet.x;
WayPoints[1 + packet.seq].p_long = packet.y;
WayPoints[1 + packet.seq].p_alt = packet.z;
WayPoints[1 + packet.seq].yaw = packet.param4;
sprintf(WayPoints[1 + packet.seq].name, "WP%i", packet.seq + 1);
WayPoints[1 + packet.seq + 1].p_lat = 0.0;
WayPoints[1 + packet.seq + 1].p_long = 0.0;
WayPoints[1 + packet.seq + 1].p_alt = 0.0;
WayPoints[1 + packet.seq + 1].yaw = 0.0;
WayPoints[1 + packet.seq + 1].name[0] = 0;
WayPoints[1 + packet.seq + 1].command[0] = 0;
/*
float param1; ///< PARAM1 / For NAV command MISSIONs: Radius in which the MISSION is accepted as reached, in meters
float param2; ///< PARAM2 / For NAV command MISSIONs: Time that the MAV should stay inside the PARAM1 radius before advancing, in milliseconds
float param3; ///< PARAM3 / For LOITER command MISSIONs: Orbit to circle around the MISSION, in meters. If positive the orbit direction should be clockwise, if negative the orbit direction should be counter-clockwise.
float param4; ///< PARAM4 / For NAV and LOITER command MISSIONs: Yaw orientation in degrees, [0..360] 0 = NORTH
float x; ///< PARAM5 / local: x position, global: latitude
float y; ///< PARAM6 / y position: global: longitude
float z; ///< PARAM7 / z position: global: altitude
uint16_t seq; ///< Sequence
uint16_t command; ///< The scheduled action for the MISSION. see MAV_CMD in common.xml MAVLink specs
uint8_t target_system; ///< System ID
uint8_t target_component; ///< Component ID
uint8_t frame; ///< The coordinate system of the MISSION. see MAV_FRAME in mavlink_types.h
uint8_t current; ///< false:0, true:1
uint8_t autocontinue; ///< autocontinue to next wp
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="0" name="MAV_FRAME_GLOBAL">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="1" name="MAV_FRAME_LOCAL_NED">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="2" name="MAV_FRAME_MISSION">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="3" name="MAV_FRAME_GLOBAL_RELATIVE_ALT">
GCS_MAVLink/message_definitions_v1.0/common.xml: <entry value="4" name="MAV_FRAME_LOCAL_ENU">
*/
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_MISSION_CURRENT: {
mavlink_mission_current_t packet;
mavlink_msg_mission_current_decode(msg, &packet);
// SDL_Log("mavlink: ## Active_WP %f ##\n", packet.seq);
uav_active_waypoint = (uint8_t)packet.seq;
break;
}
case MAVLINK_MSG_ID_RAW_IMU: {
mavlink_raw_imu_t packet;
mavlink_msg_raw_imu_decode(msg, &packet);
/*
SDL_Log("## IMU_RAW_ACC_X %i ##\n", packet.xacc);
SDL_Log("## IMU_RAW_ACC_Y %i ##\n", packet.yacc);
SDL_Log("## IMU_RAW_ACC_Z %i ##\n", packet.zacc);
SDL_Log("## IMU_RAW_GYRO_X %i ##\n", packet.xgyro);
SDL_Log("## IMU_RAW_GYRO_Y %i ##\n", packet.ygyro);
SDL_Log("## IMU_RAW_GYRO_Z %i ##\n", packet.zgyro);
SDL_Log("## IMU_RAW_MAG_X %i ##\n", packet.xmag);
SDL_Log("## IMU_RAW_MAG_Y %i ##\n", packet.ymag);
SDL_Log("## IMU_RAW_MAG_Z %i ##\n", packet.zmag);
*/
ModelData.acc_x = (float)packet.xacc / 1000.0;
ModelData.acc_y = (float)packet.yacc / 1000.0;
ModelData.acc_z = (float)packet.zacc / 1000.0;
ModelData.gyro_x = (float)packet.zgyro;
ModelData.gyro_y = (float)packet.zgyro;
ModelData.gyro_z = (float)packet.zgyro;
redraw_flag = 1;
break;
}
case MAVLINK_MSG_ID_NAV_CONTROLLER_OUTPUT: {
mavlink_nav_controller_output_t packet;
mavlink_msg_nav_controller_output_decode(msg, &packet);
/*
nav_roll
nav_pitch
alt_error
aspd_error
xtrack_error
nav_bearing
target_bearing
wp_dist
*/
break;
}
case MAVLINK_MSG_ID_VFR_HUD: {
mavlink_vfr_hud_t packet;
mavlink_msg_vfr_hud_decode(msg, &packet);
// SDL_Log("## pa %f ##\n", packet.airspeed);
// SDL_Log("## pg %f ##\n", packet.groundspeed);
// SDL_Log("## palt %f ##\n", packet.alt);
if (GPS_found == 0) {
ModelData.p_alt = packet.alt;
}
// SDL_Log("## pc %f ##\n", packet.climb);
// SDL_Log("## ph %i ##\n", packet.heading);
// SDL_Log("## pt %i ##\n", packet.throttle);
break;
}
case MAVLINK_MSG_ID_RADIO: {
mavlink_radio_t packet;
mavlink_msg_radio_decode(msg, &packet);
SDL_Log("mavlink: ## rxerrors %i ##\n", packet.rxerrors);
SDL_Log("mavlink: ## fixed %i ##\n", packet.fixed);
SDL_Log("mavlink: ## rssi %i ##\n", packet.rssi);
SDL_Log("mavlink: ## remrssi %i ##\n", packet.remrssi);
SDL_Log("mavlink: ## txbuf %i ##\n", packet.txbuf);
SDL_Log("mavlink: ## noise %i ##\n", packet.noise);
SDL_Log("mavlink: ## remnoise %i ##\n", packet.remnoise);
break;
}
default: {
// SDL_Log(" ## MSG_ID == %i ##\n", msg->msgid);
break;
}
}
}
