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