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
void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { // If we are currently operating as a proxy for a remote, // alas we have to look inside each packet to see if its for us or for the remote case MAVLINK_MSG_ID_REQUEST_DATA_STREAM: { handle_request_data_stream(msg, true); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { handle_param_request_list(msg); break; } case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { handle_param_request_read(msg); break; } case MAVLINK_MSG_ID_PARAM_SET: { handle_param_set(msg, NULL); break; } case MAVLINK_MSG_ID_HEARTBEAT: break; case MAVLINK_MSG_ID_COMMAND_LONG: { // decode mavlink_command_long_t packet; mavlink_msg_command_long_decode(msg, &packet); uint8_t result = MAV_RESULT_UNSUPPORTED; // do command send_text_P(SEVERITY_LOW,PSTR("command received: ")); switch(packet.command) { case MAV_CMD_PREFLIGHT_CALIBRATION: { if (is_equal(packet.param1,1.0f)) { tracker.ins.init_gyro(); if (tracker.ins.gyro_calibrated_ok_all()) { tracker.ahrs.reset_gyro_drift(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } if (is_equal(packet.param3,1.0f)) { tracker.init_barometer(); // zero the altitude difference on next baro update tracker.nav_status.need_altitude_calibration = true; } if (is_equal(packet.param4,1.0f)) { // Cant trim radio } else if (is_equal(packet.param5,1.0f)) { float trim_roll, trim_pitch; AP_InertialSensor_UserInteract_MAVLink interact(this); if(tracker.ins.calibrate_accel(&interact, trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); } } else if (is_equal(packet.param5,2.0f)) { // accel trim float trim_roll, trim_pitch; if(tracker.ins.calibrate_trim(trim_roll, trim_pitch)) { // reset ahrs's trim to suggested values from calibration routine tracker.ahrs.set_trim(Vector3f(trim_roll, trim_pitch, 0)); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } result = MAV_RESULT_ACCEPTED; break; } case MAV_CMD_COMPONENT_ARM_DISARM: if (packet.target_component == MAV_COMP_ID_SYSTEM_CONTROL) { if (is_equal(packet.param1,1.0f)) { tracker.arm_servos(); result = MAV_RESULT_ACCEPTED; } else if (is_zero(packet.param1)) { tracker.disarm_servos(); result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_UNSUPPORTED; } } else { result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_MODE: switch ((uint16_t)packet.param1) { case MAV_MODE_MANUAL_ARMED: case MAV_MODE_MANUAL_DISARMED: tracker.set_mode(MANUAL); result = MAV_RESULT_ACCEPTED; break; case MAV_MODE_AUTO_ARMED: case MAV_MODE_AUTO_DISARMED: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; default: result = MAV_RESULT_UNSUPPORTED; } break; case MAV_CMD_DO_SET_SERVO: if (tracker.servo_test_set_servo(packet.param1, packet.param2)) { result = MAV_RESULT_ACCEPTED; } break; // mavproxy/mavutil sends this when auto command is entered case MAV_CMD_MISSION_START: tracker.set_mode(AUTO); result = MAV_RESULT_ACCEPTED; break; case MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if (is_equal(packet.param1,1.0f) || is_equal(packet.param1,3.0f)) { // when packet.param1 == 3 we reboot to hold in bootloader hal.scheduler->reboot(is_equal(packet.param1,3.0f)); result = MAV_RESULT_ACCEPTED; } break; } case MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES: { if (is_equal(packet.param1,1.0f)) { tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); result = MAV_RESULT_ACCEPTED; } break; } default: break; } mavlink_msg_command_ack_send( chan, packet.command, result); break; } // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); if (packet.start_index == 0) { // New home at wp index 0. Ask for it waypoint_receiving = true; waypoint_request_i = 0; waypoint_request_last = 0; send_message(MSG_NEXT_WAYPOINT); waypoint_receiving = true; } break; } // XXX receive a WP from GCS and store in EEPROM if it is HOME case MAVLINK_MSG_ID_MISSION_ITEM: { // decode mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); struct Location tell_command = {}; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } default: result = MAV_MISSION_UNSUPPORTED_FRAME; break; } if (result != MAV_MISSION_ACCEPTED) goto mission_failed; // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_failed; } // check if this is the HOME wp if (packet.seq == 0) { tracker.set_home(tell_command); // New home in EEPROM send_text_P(SEVERITY_LOW,PSTR("new HOME received")); waypoint_receiving = false; } mission_failed: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, result); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); tracker.tracking_manual_control(packet); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracker.tracking_update_pressure(packet); break; } case MAVLINK_MSG_ID_SET_MODE: { handle_set_mode(msg, FUNCTOR_BIND(&tracker, &Tracker::mavlink_set_mode, bool, uint8_t)); break; } #if HAL_CPU_CLASS > HAL_CPU_CLASS_16 case MAVLINK_MSG_ID_SERIAL_CONTROL: handle_serial_control(msg, tracker.gps); break; case MAVLINK_MSG_ID_GPS_INJECT_DATA: handle_gps_inject(msg, tracker.gps); break; #endif case MAVLINK_MSG_ID_AUTOPILOT_VERSION_REQUEST: tracker.gcs[chan-MAVLINK_COMM_0].send_autopilot_version(); break; } // end switch } // end handle mavlink
void GCS_MAVLINK_Tracker::handleMessage(mavlink_message_t* msg) { switch (msg->msgid) { case MAVLINK_MSG_ID_HEARTBEAT: break; // When mavproxy 'wp sethome' case MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST: { // decode mavlink_mission_write_partial_list_t packet; mavlink_msg_mission_write_partial_list_decode(msg, &packet); if (packet.start_index == 0) { // New home at wp index 0. Ask for it waypoint_receiving = true; waypoint_request_i = 0; waypoint_request_last = 0; send_message(MSG_NEXT_WAYPOINT); } break; } // XXX receive a WP from GCS and store in EEPROM if it is HOME case MAVLINK_MSG_ID_MISSION_ITEM: { // decode mavlink_mission_item_t packet; MAV_MISSION_RESULT result = MAV_MISSION_ACCEPTED; mavlink_msg_mission_item_decode(msg, &packet); struct Location tell_command = {}; switch (packet.frame) { case MAV_FRAME_MISSION: case MAV_FRAME_GLOBAL: { tell_command.lat = 1.0e7f*packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f*packet.y; // in as DD converted to * t7 tell_command.alt = packet.z*1.0e2f; // in as m converted to cm tell_command.options = 0; // absolute altitude break; } #ifdef MAV_FRAME_LOCAL_NED case MAV_FRAME_LOCAL_NED: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = -packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif #ifdef MAV_FRAME_LOCAL case MAV_FRAME_LOCAL: // local (relative to home position) { tell_command.lat = 1.0e7f*ToDeg(packet.x/ (RADIUS_OF_EARTH*cosf(ToRad(home.lat/1.0e7f)))) + home.lat; tell_command.lng = 1.0e7f*ToDeg(packet.y/RADIUS_OF_EARTH) + home.lng; tell_command.alt = packet.z*1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; break; } #endif case MAV_FRAME_GLOBAL_RELATIVE_ALT: // absolute lat/lng, relative altitude { tell_command.lat = 1.0e7f * packet.x; // in as DD converted to * t7 tell_command.lng = 1.0e7f * packet.y; // in as DD converted to * t7 tell_command.alt = packet.z * 1.0e2f; tell_command.options = MASK_OPTIONS_RELATIVE_ALT; // store altitude relative!! Always!! break; } default: result = MAV_MISSION_UNSUPPORTED_FRAME; break; } if (result != MAV_MISSION_ACCEPTED) goto mission_failed; // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_failed; } // check if this is the HOME wp if (packet.seq == 0) { tracker.set_home(tell_command); // New home in EEPROM send_text(MAV_SEVERITY_INFO,"New HOME received"); waypoint_receiving = false; } mission_failed: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send( chan, msg->sysid, msg->compid, result, MAV_MISSION_TYPE_MISSION); break; } case MAVLINK_MSG_ID_MANUAL_CONTROL: { mavlink_manual_control_t packet; mavlink_msg_manual_control_decode(msg, &packet); tracker.tracking_manual_control(packet); break; } case MAVLINK_MSG_ID_GLOBAL_POSITION_INT: { // decode mavlink_global_position_int_t packet; mavlink_msg_global_position_int_decode(msg, &packet); tracker.tracking_update_position(packet); break; } case MAVLINK_MSG_ID_SCALED_PRESSURE: { // decode mavlink_scaled_pressure_t packet; mavlink_msg_scaled_pressure_decode(msg, &packet); tracker.tracking_update_pressure(packet); break; } default: handle_common_message(msg); break; } // end switch } // end handle mavlink
void MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); if (CHECK_SYSID_COMPID_MISSION(wp)) { if (_state == MAVLINK_WPM_STATE_GETLIST) { _time_last_recv = hrt_absolute_time(); if (wp.seq != _transfer_seq) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u was not the expected %u", wp.seq, _transfer_seq); } /* don't send request here, it will be performed in eventloop after timeout */ return; } } else if (_state == MAVLINK_WPM_STATE_IDLE) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: no transfer"); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: No transfer"); return; } else { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: busy, state %i", _state); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); return; } struct mission_item_s mission_item; int ret = parse_mavlink_mission_item(&wp, &mission_item); if (ret != OK) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: seq %u invalid item", wp.seq); } _mavlink->send_statustext_critical("IGN MISSION_ITEM: Busy"); send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, ret); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } dm_item_t dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_transfer_dataman_id); if (dm_write(dm_item, wp.seq, DM_PERSIST_POWER_ON_RESET, &mission_item, sizeof(struct mission_item_s)) != sizeof(struct mission_item_s)) { if (_verbose) { warnx("WPM: MISSION_ITEM ERROR: error writing seq %u to dataman ID %i", wp.seq, _transfer_dataman_id); } send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); _mavlink->send_statustext_critical("Unable to write on micro SD"); _state = MAVLINK_WPM_STATE_IDLE; _transfer_in_progress = false; return; } /* waypoint marked as current */ if (wp.current) { _transfer_current_seq = wp.seq; } if (_verbose) { warnx("WPM: MISSION_ITEM seq %u received", wp.seq); } _transfer_seq = wp.seq + 1; if (_transfer_seq == _transfer_count) { /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } _mavlink->send_statustext_info("WPM: Transfer complete."); _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); } else { send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ERROR); } _transfer_in_progress = false; } else { /* request next item */ send_mission_request(_transfer_partner_sysid, _transfer_partner_compid, _transfer_seq); } } }
/* handle an incoming mission item */ void GCS_MAVLINK::handle_mission_item(mavlink_message_t *msg, AP_Mission &mission) { mavlink_mission_item_t packet; uint8_t result = MAV_MISSION_ACCEPTED; struct AP_Mission::Mission_Command cmd = {}; mavlink_msg_mission_item_decode(msg, &packet); if (mavlink_check_target(packet.target_system,packet.target_component)) { return; } // convert mavlink packet to mission command if (!AP_Mission::mavlink_to_mission_cmd(packet, cmd)) { result = MAV_MISSION_INVALID; goto mission_ack; } if (packet.current == 2) { // current = 2 is a flag to tell us this is a "guided mode" // waypoint and not for the mission handle_guided_request(cmd); // verify we received the command result = 0; goto mission_ack; } if (packet.current == 3) { //current = 3 is a flag to tell us this is a alt change only // add home alt if needed handle_change_alt_request(cmd); // verify we recevied the command result = 0; goto mission_ack; } // Check if receiving waypoints (mission upload expected) if (!waypoint_receiving) { result = MAV_MISSION_ERROR; goto mission_ack; } // check if this is the requested waypoint if (packet.seq != waypoint_request_i) { result = MAV_MISSION_INVALID_SEQUENCE; goto mission_ack; } // if command index is within the existing list, replace the command if (packet.seq < mission.num_commands()) { if (mission.replace_cmd(packet.seq,cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if command is at the end of command list, add the command } else if (packet.seq == mission.num_commands()) { if (mission.add_cmd(cmd)) { result = MAV_MISSION_ACCEPTED; }else{ result = MAV_MISSION_ERROR; goto mission_ack; } // if beyond the end of the command list, return an error } else { result = MAV_MISSION_ERROR; goto mission_ack; } // update waypoint receiving state machine waypoint_timelast_receive = hal.scheduler->millis(); waypoint_request_i++; if (waypoint_request_i >= waypoint_request_last) { mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, MAV_MISSION_ACCEPTED); send_text_P(SEVERITY_LOW,PSTR("flight plan received")); waypoint_receiving = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter } else { waypoint_timelast_request = hal.scheduler->millis(); // if we have enough space, then send the next WP immediately if (comm_get_txspace(chan) - MAVLINK_NUM_NON_PAYLOAD_BYTES >= MAVLINK_MSG_ID_MISSION_ITEM_LEN) { queued_waypoint_send(); } else { send_message(MSG_NEXT_WAYPOINT); } } return; mission_ack: // we are rejecting the mission/waypoint mavlink_msg_mission_ack_send_buf( msg, chan, msg->sysid, msg->compid, result); }
void mavlink_wpm_message_handler(const mavlink_message_t* msg) { uint64_t now = mavlink_missionlib_get_system_timestamp(); switch(msg->msgid) { case MAVLINK_MSG_ID_ATTITUDE: { if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) { mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); if(wp->frame == MAV_FRAME_LOCAL_ENU || wp->frame == MAV_FRAME_LOCAL_NED) { mavlink_attitude_t att; mavlink_msg_attitude_decode(msg, &att); float yaw_tolerance = wpm.accept_range_yaw; //compare current yaw if (att.yaw - yaw_tolerance >= 0.0f && att.yaw + yaw_tolerance < 2.f*M_PI) { if (att.yaw - yaw_tolerance <= wp->param4 && att.yaw + yaw_tolerance >= wp->param4) wpm.yaw_reached = true; } else if(att.yaw - yaw_tolerance < 0.0f) { float lowerBound = 360.0f + att.yaw - yaw_tolerance; if (lowerBound < wp->param4 || wp->param4 < att.yaw + yaw_tolerance) wpm.yaw_reached = true; } else { float upperBound = att.yaw + yaw_tolerance - 2.f*M_PI; if (att.yaw - yaw_tolerance < wp->param4 || wp->param4 < upperBound) wpm.yaw_reached = true; } } } break; } case MAVLINK_MSG_ID_LOCAL_POSITION_NED: { if(msg->sysid == mavlink_system.sysid && wpm.current_active_wp_id < wpm.size) { mavlink_mission_item_t *wp = &(wpm.waypoints[wpm.current_active_wp_id]); if(wp->frame == MAV_FRAME_LOCAL_ENU || MAV_FRAME_LOCAL_NED) { mavlink_local_position_ned_t pos; mavlink_msg_local_position_ned_decode(msg, &pos); //// if (debug) // printf("Received new position: x: %f | y: %f | z: %f\n", pos.x, pos.y, pos.z); wpm.pos_reached = false; // compare current position (given in message) with current waypoint float orbit = wp->param1; float dist; // if (wp->param2 == 0) // { // // FIXME segment distance // //dist = mavlink_wpm_distance_to_segment(current_active_wp_id, pos.x, pos.y, pos.z); // } // else // { dist = mavlink_wpm_distance_to_point(wpm.current_active_wp_id, pos.x, pos.y, pos.z); // } if (dist >= 0.f && dist <= orbit && wpm.yaw_reached) { wpm.pos_reached = true; } } } break; } // case MAVLINK_MSG_ID_CMD: // special action from ground station // { // mavlink_cmd_t action; // mavlink_msg_cmd_decode(msg, &action); // if(action.target == mavlink_system.sysid) // { // // if (verbose) std::cerr << "Waypoint: received message with action " << action.action << std::endl; // switch (action.action) // { // // case MAV_ACTION_LAUNCH: // // // if (verbose) std::cerr << "Launch received" << std::endl; // // current_active_wp_id = 0; // // if (wpm.size>0) // // { // // setActive(waypoints[current_active_wp_id]); // // } // // else // // // if (verbose) std::cerr << "No launch, waypointList empty" << std::endl; // // break; // // // case MAV_ACTION_CONTINUE: // // // if (verbose) std::c // // err << "Continue received" << std::endl; // // idle = false; // // setActive(waypoints[current_active_wp_id]); // // break; // // // case MAV_ACTION_HALT: // // // if (verbose) std::cerr << "Halt received" << std::endl; // // idle = true; // // break; // // // default: // // // if (verbose) std::cerr << "Unknown action received with id " << action.action << ", no action taken" << std::endl; // // break; // } // } // break; // } case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm.current_wp_id == wpm.size-1) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); #else if (MAVLINK_WPM_VERBOSE) printf("Received ACK after having sent last waypoint, going to state MAVLINK_WPM_STATE_IDLE\n"); #endif wpm.current_state = MAVLINK_WPM_STATE_IDLE; wpm.current_wp_id = 0; } } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); if(wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < wpm.size) { // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); wpm.current_active_wp_id = wpc.seq; uint32_t i; for(i = 0; i < wpm.size; i++) { if (i == wpm.current_active_wp_id) { wpm.waypoints[i].current = true; } else { wpm.waypoints[i].current = false; } } #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("NEW WP SET"); #else if (MAVLINK_WPM_VERBOSE) printf("New current waypoint %u\n", wpm.current_active_wp_id); #endif wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); mavlink_wpm_send_setpoint(wpm.current_active_wp_id); wpm.timestamp_firstinside_orbit = 0; } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT: Index out of bounds\n"); #endif } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE NOT IN IDLE STATE\n"); #endif } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); if(wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm.size > 0) { //if (verbose && wpm.current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); // if (verbose && wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); wpm.current_state = MAVLINK_WPM_STATE_SENDLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; wpm.current_partner_compid = msg->compid; } else { // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); } wpm.current_count = wpm.size; mavlink_wpm_send_waypoint_count(msg->sysid,msg->compid, wpm.current_count); } else { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm.current_state); } } else { // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); } break; } case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); if(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) if ((wpm.current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm.current_wp_id || wpr.seq == wpm.current_wp_id + 1) && wpr.seq < wpm.size)) { if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id + 1) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm.current_wp_id) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } wpm.current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm.current_wp_id = wpr.seq; mavlink_wpm_send_waypoint(wpm.current_partner_sysid, wpm.current_partner_compid, wpr.seq); } else { // if (verbose) { if (!(wpm.current_state == MAVLINK_WPM_STATE_SENDLIST || wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm.current_state); #endif break; } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq != 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); #endif } } else if (wpm.current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpr.seq != wpm.current_wp_id && wpr.seq != wpm.current_wp_id + 1) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm.current_wp_id, wpm.current_wp_id+1); #endif } else if (wpr.seq >= wpm.size) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); #endif } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); #endif } } } } else { //we we're target but already communicating with someone else if((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm.current_partner_sysid); #endif } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } } break; } case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); if(wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm.timestamp_lastaction = now; if (wpm.current_state == MAVLINK_WPM_STATE_IDLE || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id == 0)) { if (wpc.count > 0) { if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); #endif } if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); #endif } wpm.current_state = MAVLINK_WPM_STATE_GETLIST; wpm.current_wp_id = 0; wpm.current_partner_sysid = msg->sysid; wpm.current_partner_compid = msg->compid; wpm.current_count = wpc.count; #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); #else if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); #endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { // delete waypoints_receive_buffer->back(); // waypoints_receive_buffer->pop_back(); // } mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); } else if (wpc.count == 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("COUNT 0"); #else if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); #endif wpm.rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { // delete waypoints->back(); // waypoints->pop_back(); // } wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; break; } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CMD"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); #endif } } else { if (!(wpm.current_state == MAVLINK_WPM_STATE_IDLE || wpm.current_state == MAVLINK_WPM_STATE_GETLIST)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm.current_state); #endif } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wpm.current_wp_id != 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm.current_wp_id); #endif } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); #endif } } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } } break; case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); mavlink_missionlib_send_gcs_string("GOT WP"); if((msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) { wpm.timestamp_lastaction = now; //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm.current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id && wp.seq < wpm.current_count)) { // if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); // if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); // if (verbose && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm.current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); // wpm.current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_mission_item_t* newwp = &(wpm.rcv_waypoints[wp.seq]); memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); wpm.current_wp_id = wp.seq + 1; // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); if(wpm.current_wp_id == wpm.current_count && wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { mavlink_missionlib_send_gcs_string("GOT ALL WPS"); // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm.current_count); mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); if (wpm.current_active_wp_id > wpm.rcv_size-1) { wpm.current_active_wp_id = wpm.rcv_size-1; } // switch the waypoints list // FIXME CHECK!!! for (int i = 0; i < wpm.current_count; ++i) { wpm.waypoints[i] = wpm.rcv_waypoints[i]; } wpm.size = wpm.current_count; //get the new current waypoint uint32_t i; for(i = 0; i < wpm.size; i++) { if (wpm.waypoints[i].current == 1) { wpm.current_active_wp_id = i; //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); wpm.yaw_reached = false; wpm.pos_reached = false; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); mavlink_wpm_send_setpoint(wpm.current_active_wp_id); wpm.timestamp_firstinside_orbit = 0; break; } } if (i == wpm.size) { wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; wpm.timestamp_firstinside_orbit = 0; } wpm.current_state = MAVLINK_WPM_STATE_IDLE; } else { mavlink_wpm_send_waypoint_request(wpm.current_partner_sysid, wpm.current_partner_compid, wpm.current_wp_id); } } else { if (wpm.current_state == MAVLINK_WPM_STATE_IDLE) { //we're done receiving waypoints, answer with ack. mavlink_wpm_send_waypoint_ack(wpm.current_partner_sysid, wpm.current_partner_compid, 0); // printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); } // if (verbose) { if (!(wpm.current_state == MAVLINK_WPM_STATE_GETLIST || wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm.current_state); break; } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST) { if(!(wp.seq == 0)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } else if (wpm.current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { if (!(wp.seq == wpm.current_wp_id)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm.current_wp_id); } else if (!(wp.seq < wpm.current_count)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } } } else { //we we're target but already communicating with someone else if((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm.current_partner_sysid && msg->compid == wpm.current_partner_compid) && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm.current_partner_sysid); } else if(wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } } break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); if(wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state == MAVLINK_WPM_STATE_IDLE) { wpm.timestamp_lastaction = now; // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); // Delete all waypoints wpm.size = 0; wpm.current_active_wp_id = -1; wpm.yaw_reached = false; wpm.pos_reached = false; } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm.current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm.current_state); } break; } default: { // if (debug) // printf("Waypoint: received message of unknown type"); break; } } //check if the current waypoint was reached if (wpm.pos_reached /*wpm.yaw_reached &&*/ && !wpm.idle) { if (wpm.current_active_wp_id < wpm.size) { mavlink_mission_item_t *cur_wp = &(wpm.waypoints[wpm.current_active_wp_id]); if (wpm.timestamp_firstinside_orbit == 0) { // Announce that last waypoint was reached // if (verbose) // printf("*** Reached waypoint %u ***\n", cur_wp->seq); mavlink_wpm_send_waypoint_reached(cur_wp->seq); wpm.timestamp_firstinside_orbit = now; } // check if the MAV was long enough inside the waypoint orbit //if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000)) if(now-wpm.timestamp_firstinside_orbit >= cur_wp->param2*1000) { if (cur_wp->autocontinue) { cur_wp->current = 0; if (wpm.current_active_wp_id == wpm.size - 1 && wpm.size > 1) { //the last waypoint was reached, if auto continue is //activated restart the waypoint list from the beginning wpm.current_active_wp_id = 1; } else { if ((uint16_t)(wpm.current_active_wp_id + 1) < wpm.size) wpm.current_active_wp_id++; } // Fly to next waypoint wpm.timestamp_firstinside_orbit = 0; mavlink_wpm_send_waypoint_current(wpm.current_active_wp_id); mavlink_wpm_send_setpoint(wpm.current_active_wp_id); wpm.waypoints[wpm.current_active_wp_id].current = true; wpm.pos_reached = false; wpm.yaw_reached = false; // if (verbose) // printf("Set new waypoint (%u)\n", wpm.current_active_wp_id); } } } } else { wpm.timestamp_lastoutside_orbit = now; } }
/** * @brief Receive communication packets and handle them. Should be called at the system sample rate. * * This function decodes packets on the protocol level and also handles * their value by calling the appropriate functions. */ void MavLinkReceive(void) { mavlink_message_t msg; mavlink_status_t status; // Track whether we actually handled any data in this function call. // Used for updating the number of MAVLink messages handled bool processedData = false; while (GetLength(&uart1RxBuffer) > 0) { processedData = true; uint8_t c; Read(&uart1RxBuffer, &c); // Parse another byte and if there's a message found process it. if (mavlink_parse_char(MAVLINK_COMM_0, c, &msg, &status)) { // Latch the groundstation system and component ID if we haven't yet. if (!groundStationSystemId && !groundStationComponentId) { groundStationSystemId = msg.sysid; groundStationComponentId = msg.compid; } switch(msg.msgid) { // If we are not doing any mission protocol operations, record the size of the incoming mission // list and transition into the write missions state machine loop. case MAVLINK_MSG_ID_MISSION_COUNT: { uint8_t mavlinkNewMissionListSize = mavlink_msg_mission_count_get_count(&msg); MavLinkEvaluateMissionState(MISSION_EVENT_COUNT_RECEIVED, &mavlinkNewMissionListSize); } break; // Handle receiving a mission. case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t currentMission; mavlink_msg_mission_item_decode(&msg, ¤tMission); MavLinkEvaluateMissionState(MISSION_EVENT_ITEM_RECEIVED, ¤tMission); } break; // Responding to a mission request entails moving into the first active state and scheduling a MISSION_COUNT message case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: MavLinkEvaluateMissionState(MISSION_EVENT_REQUEST_LIST_RECEIVED, NULL); break; // When a mission request message is received, respond with that mission information from the MissionManager case MAVLINK_MSG_ID_MISSION_REQUEST: { uint8_t receivedMissionIndex = mavlink_msg_mission_request_get_seq(&msg); MavLinkEvaluateMissionState(MISSION_EVENT_REQUEST_RECEIVED, &receivedMissionIndex); } break; // Allow for clearing waypoints. Here we respond simply with an ACK message if we successfully // cleared the mission list. case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: MavLinkEvaluateMissionState(MISSION_EVENT_CLEAR_ALL_RECEIVED, NULL); break; // Allow for the groundstation to set the current mission. This requires a WAYPOINT_CURRENT response message agreeing with the received current message index. case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { uint8_t newCurrentMission = mavlink_msg_mission_set_current_get_seq(&msg); MavLinkEvaluateMissionState(MISSION_EVENT_SET_CURRENT_RECEIVED, &newCurrentMission); } break; case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_msg_mission_ack_get_type(&msg); MavLinkEvaluateMissionState(MISSION_EVENT_ACK_RECEIVED, NULL); } break; // If they're requesting a list of all parameters, call a separate function that'll track the state and transmit the necessary messages. // This reason that this is an external function is so that it can be run separately at 20Hz. case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: { MavLinkEvaluateParameterState(PARAM_EVENT_REQUEST_LIST_RECEIVED, NULL); } break; // If a request comes for a single parameter then set that to be the current parameter and move into the proper state. case MAVLINK_MSG_ID_PARAM_REQUEST_READ: { uint16_t currentParameter = mavlink_msg_param_request_read_get_param_index(&msg); MavLinkEvaluateParameterState(PARAM_EVENT_REQUEST_READ_RECEIVED, ¤tParameter); } break; case MAVLINK_MSG_ID_PARAM_SET: { mavlink_param_set_t x; mavlink_msg_param_set_decode(&msg, &x); MavLinkEvaluateParameterState(PARAM_EVENT_SET_RECEIVED, &x); } break; default: break; } } } // Update the number of messages received, both successful and not. Note that the 'status' variable // will be updated on every call to *_parse_char(), so this will always be a valid value. if (processedData) { mavLinkMessagesReceived = status.packet_rx_success_count; mavLinkMessagesFailedParsing = status.packet_rx_drop_count; } }
void mavlink_connector::handle_missions(mavlink_message_t *msg) { mavlink_message_t resp; switch (msg->msgid) { case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_count_t count; count.count = dji_variable::wp_m.missions[0].size(); count.target_component = msg->compid; count.target_system = msg->sysid; mavlink_msg_mission_count_encode(0, 200, &resp, &count); this->send_msg(&resp); break; } case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t req; mavlink_msg_mission_request_decode(msg, &req); // printf("ground is trying to request mission %d \n",req.seq); std::vector<mavlink_mission_item_t> mission_msgs = dji_variable::wp_m.missions[0].to_mission_items(); mavlink_mission_item_t a = mission_msgs[req.seq]; a.target_system = msg->sysid; a.target_component = msg->compid; a.current = (a.seq == dji_variable::wp_m.waypoint_ptr && dji_variable::wp_m.mission_id == 0)? 1:0; mavlink_msg_mission_item_encode(0,200, &resp, &a); this->send_msg(&resp); break; } case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t mission_count_t; mavlink_msg_mission_count_decode(msg, &mission_count_t); if (MISSION_REC_STATE == MISSION_REC_STANDBY) { printf("Ground is trying to set a waypoint long as %d\n", mission_count_t.count); waypoint_length_size = mission_count_t.count; waypoint_waitfor = 0; dji_variable::wp_m.missions[0].clear(); MISSION_REC_STATE = MISSION_REC_WAIT_FOR; mavlink_mission_request_t request_t; request_t.target_component = msg->compid; request_t.target_system = msg->sysid; request_t.seq = 0; mavlink_msg_mission_request_encode(0,200,&resp,&request_t); this->send_msg(&resp); break; } }; case MAVLINK_MSG_ID_MISSION_ITEM: { if (MISSION_REC_STATE != MISSION_REC_WAIT_FOR) break; mavlink_mission_item_t mission_item_t; mavlink_msg_mission_item_decode(msg,&mission_item_t); printf("seq:%d %f %f cmd: %d\n",mission_item_t.seq,mission_item_t.x,mission_item_t.y,mission_item_t.command); api_pos_custom_data_t wp; wp.lati = mission_item_t.x /180 * M_PI; wp.longti = mission_item_t.y /180 * M_PI; wp.alti = mission_item_t.z; wp.cmd = mission_item_t.command; // wp.uncertain = mission_item_t. wp.uncertain = 10 ; mavlink_mission_request_t request_t; request_t.target_component = msg->compid; request_t.target_system = msg->sysid; if (mission_item_t.seq == waypoint_waitfor) { waypoint_waitfor ++; request_t.seq = waypoint_waitfor; dji_variable::wp_m.missions[0].waypoints.push_back(wp); if (request_t.seq < waypoint_length_size) { mavlink_msg_mission_request_encode(0, 200, &resp, &request_t); this->send_msg(&resp); } else{ std::cout << dji_variable::wp_m.missions[0]; waypoint_waitfor = 0; waypoint_length_size = 0; MISSION_REC_STATE = MISSION_REC_STANDBY; mavlink_mission_ack_t ack_t; ack_t.target_component = msg->compid; ack_t.target_system = msg->sysid; ack_t.type = MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(0,200,&resp,&ack_t); this->send_msg(&resp); } } else{ request_t.seq = waypoint_waitfor; mavlink_msg_mission_request_encode(0, 200, &resp, &request_t); this->send_msg(&resp); } break; }; case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t set_current_t; mavlink_msg_mission_set_current_decode(msg,&set_current_t); int ptr = set_current_t.seq; printf("say system is trying to set current %d \n",ptr); dji_variable::wp_m.mission_id = 0; dji_variable::wp_m.waypoint_ptr = ptr; dji_variable::wp_m.begin_fly_waypoints(0,ptr); mavlink_mission_ack_t ack_t; ack_t.target_component = msg->compid; ack_t.target_system = msg->sysid; ack_t.type = MAV_MISSION_RESULT::MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(0,200,&resp,&ack_t); this->send_msg(&resp); }; default: break; } }
void protDecodeMavlink(void) { uint8_t indx, writeSuccess, commChannel = 1; mavlink_param_set_t set; mavlink_message_t msg; mavlink_status_t status; // uint8_t* dataIn; // fix the data length so if the interrupt adds data // during execution of this block, it will be read // until the next gsRead unsigned int tmpLen = getLength(uartMavlinkInBuffer), i = 0; // if the buffer has more data than the max size, set it to max, // otherwise set it to the length //DatafromGSmavlink[0] = (tmpLen > MAXINLEN) ? MAXINLEN : tmpLen; // read the data //for (i = 1; i <= DatafromGSmavlink[0]; i += 1) { //mavlink_parse_char(commChannel, readFront(uartBufferInMavlink), &msg, &status); //DatafromGSmavlink[i] = readFront(uartMavlinkInBuffer); //} //dataIn = DatafromGSmavlink; // increment the age of heartbeat mlPending.heartbeatAge++; for (i = 0; i <= tmpLen; i++) { // Try to get a new message if (mavlink_parse_char(commChannel, readFront(uartMavlinkInBuffer), &msg, &status)) { // Handle message switch (msg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: mavlink_msg_heartbeat_decode(&msg, &mlHeartbeat); // Reset the heartbeat mlPending.heartbeatAge = 0; break; //AM DBG case MAVLINK_MSG_ID_MISSION_COUNT: if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) { mavlink_msg_mission_count_decode(&msg, &mlWpCount); // Start the transaction mlPending.wpTransaction = 1; // change the state mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE; // reset the rest of the state machine mlPending.wpTotalWps = mlWpCount.count; mlPending.wpCurrentWpInTransaction = 0; mlPending.wpTimeOut = 0; } break; case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: // if there is no transaction going on if (!mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_IDLE)) { // Start the transaction mlPending.wpTransaction = 1; // change the state mlPending.wpProtState = WP_PROT_LIST_REQUESTED; // reset the rest of the state machine mlPending.wpCurrentWpInTransaction = 0; mlPending.wpTimeOut = 0; } break; case MAVLINK_MSG_ID_MISSION_REQUEST: mavlink_msg_mission_request_decode(&msg, &mlWpRequest); if (mlPending.wpTransaction && (mlWpRequest.seq < mlWpValues.wpCount)) { // change the state mlPending.wpProtState = WP_PROT_TX_WP; // reset the rest of the state machine mlPending.wpCurrentWpInTransaction = mlWpRequest.seq; mlPending.wpTimeOut = 0; } else { // TODO: put here a report for a single WP, i.e. not inside a transaction } break; case MAVLINK_MSG_ID_MISSION_ACK: mavlink_msg_mission_ack_decode(&msg, &mlWpAck); if (mlPending.wpTransaction) { // End the transaction mlPending.wpTransaction = 0; // change the state mlPending.wpProtState = WP_PROT_IDLE; // reset the rest of the state machine mlPending.wpCurrentWpInTransaction = 0; mlPending.wpTimeOut = 0; // send current waypoint index mlPending.wpSendCurrent = TRUE; } break; case MAVLINK_MSG_ID_MISSION_ITEM: writeSuccess = SUCCESS; mavlink_msg_mission_item_decode(&msg, &mlSingleWp); if (mlPending.wpTransaction && (mlPending.wpProtState == WP_PROT_RX_WP)) { mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE; } indx = (uint8_t) mlSingleWp.seq; mlWpValues.lat[indx] = mlSingleWp.x; mlWpValues.lon[indx] = mlSingleWp.y; mlWpValues.alt[indx] = mlSingleWp.z; mlWpValues.type[indx] = mlSingleWp.command; mlWpValues.orbit[indx] = (uint16_t) mlSingleWp.param3; /* // Record the data in EEPROM writeSuccess = storeWaypointInEeprom(&mlSingleWp); // Set the flag of Aknowledge for the AKN Message // if the write was not successful if (writeSuccess != SUCCESS) { mlPending.wpAck++; mlWpAck.target_component = MAV_COMP_ID_MISSIONPLANNER; mlWpAck.type = MAV_MISSION_ERROR; } */ break; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: writeSuccess = SUCCESS; // clear the WP values in memory; memset(&mlWpValues, 0, sizeof (mavlink_mission_item_values_t)); /* writeSuccess = clearWaypointsFrom(0); // Set the flag of Aknowledge fail // if the write was unsuccessful if (writeSuccess != SUCCESS) { mlPending.statustext++; mlStatustext.severity = MAV_SEVERITY_ERROR; strncpy(mlStatustext.text, "Failed to clear waypoints from EEPROM.", 49); } */ // Update the waypoint count mlWpValues.wpCount = 0; // Set the state machine ready to send the WP akn mlPending.wpCurrentWpInTransaction = 0; mlPending.wpTotalWps = 0; mlPending.wpTransaction = 1; mlPending.wpProtState = WP_PROT_GETTING_WP_IDLE; break; case MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN: writeSuccess = SUCCESS; memset(&mlSingleWp, 0, sizeof (mavlink_mission_item_t)); mavlink_msg_set_gps_global_origin_decode(&msg, &mlGSLocation); mlSingleWp.x = (float) (mlGSLocation.latitude); mlSingleWp.y = (float) (mlGSLocation.longitude); mlSingleWp.z = (float) (mlGSLocation.altitude); indx = (uint8_t) MAX_NUM_WPS - 1; mlWpValues.lat[indx] = mlSingleWp.x; mlWpValues.lon[indx] = mlSingleWp.y; mlWpValues.alt[indx] = mlSingleWp.z; mlWpValues.type[indx] = MAV_CMD_NAV_LAND; mlWpValues.orbit[indx] = 0; // Record the data in EEPROM /* writeSuccess = storeWaypointInEeprom(&mlSingleWp); if (writeSuccess != SUCCESS) { mlPending.statustext++; mlStatustext.severity = MAV_SEVERITY_ERROR; strncpy(mlStatustext.text, "Failed to write origin to EEPROM.", 49); } else { mlPending.statustext++; mlStatustext.severity = MAV_SEVERITY_INFO; strncpy(mlStatustext.text, "Control DSC GPS origin set.", 49); } */ break; //AM DBG case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: mlPending.piTransaction = 1; mlPending.piProtState = PI_SEND_ALL_PARAM; mlPending.piCurrentParamInTransaction = 0; break; case MAVLINK_MSG_ID_PARAM_REQUEST_READ: // If it was in the middle of a list transmission or there is already a param enqueued mlPending.piTransaction = 1; switch (mlPending.piProtState) { case PI_IDLE: mlPending.piBackToList = 0; // no need to go back mlPending.piQIdx = -1; // no Index mlPending.piCurrentParamInTransaction = mavlink_msg_param_request_read_get_param_index(&msg); // assign directly mlPending.piProtState = PI_SEND_ONE_PARAM; break; case PI_SEND_ALL_PARAM: mlPending.piBackToList = 1; // mark to go back mlPending.piQIdx++; // done like this because when empty index = -1 mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue mlPending.piProtState = PI_SEND_ONE_PARAM; break; case PI_SEND_ONE_PARAM: if (mlPending.piBackToList) { mlPending.piQIdx++; // done like this because when empty index = -1 mlPending.piQueue[mlPending.piQIdx] = mavlink_msg_param_request_read_get_param_index(&msg); // put in in queue } mlPending.piProtState = PI_SEND_ONE_PARAM; break; } break; case MAVLINK_MSG_ID_PARAM_SET: mavlink_msg_param_set_decode(&msg, &set); if ((uint8_t) set.target_system == (uint8_t) SYSTEMID && (uint8_t) set.target_component == (uint8_t) COMPID) { char* key = (char*) set.param_id; uint8_t i, j; uint8_t match; for (i = 0; i < PAR_PARAM_COUNT; i++) { match = 1; for (j = 0; j < PARAM_NAME_LENGTH; j++) { // Compare if (((char) (mlParamInterface.param_name[i][j])) != (char) (key[j])) { match = 0; } // if // End matching if null termination is reached if (((char) mlParamInterface.param_name[i][j]) == '\0') { break; } // if }// for j // Check if matched if (match) { //sw_debug = 1; // Only write and emit changes if there is actually a difference // AND only write if new value is NOT "not-a-number" // AND is NOT infinity if (isFinite(set.param_value)) { mlParamInterface.param[i] = set.param_value; // Report back new value mlPending.piBackToList = 0; // no need to go back mlPending.piQIdx = -1; // no Index mlPending.piCurrentParamInTransaction = i; // assign directly mlPending.piProtState = PI_SEND_ONE_PARAM; mlPending.piTransaction = 1; } // if different and not nan and not inf } // if match }// for i } // if addressed to this break; default: break; } } } }
void MAVLinkProxy::dataReceived(const boost::system::error_code &error, size_t received_bytes) { static mavlink_message_t msg; static mavlink_status_t status; // Response message and buffer static mavlink_message_t msg_response; static uint8_t buf[MAVLINK_MAX_PACKET_LEN]; // Waypoint reception variables static int waypoints_to_receive = 0; static uint16_t waypoints_received = 0; static int waypoint_timeout = 200; static unsigned long last_waypoint_time = 0; for(int i = 0; i < (int) received_bytes; i++) { if(mavlink_parse_char(MAVLINK_COMM_0, _received_msg_buf[i], &msg, &status)) { // Handle received messages msg_response.magic = 0; // Waypoint reception if(waypoints_to_receive > 0 && (last_waypoint_time + waypoint_timeout <= (std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1)))) { // Timeout printf("Timeout receiving waypoints!\n"); waypoints_to_receive = 0; last_waypoint_time = 0; } if(waypoints_to_receive > 0 && msg.msgid == MAVLINK_MSG_ID_MISSION_ITEM) { // Got waypoint waypoints_received++; waypoints_to_receive--; mavlink_mission_item_t waypoint; mavlink_msg_mission_item_decode(&msg, &waypoint); handleWaypoint(&waypoint); last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); if(waypoints_to_receive > 0) { mavlink_mission_request_t request; request.seq = waypoints_received; request.target_system = msg.sysid; request.target_component = msg.compid; mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request); } else { mavlink_mission_ack_t ack; ack.target_system = msg.sysid; ack.target_component = msg.compid; ack.type = MAV_MISSION_ACCEPTED; mavlink_msg_mission_ack_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &ack); processPartialMission(); } } else if(msg.msgid == MAVLINK_MSG_ID_MISSION_REQUEST_LIST) { printf("Waypoint count request\n"); uint16_t mission_length = 0; mavlink_msg_mission_count_pack(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, msg.sysid, msg.compid, mission_length); } else if(msg.msgid == MAVLINK_MSG_ID_PARAM_REQUEST_LIST) { printf("Parameter request\n"); mavlink_param_value_t param; param.param_count = 0; param.param_id[0] = '\0'; param.param_index = 0; param.param_type = MAV_PARAM_TYPE_UINT8; param.param_value = 0; mavlink_msg_param_value_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, ¶m); } else if(msg.msgid == MAVLINK_MSG_ID_MISSION_COUNT) { mavlink_mission_count_t mission_count; mavlink_msg_mission_count_decode(&msg, &mission_count); printf("%d waypoints available\n", mission_count.count); _mission_ready = false; _partial_mission.clear(); waypoints_to_receive = mission_count.count; waypoints_received = 0; last_waypoint_time = std::chrono::system_clock::now().time_since_epoch() / std::chrono::milliseconds(1); mavlink_mission_request_t request; request.seq = 0; request.target_system = msg.sysid; request.target_component = msg.compid; mavlink_msg_mission_request_encode(_mavlink_system.sysid, _mavlink_system.compid, &msg_response, &request); } else if(msg.msgid == MAVLINK_MSG_ID_HEARTBEAT) { // Heartbeat from MAVLink ground station } else { // Unhandled message type printf("Unknown MAVLink packet: SYS: %d, COMP: %d, LEN: %d, MSG ID: %d\n", msg.sysid, msg.compid, msg.len, msg.msgid); } // Send a response if the message has been populated if(msg_response.magic != 0) { mavlink_msg_to_send_buffer(buf, &msg_response); _socket.send_to(boost::asio::buffer(buf), _endpoint); } } } // Listen for next packet _socket.async_receive_from(boost::asio::buffer(_received_msg_buf), _endpoint, boost::bind(&MAVLinkProxy::dataReceived, this, boost::asio::placeholders::error, boost::asio::placeholders::bytes_transferred)); }
void mavlink_wpm_message_handler(const mavlink_message_t *msg, const struct vehicle_global_position_s *global_pos , struct vehicle_local_position_s *local_pos) { uint64_t now = mavlink_missionlib_get_system_timestamp(); switch (msg->msgid) { case MAVLINK_MSG_ID_MISSION_ACK: { mavlink_mission_ack_t wpa; mavlink_msg_mission_ack_decode(msg, &wpa); if ((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wpa.target_system == mavlink_system.sysid /*&& wpa.target_component == mavlink_wpm_comp_id*/)) { wpm->timestamp_lastaction = now; if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpm->current_wp_id == wpm->size - 1) { mavlink_missionlib_send_gcs_string("Got last WP ACK state -> IDLE"); wpm->current_state = MAVLINK_WPM_STATE_IDLE; wpm->current_wp_id = 0; } } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: curr partner id mismatch"); } break; } case MAVLINK_MSG_ID_MISSION_SET_CURRENT: { mavlink_mission_set_current_t wpc; mavlink_msg_mission_set_current_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid /*&& wpc.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { if (wpc.seq < wpm->size) { // if (verbose) // printf("Received MAVLINK_MSG_ID_MISSION_ITEM_SET_CURRENT\n"); wpm->current_active_wp_id = wpc.seq; uint32_t i; for (i = 0; i < wpm->size; i++) { if (i == wpm->current_active_wp_id) { wpm->waypoints[i].current = true; } else { wpm->waypoints[i].current = false; } } mavlink_missionlib_send_gcs_string("NEW WP SET"); wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); mavlink_wpm_send_setpoint(wpm->current_active_wp_id); wpm->timestamp_firstinside_orbit = 0; } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Not in list"); } } else { mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy"); } } else { mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); } break; } case MAVLINK_MSG_ID_MISSION_REQUEST_LIST: { mavlink_mission_request_list_t wprl; mavlink_msg_mission_request_list_decode(msg, &wprl); if (wprl.target_system == mavlink_system.sysid /*&& wprl.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpm->size > 0) { //if (verbose && wpm->current_state == MAVLINK_WPM_STATE_IDLE) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u changing state to MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST again from %u staying in state MAVLINK_WPM_STATE_SENDLIST\n", msg->sysid); wpm->current_state = MAVLINK_WPM_STATE_SENDLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; } else { // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST from %u but have no waypoints, staying in \n", msg->sysid); } wpm->current_count = wpm->size; mavlink_wpm_send_waypoint_count(msg->sysid, msg->compid, wpm->current_count); } else { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST_LIST because i'm doing something else already (state=%i).\n", wpm->current_state); } } else { // if (verbose) // printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT MISMATCH\n"); } break; } case MAVLINK_MSG_ID_MISSION_REQUEST: { mavlink_mission_request_t wpr; mavlink_msg_mission_request_decode(msg, &wpr); if (msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid && wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; //ensure that we are in the correct state and that the first request has id 0 and the following requests have either the last id (re-send last waypoint) or last_id+1 (next waypoint) if ((wpm->current_state == MAVLINK_WPM_STATE_SENDLIST && wpr.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && (wpr.seq == wpm->current_wp_id || wpr.seq == wpm->current_wp_id + 1) && wpr.seq < wpm->size)) { if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT WP REQ, state -> SEND"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u changing state to MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id + 1) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS && wpr.seq == wpm->current_wp_id) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("GOT 2nd WP REQ"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_REQUEST of waypoint %u (again) from %u staying in state MAVLINK_WPM_STATE_SENDLIST_SENDWPS\n", wpr.seq, msg->sysid); #endif } wpm->current_state = MAVLINK_WPM_STATE_SENDLIST_SENDWPS; wpm->current_wp_id = wpr.seq; mavlink_wpm_send_waypoint(wpm->current_partner_sysid, wpm->current_partner_compid, wpr.seq); } else { // if (verbose) { if (!(wpm->current_state == MAVLINK_WPM_STATE_SENDLIST || wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because i'm doing something else already (state=%i).\n", wpm->current_state); #endif break; } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST) { if (wpr.seq != 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: First id != 0"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the first requested waypoint ID (%u) was not 0.\n", wpr.seq); #endif } } else if (wpm->current_state == MAVLINK_WPM_STATE_SENDLIST_SENDWPS) { if (wpr.seq != wpm->current_wp_id && wpr.seq != wpm->current_wp_id + 1) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP was unexpected"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was not the expected (%u or %u).\n", wpr.seq, wpm->current_wp_id, wpm->current_wp_id + 1); #endif } else if (wpr.seq >= wpm->size) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Req. WP not in list"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST because the requested waypoint ID (%u) was out of bounds.\n", wpr.seq); #endif } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST - FIXME: missed error description\n"); #endif } } } } else { //we we're target but already communicating with someone else if ((wpr.target_system == mavlink_system.sysid /*&& wpr.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.\n", msg->sysid, wpm->current_partner_sysid); #endif } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } } break; } case MAVLINK_MSG_ID_MISSION_COUNT: { mavlink_mission_count_t wpc; mavlink_msg_mission_count_decode(msg, &wpc); if (wpc.target_system == mavlink_system.sysid/* && wpc.target_component == mavlink_wpm_comp_id*/) { wpm->timestamp_lastaction = now; if (wpm->current_state == MAVLINK_WPM_STATE_IDLE || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id == 0)) { // printf("wpc count in: %d\n",wpc.count); // printf("Comp id: %d\n",msg->compid); // printf("Current partner sysid: %d\n",wpm->current_partner_sysid); if (wpc.count > 0) { if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("WP CMD OK: state -> GETLIST"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) from %u changing state to MAVLINK_WPM_STATE_GETLIST\n", wpc.count, msg->sysid); #endif } if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("WP CMD OK AGAIN"); #else if (MAVLINK_WPM_VERBOSE) printf("Got MAVLINK_MSG_ID_MISSION_ITEM_COUNT (%u) again from %u\n", wpc.count, msg->sysid); #endif } wpm->current_state = MAVLINK_WPM_STATE_GETLIST; wpm->current_wp_id = 0; wpm->current_partner_sysid = msg->sysid; wpm->current_partner_compid = msg->compid; wpm->current_count = wpc.count; #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("CLR RCV BUF: READY"); #else if (MAVLINK_WPM_VERBOSE) printf("clearing receive buffer and readying for receiving waypoints\n"); #endif wpm->rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { // delete waypoints_receive_buffer->back(); // waypoints_receive_buffer->pop_back(); // } mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } else if (wpc.count == 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("COUNT 0"); #else if (MAVLINK_WPM_VERBOSE) printf("got waypoint count of 0, clearing waypoint list and staying in state MAVLINK_WPM_STATE_IDLE\n"); #endif wpm->rcv_size = 0; //while(waypoints_receive_buffer->size() > 0) // { // delete waypoints->back(); // waypoints->pop_back(); // } wpm->current_active_wp_id = -1; wpm->yaw_reached = false; wpm->pos_reached = false; break; } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("IGN WP CMD"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignoring MAVLINK_MSG_ID_MISSION_ITEM_COUNT from %u with count of %u\n", msg->sysid, wpc.count); #endif } } else { if (!(wpm->current_state == MAVLINK_WPM_STATE_IDLE || wpm->current_state == MAVLINK_WPM_STATE_GETLIST)) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm doing something else already (state=%i).\n", wpm->current_state); #endif } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wpm->current_wp_id != 0) { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT because i'm already receiving waypoint %u.\n", wpm->current_wp_id); #endif } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: ?"); #else if (MAVLINK_WPM_VERBOSE) printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_COUNT - FIXME: missed error description\n"); #endif } } } else { #ifdef MAVLINK_WPM_NO_PRINTF mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch"); #else if (MAVLINK_WPM_VERBOSE) printf("IGNORED WAYPOINT COMMAND BECAUSE TARGET SYSTEM AND COMPONENT OR COMM PARTNER ID MISMATCH\n"); #endif } } break; case MAVLINK_MSG_ID_MISSION_ITEM: { mavlink_mission_item_t wp; mavlink_msg_mission_item_decode(msg, &wp); mavlink_missionlib_send_gcs_string("GOT WP"); // printf("sysid=%d, current_partner_sysid=%d\n", msg->sysid, wpm->current_partner_sysid); // printf("compid=%d, current_partner_compid=%d\n", msg->compid, wpm->current_partner_compid); // if((msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && (wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/)) if (wp.target_system == mavlink_system.sysid && wp.target_component == mavlink_wpm_comp_id) { wpm->timestamp_lastaction = now; // printf("wpm->current_state=%u, wp.seq = %d, wpm->current_wp_id=%d\n", wpm->current_state, wp.seq, wpm->current_wp_id); // wpm->current_state = MAVLINK_WPM_STATE_GETLIST;//removeme debug XXX TODO //ensure that we are in the correct state and that the first waypoint has id 0 and the following waypoints have the correct ids if ((wpm->current_state == MAVLINK_WPM_STATE_GETLIST && wp.seq == 0) || (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id && wp.seq < wpm->current_count)) { //mavlink_missionlib_send_gcs_string("DEBUG 2"); // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u changing state to MAVLINK_WPM_STATE_GETLIST_GETWPS\n", wp.seq, msg->sysid); // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u from %u\n", wp.seq, msg->sysid); // if (verbose && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS && wp.seq-1 == wpm->current_wp_id) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM %u (again) from %u\n", wp.seq, msg->sysid); // wpm->current_state = MAVLINK_WPM_STATE_GETLIST_GETWPS; mavlink_mission_item_t *newwp = &(wpm->rcv_waypoints[wp.seq]); memcpy(newwp, &wp, sizeof(mavlink_mission_item_t)); // printf("WP seq: %d\n",wp.seq); wpm->current_wp_id = wp.seq + 1; // if (verbose) // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); // printf ("Added new waypoint to list. X= %f\t Y= %f\t Z= %f\t Yaw= %f\n", newwp->x, newwp->y, newwp->z, newwp->param4); // printf ("wpm->current_wp_id =%d, wpm->current_count=%d\n\n", wpm->current_wp_id, wpm->current_count); if (wpm->current_wp_id == wpm->current_count && wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { mavlink_missionlib_send_gcs_string("GOT ALL WPS"); // if (verbose) // printf("Got all %u waypoints, changing state to MAVLINK_WPM_STATE_IDLE\n", wpm->current_count); mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); if (wpm->current_active_wp_id > wpm->rcv_size - 1) { wpm->current_active_wp_id = wpm->rcv_size - 1; } // switch the waypoints list // FIXME CHECK!!! uint32_t i; for (i = 0; i < wpm->current_count; ++i) { wpm->waypoints[i] = wpm->rcv_waypoints[i]; } wpm->size = wpm->current_count; //get the new current waypoint for (i = 0; i < wpm->size; i++) { if (wpm->waypoints[i].current == 1) { wpm->current_active_wp_id = i; //// if (verbose) // printf("New current waypoint %u\n", current_active_wp_id); wpm->yaw_reached = false; wpm->pos_reached = false; mavlink_wpm_send_waypoint_current(wpm->current_active_wp_id); mavlink_wpm_send_setpoint(wpm->current_active_wp_id); wpm->timestamp_firstinside_orbit = 0; break; } } if (i == wpm->size) { wpm->current_active_wp_id = -1; wpm->yaw_reached = false; wpm->pos_reached = false; wpm->timestamp_firstinside_orbit = 0; } wpm->current_state = MAVLINK_WPM_STATE_IDLE; } else { mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } } else { if (wpm->current_state == MAVLINK_WPM_STATE_IDLE) { //we're done receiving waypoints, answer with ack. mavlink_wpm_send_waypoint_ack(wpm->current_partner_sysid, wpm->current_partner_compid, 0); printf("Received MAVLINK_MSG_ID_MISSION_ITEM while state=MAVLINK_WPM_STATE_IDLE, answered with WAYPOINT_ACK.\n"); } // if (verbose) { if (!(wpm->current_state == MAVLINK_WPM_STATE_GETLIST || wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u because i'm doing something else already (state=%i).\n", wp.seq, wpm->current_state); break; } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST) { if (!(wp.seq == 0)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the first waypoint ID (%u) was not 0.\n", wp.seq); } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } else if (wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) { if (!(wp.seq == wpm->current_wp_id)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was not the expected %u.\n", wp.seq, wpm->current_wp_id); mavlink_wpm_send_waypoint_request(wpm->current_partner_sysid, wpm->current_partner_compid, wpm->current_wp_id); } else if (!(wp.seq < wpm->current_count)) { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM because the waypoint ID (%u) was out of bounds.\n", wp.seq); } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } else { // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u - FIXME: missed error description\n", wp.seq); } } } } else { //we we're target but already communicating with someone else if ((wp.target_system == mavlink_system.sysid /*&& wp.target_component == mavlink_wpm_comp_id*/) && !(msg->sysid == wpm->current_partner_sysid && msg->compid == wpm->current_partner_compid) && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i'm already talking to ID %u.\n", wp.seq, msg->sysid, wpm->current_partner_sysid); } else if (wp.target_system == mavlink_system.sysid /* && wp.target_component == mavlink_wpm_comp_id*/) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM %u from ID %u because i have no idea what to do with it\n", wp.seq, msg->sysid); } } break; } case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: { mavlink_mission_clear_all_t wpca; mavlink_msg_mission_clear_all_decode(msg, &wpca); if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state == MAVLINK_WPM_STATE_IDLE) { wpm->timestamp_lastaction = now; // if (verbose) // printf("Got MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u deleting all waypoints\n", msg->sysid); // Delete all waypoints wpm->size = 0; wpm->current_active_wp_id = -1; wpm->yaw_reached = false; wpm->pos_reached = false; } else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && wpm->current_state != MAVLINK_WPM_STATE_IDLE) { // if (verbose) // printf("Ignored MAVLINK_MSG_ID_MISSION_ITEM_CLEAR_LIST from %u because i'm doing something else already (state=%i).\n", msg->sysid, wpm->current_state); } break; } default: { // if (debug) // printf("Waypoint: received message of unknown type"); break; } } check_waypoints_reached(now, global_pos, local_pos); }
static inline void MissionItem(mavlink_message_t* handle_msg) { int16_t flags; struct waypoint3D wp; mavlink_mission_item_t packet; //send_text((uint8_t*)"waypoint\r\n"); // DPRINT("mission item\r\n"); // Check if receiving waypoint if (!mavlink_flags.mavlink_receiving_waypoints) return; // decode mavlink_msg_mission_item_decode(handle_msg, &packet); if (mavlink_check_target(packet.target_system, packet.target_component)) return; DPRINT("mission item: %u\r\n", packet.seq); // check if this is the requested waypoint if (packet.seq != waypoint_request_i) return; // store waypoint //uint8_t loadAction = 0; // 0 insert in list, 1 exec now switch (packet.frame) { case MAV_FRAME_GLOBAL: { // DPRINT("FRAME_GLOBAL\r\n"); //struct waypoint3D { int32_t x; int32_t y; int16_t z; }; //struct waypointDef { struct waypoint3D loc; int16_t flags; struct waypoint3D viewpoint; }; // DPRINT("packet.x %f packet.y %f packet.z %f\r\n", packet.x, packet.y, packet.z); //tell_command.lng = 1.0e7*packet.x; //tell_command.lat = 1.0e7*packet.y; //tell_command.alt = packet.z*1.0e2; // MatrixPilot uses X & Y in reverse to QGC wp.x = packet.y * 1.0e7; wp.y = packet.x * 1.0e7; wp.z = packet.z; flags = F_ABSOLUTE; break; } case MAV_FRAME_LOCAL_NED: // local (relative to home position) { DPRINT("FRAME_LOCAL - not implemented\r\n"); //tell_command.lng = 1.0e7*ToDeg(packet.x/ //(radius_of_earth*cos(ToRad(home.lat/1.0e7)))) + home.lng; //tell_command.lat = 1.0e7*ToDeg(packet.y/radius_of_earth) + home.lat; //tell_command.alt = -packet.z*1.0e2 + home.alt; break; } } // defaults //tell_command.id = CMD_BLANK; // Currently F can be set to: F_NORMAL, or any combination of: // F_ABSOLUTE - Waypoints are Relative by default, unless F_ABSOLUTE is specified. // // F_TAKEOFF - More quickly gain altitude at takeoff. // F_INVERTED - Navigate to this waypoint with the plane upside down. (only if STABILIZE_INVERTED_FLIGHT is set to 1 in options.h) // F_HOVER - Hover the plane until reaching this waypoint. (only if STABILIZE_HOVER is set to 1 in options.h) // NOTE: while hovering, no navigation is performed, and throttle is under manual control. // F_LOITER - After reaching this waypoint, continue navigating towards this same waypoint. Repeat until leaving waypoint mode. // F_TRIGGER - Trigger an action to happen when this waypoint leg starts. (See the Trigger Action section of the options.h file.) // F_ALTITUDE_GOAL - Climb or descend to the given altitude, then continue to the next waypoint. // F_CROSS_TRACK - Navigate using cross-tracking. Best used for longer waypoint legs. // F_LAND - Navigate towards this waypoint with the throttle off. switch (packet.command) { case MAV_CMD_NAV_TAKEOFF: DPRINT("NAV_TAKEOFF\r\n"); //tell_command.id = CMD_TAKEOFF; flags |= F_TAKEOFF; break; case MAV_CMD_NAV_LAND: DPRINT("NAV_LAND\r\n"); //tell_command.id = CMD_LAND; flags |= F_LAND; break; case MAV_CMD_NAV_WAYPOINT: // DPRINT("NAV_WAYPOINT\r\n"); //tell_command.id = CMD_WAYPOINT; break; case MAV_CMD_NAV_LOITER_UNLIM: // DPRINT("NAV_LOITER\r\n"); //tell_command.id = CMD_LOITER_TIME; //tell_command.p1 = packet.param2/1.0e2; break; } // save waypoint add_waypoint(wp, flags); //set_wp_with_index(tell_command, packet.seq); // update waypoint receiving state machine //global_data.waypoint_timelast_receive = millis(); mavlink_waypoint_timeout = MAVLINK_WAYPOINT_TIMEOUT; waypoint_request_i++; if (waypoint_request_i == get(PARAM_WP_TOTAL)) { uint8_t type = 0; // ok (0), error(1) //gcs.send_text("flight plane received"); DPRINT("flight plan received\r\n"); mavlink_msg_mission_ack_send(MAVLINK_COMM_0, handle_msg->sysid, handle_msg->compid, type); mavlink_flags.mavlink_receiving_waypoints = false; // XXX ignores waypoint radius for individual waypoints, can // only set WP_RADIUS parameter // send MAVLINK_MSG_ID_MISSION_ACK ? } else { mavlink_flags.mavlink_request_specific_waypoint = 1; } }
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); }