void MavLinkSendMissionRequest(uint8_t currentMissionIndex)
{
mavlink_message_t msg;
mavlink_msg_mission_request_pack(mavlink_system.sysid, mavlink_system.compid, &msg,
groundStationSystemId, groundStationComponentId, currentMissionIndex);
len = mavlink_msg_to_send_buffer(buf, &msg);
uart1EnqueueData(buf, (uint8_t)len);
}
// function to call at the beginning of a mission item write transaction (i.e. after receiving MISSION_COUNT)
COMM_MAV_RESULT navigation_rx_mission_items_start(COMM_MAV_MSG_TARGET *target, mavlink_message_t *msg, mavlink_message_t *answer_msg)
{
mission_item_rx_count = mavlink_msg_mission_count_get_count(msg);
/* send mission acknowledge */
mavlink_msg_mission_request_pack(mavlink_system.sysid, target->component, answer_msg,
msg->sysid, target->component, 0);
return REPLY_TO_SENDER;
}
COMM_MAV_RESULT navigation_rx_mission_item(COMM_MAV_MSG_TARGET *target, mavlink_message_t *msg, mavlink_message_t *answer_msg)
{
uint16_t next_index = mavlink_msg_mission_item_get_seq(msg);
if(next_index < N_TARGETS_MAX)
{ // buffer has space --> add to list
mavlink_msg_mission_item_decode(msg, &(mission_items.item[next_index]));
if(mavlink_msg_mission_item_get_current(msg))
{
mission_items.current_index = next_index;
}
if(mission_item_rx_count > next_index + 1) // request next item
{
mavlink_msg_mission_request_pack(mavlink_system.sysid, target->component, answer_msg,
msg->sysid, target->component, next_index + 1);
}
else // transfer done
{
mavlink_msg_mission_ack_pack(mavlink_system.sysid, target->component, answer_msg,
msg->sysid, target->component, MAV_MISSION_ACCEPTED);
if(mission_item_rx_count == 0)
{// If a waypoint planner component receives MISSION_ITEM messages outside of transactions it answers with a MISSION_ACK message.
if(mission_items.count == next_index + 1)
mission_items.count++; //an item was appended to the end of the list list
}
else
{
mission_items.count = next_index + 1;
mission_item_rx_count = 0;
}
#ifdef FLASH_ENABLED
log_write_mavlink_item_list(false, &pos_counter);
#endif
}
return REPLY_TO_SENDER;
}
else
{ //buffer overflow...
//TODO: reply with MAV_CMD_ACK_ERR_FAIL because buffer is full.
return NO_ANSWER; //for now we just don't answer -->timeout.
}
}
void Create_packets(ExtU_ANN_EKF_NMPC_2_T *data,int sock)
{ int i=0;
int bytes_sent;
uint8_t buf[BUFFER_LENGTH];
uint8_t buf1[BUFFER_LENGTH];
uint8_t buf2[BUFFER_LENGTH];
mavlink_message_t msg,msg1,msg2;
uint16_t len;
mavlink_msg_heartbeat_pack(2, 200, &msg, 2, 12, 65, 0, 3);
len = mavlink_msg_to_send_buffer(buf, &msg);
int len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
mavlink_msg_attitude_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAnglesmeas.phi, data->EulerAnglesmeas.theta, data->EulerAnglesmeas.psi, data->BodyRatesmeas.P,data->BodyRatesmeas.Q, data->BodyRatesmeas.R);
len = mavlink_msg_to_send_buffer(buf, &msg);
len1 = strlen(buf);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
// mavlink_msg_hil_state_pack(1, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,data->GPSPosition.Latitude,data->GPSPosition.Longitude,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
// mavlink_msg_hil_state_pack(2, 200, &msg, microsSinceEpoch(),data->EulerAngles.phi, data->EulerAngles.theta, data->EulerAngles.psi, data->BodyRatesmeas.P, data->BodyRatesmeas.Q, data->BodyRatesmeas.R,39,-95,data->GPSPosition.Altitude,0,0,0,data->Accelerometermeas.Axb,data->Accelerometermeas.Ayb,data->Accelerometermeas.Azb);
//len = mavlink_msg_to_send_buffer(buf, &msg);
// bytes_sent = write(tty_fd1,buf,len);
mavlink_msg_gps_raw_int_pack(2, 200, &msg, microsSinceEpoch(),2,data->GPSPositionmeas.Latitude,data->GPSPositionmeas.Longitude,data->GPSPositionmeas.Altitude,UINT16_MAX,UINT16_MAX,UINT16_MAX,UINT16_MAX,255);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf, 0, BUFFER_LENGTH);
//mavlink_message_t msg;
mavlink_status_t status,status1;
mavlink_mission_count_t mission_count;
int cn;
recsize = recvfrom(sock, (void *)buf, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen);
if (recsize > 0)
{
// Something received - print out all bytes and parse packet
printf("Bytes Received: %d\nDatagram: ", (int)recsize);
for (i = 0; i < recsize; ++i)
{
//temp = buf[i];
// printf("%02x ", (unsigned char)temp);
if (mavlink_parse_char(MAVLINK_COMM_0, buf[i], &msg, &status))
{
// Packet received
printf("\nReceived packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid);
printf("\n\n Incomming packet\n\n");
if(msg.msgid == 44)
{
mavlink_msg_mission_count_decode( &msg, &mission_count);
printf("\n\n the # of wypts received is %d########\n\n",mission_count.count);
data->wcn=mission_count.count;
for(cn=1;cn<=data->wcn;cn++)
{
//sendrequest(sock,cn);
memset(buf1, 0, BUFFER_LENGTH);
mavlink_msg_mission_request_pack(2, 200, &msg1,255,0,(cn-1));
len = mavlink_msg_to_send_buffer(buf1, &msg1);
bytes_sent = sendto(sock, buf1, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
//printf("\n\n @@@@@ sent successfull\n\n");
memset(buf2, 0, BUFFER_LENGTH);
//receive_waypoints(sock,cn);
while( (recsize1 = recvfrom(sock, (void *)buf2, BUFFER_LENGTH, 0, (struct sockaddr *)&gcAddr, &fromlen)) <= 0);
if (recsize1 > 0)
{
int ii;
mavlink_mission_item_t mission_item;
for (ii = 0; ii < recsize1; ++ii)
{
if (mavlink_parse_char(MAVLINK_COMM_0, buf2[ii], &msg2, &status1))
{
printf("\n\n reading waypoint # %d",(cn-1));
}
}//if (msg2.msgid==0)
//goto loop;
if(msg2.msgid == 39)
{
mavlink_msg_mission_item_decode(&msg2, &mission_item);
printf("\n\nthe waypoint # %d is lat:%f lon:%f alt:%f",cn,mission_item.x,mission_item.y,mission_item.z);
data->lat[cn]=mission_item.x;
data->alt[cn] =mission_item.z;
data->lon[cn] = mission_item.y;
data->WaypointsIN.v[cn] = mission_item.param1;
latlon(&ANN_EKF_NMPC_2_U,cn);
}
}else printf("!!!!!!!!\n\n receiving failed \n\n ");
}
if((cn-1) == mission_count.count)
{
//sendack(sock,cn);
mavlink_msg_mission_ack_pack(2, 200, &msg2,255,0,0);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n ***** sent acknowledgement *****\n\n");
}
} // for msg id 44
if(msg.msgid == 43)
{
int j;
//sendcn(sock,cn);
mavlink_msg_mission_count_pack(2, 200, &msg2,255,0,data->wcn);
memset(buf2, 0, BUFFER_LENGTH);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
memset(buf2, 0, BUFFER_LENGTH);
for(j=1; j<=data->wcn;j++)
{
//receive_request(sock);
//sendwyp(sock,-95,95);
mavlink_msg_mission_item_pack(2, 200, &msg2,255,0,(j-1),0,16,0,1,data->WaypointsIN.v[j],0,0,0,data->lat[j],data->lon[j],data->alt[j]);
len = mavlink_msg_to_send_buffer(buf2, &msg2);
bytes_sent = sendto(sock, buf2, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
printf("\n\n @@@@@ sent count successfull\n\n");
}
//rec_ack(sock);
}// for msg id 43
} //parse - if
}//for
}//else printf("\n\n 12121212not good");//recsize - if
// printf("\n");
//memset(buf, 0, BUFFER_LENGTH);
//printf("\n\n !!!@@##$$ end of a loop\n\n");
}//for Createpacket
/**
* @brief Encode a mission_request struct
*
* @param system_id ID of this system
* @param component_id ID of this component (e.g. 200 for IMU)
* @param msg The MAVLink message to compress the data into
* @param mission_request C-struct to read the message contents from
*/
uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request)
{
return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq);
}
uint16_t MissionInterfaceResponse(uint8_t system_id, uint8_t component_id){
mavlink_message_t msg;
uint16_t bytes2Send = 0;
uint8_t CopyMsgToBuff = 0;
char vr_message[50];
// if (mlPending.wpProtState == WP_PROT_TX_WP) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "%d: y =%2.2f x =%2.2f z =%2.2f o =%d t =%d", mlPending.wpCurrentWpInTransaction, (double)mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
// (double)mlWpValues.alt[mlPending.wpCurrentWpInTransaction],
// mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
// mlWpValues.type[mlPending.wpCurrentWpInTransaction]);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
// }
//if (mlPending.wpProtState == WP_PROT_GETTING_WP_IDLE) { //SLUGS Nav DBG info
// memset(vr_message, 0, sizeof (vr_message));
// sprintf(vr_message, "com = %d, tb =%2.2f ta = %2.2f", sw_intTemp, (double)fl_temp1, (double)fl_temp2);
// bytes2Send += sendQGCDebugMessage(vr_message, 0, UartOutBuff, bytes2Send + 1);
//}
// Current mission item (1 off indexing issue in qgc vs et)
if (mlPending.wpSendCurrent) { //--DBG TODO AM implement with nav implamntation
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_current_pack(system_id,
component_id,
&msg, ((uint16_t)mlNavigation.toWP) - 1);
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
mlPending.wpSendCurrent = FALSE;
}
// clear the msg
memset(&msg, 0, sizeof (mavlink_message_t));
//if (mlPending.wpTransaction){ //DBG
CopyMsgToBuff = 1; //Set send msg flag - if nothing to send the switch statement will reset this
switch (mlPending.wpProtState) {
case WP_PROT_LIST_REQUESTED:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Got mission list request. Sending count...", 49);
mavlink_msg_mission_count_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlWpValues.wpCount);
// Change the state machine state
mlPending.wpProtState = WP_PROT_NUM_SENT;
// Reset the timeout
mlPending.wpTimeOut = 0;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
break;
case WP_PROT_GETTING_WP_IDLE:
if (mlPending.wpCurrentWpInTransaction < mlPending.wpTotalWps) {
mavlink_msg_mission_request_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction++);
// Change the state machine state
mlPending.wpProtState = WP_PROT_RX_WP;
} else {
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
MAV_MISSION_ACCEPTED); // 0 is success
// Update the waypoint count
mlWpValues.wpCount = mlPending.wpTotalWps;
// End the transaction
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTotalWps = 0;
mlPending.wpSendCurrent = TRUE; // send current waypoint index
// put zeros in the rest of the waypoints;
//clearWaypointsFrom(mlWpValues.wpCount); //this clears the EEPROM WP storage
}
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
case WP_PROT_TX_WP:
//mlPending.statustext++;
//mlStatustext.severity = MAV_SEVERITY_INFO;
//strncpy(mlStatustext.text, "Sending waypoint.", 25);
// Send WP
mavlink_msg_mission_item_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
mlPending.wpCurrentWpInTransaction,
MAV_FRAME_GLOBAL,
mlWpValues.type[mlPending.wpCurrentWpInTransaction],
0, // not current
1, // autocontinue
0.0, // Param 1 not used
0.0, // Param 2 not used
(float) mlWpValues.orbit[mlPending.wpCurrentWpInTransaction],
0.0, // Param 4 not used
mlWpValues.lat[mlPending.wpCurrentWpInTransaction],
mlWpValues.lon[mlPending.wpCurrentWpInTransaction],
mlWpValues.alt[mlPending.wpCurrentWpInTransaction]); // always autocontinue
// Switch the state waiting for the next request
// Change the state machine state
mlPending.wpProtState = WP_PROT_SENDING_WP_IDLE;
bytes2Send += mavlink_msg_to_send_buffer(UartOutBuff, &msg);
// Reset the timeout
mlPending.wpTimeOut = 0;
break;
default:
CopyMsgToBuff = 0;
break;
} // switch wpProtState
// if (CopyMsgToBuff) // Copy the message to the send buffer
// bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
//}//if DBG
mlPending.wpTimeOut++;
// if Timed out reset the state machine and send an error
if (mlPending.wpTimeOut > PROTOCOL_TIMEOUT_TICKS) {
memset(&msg, 0, sizeof (mavlink_message_t));
mavlink_msg_mission_ack_pack(system_id,
MAV_COMP_ID_MISSIONPLANNER,
&msg,
GS_SYSTEMID,
GS_COMPID,
1); // 1 is failure
// Copy the message to the send buffer
bytes2Send += mavlink_msg_to_send_buffer((UartOutBuff + 1 + bytes2Send), &msg);
// reset the state machine
mlPending.wpTransaction = 0;
mlPending.wpProtState = WP_PROT_IDLE;
mlPending.wpCurrentWpInTransaction = 0;
mlPending.wpTimeOut = 0;
mlPending.wpTotalWps = 0;
}
return(bytes2Send);
}
static void calc_home(struct timer *t, void *d)
{
mavlink_heartbeat_t *hb = mavdata_get(MAVLINK_MSG_ID_HEARTBEAT);
mavlink_global_position_int_t *gpi;
mavlink_message_t this_msg;
switch (home.lock) {
case HOME_NONE:
default:
if (mavdata_age(MAVLINK_MSG_ID_HEARTBEAT) > 5000)
return;
/* check arming status */
if (hb->base_mode & MAV_MODE_FLAG_SAFETY_ARMED)
home.lock = HOME_WAIT;
break;
case HOME_WAIT:
if (mavdata_age(MAVLINK_MSG_ID_MISSION_ITEM) > 2000) {
/* when UAV is armed, home is WP0 */
mavlink_msg_mission_request_pack(config.mav.osd_sysid, MAV_COMP_ID_OSD, &this_msg,
config.mav.uav_sysid, MAV_COMP_ID_ALL, 0);
mavlink_send_msg(&this_msg);
} else {
mavlink_mission_item_t *mi = mavdata_get(MAVLINK_MSG_ID_MISSION_ITEM);
priv.home_coord.lat = DEG2RAD(mi->x);
priv.home_coord.lon = DEG2RAD(mi->y);
priv.home_altitude = (unsigned int) mi->z;
home.lock = HOME_GOT;
}
break;
case HOME_GOT:
home.lock = HOME_LOCKED;
set_timer_period(t, 200);
break;
case HOME_LOCKED:
{
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.uav_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.uav_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.altitude = (unsigned int) (gpi->alt / 1000);
priv.heading = (int) (gpi->hdg / 100);
home.uav_bearing = (int) get_bearing(&priv.home_coord, &priv.uav_coord);
home.direction = home.uav_bearing + 180;
home.direction -= priv.heading;
if (home.direction < 0)
home.direction += 360;
home.distance = earth_distance(&priv.home_coord, &priv.uav_coord);
home.altitude = priv.altitude - priv.home_altitude;
break;
case HOME_RESET:
home.lock = HOME_NONE;
set_timer_period(t, 1000);
break;
case HOME_FORCE:
gpi = mavdata_get(MAVLINK_MSG_ID_GLOBAL_POSITION_INT);
priv.home_coord.lat = DEG2RAD(gpi->lat / 10000000.0);
priv.home_coord.lon = DEG2RAD(gpi->lon / 10000000.0);
priv.home_altitude = (unsigned int) (gpi->alt / 1000);
home.lock = HOME_GOT;
break;
}
}
}
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;
}
}
}
void mission_write_waypoint_list(void)
{
uint32_t start_time, cur_time;
waypoint_t *cur_wp = NULL;
waypoint_t *new_waypoint;
/* Getting the waypoint count */
int new_waypoint_list_count = mavlink_msg_mission_count_get_count(&received_msg);
waypoint_info.is_busy = true;
int i;
for(i = 0; i < new_waypoint_list_count; i++) {
/* Generate the mission_request message */
mavlink_msg_mission_request_pack(
1, 0, &msg, 255, 0, i /* waypoint index */
);
send_package(&msg);
/* Create a new node of waypoint */
if (waypoint_info.waypoint_count > i) {
new_waypoint = get_waypoint(waypoint_info.waypoint_list, i);
} else {
/* Create a new node of waypoint */
new_waypoint = create_waypoint_node();
}
start_time = get_system_time_ms();
/* Waiting for new message */
while(received_msg.msgid != 39) {
cur_time = get_system_time_ms();
//Suspend the task to read the new message
vTaskDelay(100);
/* Time out, leave */
if((cur_time - start_time) > TIMEOUT_CNT) {
return;
}
}
/* Get the waypoint message */
mavlink_msg_mission_item_decode(&received_msg, &(new_waypoint->data));
/* Clear the received message */
clear_message_id(&received_msg);
/* insert the new waypoint at the end of the list */
if(i == 0) {
//First node of the list
waypoint_info.waypoint_list = cur_wp = new_waypoint;
} else {
cur_wp->next = new_waypoint;
cur_wp = cur_wp->next;
}
}
/* Update the wayppoint, navigation manager */
waypoint_info.waypoint_count = new_waypoint_list_count;
waypoint_info.is_busy = false;
nav_waypoint_list_is_updated = false; /* From navigation point of view */
/* Send a mission ack Message at the end */
mavlink_msg_mission_ack_pack(1, 0, &msg, 255, 0, 0);
send_package(&msg);
}