diff --git a/src/examples/2._Load_Payload_Settings/load_payload_settings.cpp b/src/examples/2._Load_Payload_Settings/load_payload_settings.cpp index 25aba39..064cac0 100644 --- a/src/examples/2._Load_Payload_Settings/load_payload_settings.cpp +++ b/src/examples/2._Load_Payload_Settings/load_payload_settings.cpp @@ -19,9 +19,6 @@ int main(int argc, char *argv[]){ printf("Starting LoadPayloadSettings example...\n"); signal(SIGINT,quit_handler); - // init thread to check receive message from payload - all_threads_init(); - // create payloadsdk object my_payload = new PayloadSdkInterface(); @@ -40,6 +37,9 @@ int main(int argc, char *argv[]){ } } + // init thread to check receive message from payload + all_threads_init(); + // request to read all settings of payload my_payload->getPayloadCameraSettingList(); diff --git a/src/examples/3._Set_Payload_Settings/set_payload_settings.cpp b/src/examples/3._Set_Payload_Settings/set_payload_settings.cpp index a86c4e3..b5c06ed 100644 --- a/src/examples/3._Set_Payload_Settings/set_payload_settings.cpp +++ b/src/examples/3._Set_Payload_Settings/set_payload_settings.cpp @@ -19,9 +19,6 @@ int main(int argc, char *argv[]){ printf("Starting SetPayloadSettings example...\n"); signal(SIGINT,quit_handler); - // init thread to check receive message from payload - all_threads_init(); - // create payloadsdk object my_payload = new PayloadSdkInterface(); @@ -40,12 +37,15 @@ int main(int argc, char *argv[]){ } } + // init thread to check receive message from payload + all_threads_init(); + // change setting of RC_MODE to DUAL my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_RC_MODE, PAYLOAD_CAMERA_RC_MODE_DUAL, PARAM_TYPE_UINT32); // change setting of OSD_MODE to STATUS to view Zoom factor my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_VIDEO_OSD_MODE, PAYLOAD_CAMERA_VIDEO_OSD_MODE_STATUS, PARAM_TYPE_UINT32); - + // request to read all settings of payload, then check the RC_MODE setting my_payload->getPayloadCameraSettingList(); @@ -53,15 +53,15 @@ int main(int argc, char *argv[]){ while(!time_to_exit){ // main loop, do some zooming commands if(is_zoom_in){ - my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_VIDEO_ZOOM_SUPER_RESOLUTION_VALUE, 10000, PARAM_TYPE_UINT32); - printf("Zoom in to 10000 \n"); + my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_VIDEO_ZOOM_SUPER_RESOLUTION_VALUE, 21846, PARAM_TYPE_UINT32); + printf("Zoom in to 21846 \n"); }else{ my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_VIDEO_ZOOM_SUPER_RESOLUTION_VALUE, 00000, PARAM_TYPE_UINT32); printf("Zoom out to 00000 \n"); } is_zoom_in = !is_zoom_in; - usleep(3000000); // sleep 3s + usleep(5000000); // sleep 5s } diff --git a/src/examples/4._Capture_Image/capture_image.cpp b/src/examples/4._Capture_Image/capture_image.cpp index 5cb4426..9b3eb3d 100644 --- a/src/examples/4._Capture_Image/capture_image.cpp +++ b/src/examples/4._Capture_Image/capture_image.cpp @@ -36,9 +36,6 @@ int main(int argc, char *argv[]){ printf("Starting CaptureImage example...\n"); signal(SIGINT,quit_handler); - // init thread to check receive message from payload - all_threads_init(); - // create payloadsdk object my_payload = new PayloadSdkInterface(); @@ -57,6 +54,9 @@ int main(int argc, char *argv[]){ } } + // init thread to check receive message from payload + all_threads_init(); + // set payload to video mode for testing my_payload->setPayloadCameraMode(CAMERA_MODE_VIDEO); diff --git a/src/examples/5._Record_Video/record_video.cpp b/src/examples/5._Record_Video/record_video.cpp index c057c9b..4be8288 100644 --- a/src/examples/5._Record_Video/record_video.cpp +++ b/src/examples/5._Record_Video/record_video.cpp @@ -38,9 +38,6 @@ int main(int argc, char *argv[]){ printf("Starting RecordVideo example...\n"); signal(SIGINT,quit_handler); - // init thread to check receive message from payload - all_threads_init(); - // create payloadsdk object my_payload = new PayloadSdkInterface(); @@ -59,6 +56,9 @@ int main(int argc, char *argv[]){ } } + // init thread to check receive message from payload + all_threads_init(); + // set payload to video mode for testing my_payload->setPayloadCameraMode(CAMERA_MODE_IMAGE); diff --git a/src/examples/6._Get_Video_Streaming/get_video_streaming.cpp b/src/examples/6._Get_Video_Streaming/get_video_streaming.cpp index b683f28..ff950ca 100644 --- a/src/examples/6._Get_Video_Streaming/get_video_streaming.cpp +++ b/src/examples/6._Get_Video_Streaming/get_video_streaming.cpp @@ -51,9 +51,6 @@ int main(int argc, char *argv[]){ printf("Starting GetStreaming example...\n"); signal(SIGINT,quit_handler); - // init thread to check receive message from payload - all_threads_init(); - // create payloadsdk object my_payload = new PayloadSdkInterface(); @@ -70,11 +67,16 @@ int main(int argc, char *argv[]){ printf("Payload connected! \n"); break; } + usleep(10000); } + // init thread to check receive message from payload + all_threads_init(); + // change setting of VIDEO_OUT to BOTH_HDMI_UDP printf("Change VideoOutput to both HDMI and Ethernet \n"); my_payload->setPayloadCameraParam(PAYLOAD_CAMERA_VIDEO_OUTPUT, PAYLOAD_CAMERA_VIDEO_OUTPUT_UDP, PARAM_TYPE_UINT32); + usleep(500000); my_job = check_camera_info; while(!time_to_exit){ @@ -145,32 +147,44 @@ void *payload_recv_handle(void *threadid) { // check payload messages while(!time_to_exit){ - if(my_payload != nullptr){ - mavlink_message_t msg; - uint8_t msg_cnt = my_payload->getNewMewssage(msg); - if(msg_cnt){ - printf("Got %d message in queue \n", msg_cnt); - printf(" --> message %d from system_id: %d with component_id: %d \n", msg.msgid, msg.sysid, msg.compid); - switch(msg.msgid){ - case MAVLINK_MSG_ID_COMMAND_ACK:{ - _handle_msg_command_ack(&msg); - break; - } - case MAVLINK_MSG_ID_CAMERA_INFORMATION:{ - _handle_msg_camera_information(&msg); - break; - } - case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:{ - _handle_msg_camera_stream_information(&msg); + #if 1 + try{ + if(my_payload != nullptr){ + mavlink_message_t msg; + uint8_t msg_cnt = my_payload->getNewMewssage(msg); + // printf("message cnt: %d \n", msg_cnt); + + if(msg_cnt){ + // printf("Got %d message in queue \n", msg_cnt); + // printf(" --> message %d from system_id: %d with component_id: %d \n", msg.msgid, msg.sysid, msg.compid); + switch(msg.msgid){ + case MAVLINK_MSG_ID_COMMAND_ACK:{ + _handle_msg_command_ack(&msg); + break; + } + case MAVLINK_MSG_ID_CAMERA_INFORMATION:{ + _handle_msg_camera_information(&msg); + break; + } + case MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:{ + _handle_msg_camera_stream_information(&msg); + break; + } + default: break; + } + }else{ + // printf("Recieved message queue empty. \n"); } - default: break; - } - }else{ + usleep(10000); + }else{ + printf("My_payload nullptr \n"); } - - usleep(10000); - } + }catch ( ... ) {}; + #else + printf("thread running \n"); + usleep(1000000); + #endif } pthread_exit(NULL); } @@ -205,6 +219,8 @@ void _handle_msg_camera_stream_information(mavlink_message_t* msg){ printf(" ---> Got streaming information: \n"); printf(" ---> Streaming type: %d \n", stream_info.type); + printf(" ---> Streaming resolution_v: %d \n", stream_info.resolution_v); + printf(" ---> Streaming resolution_h: %d \n", stream_info.resolution_h); printf(" ---> Streaming name: %s \n", stream_info.name); printf(" ---> Streaming uri: %s \n", stream_info.uri); @@ -236,7 +252,7 @@ void *start_loop_thread(void *threadid) string descr; if(!_is_rtsp_stream){ - descr = "udpsrc port=" + _stream_uri + " ! application/x-rtp,encoding-name=H264,payload=96 ! rtph264depay ! h264parse ! queue ! avdec_h264 ! nvvidconv ! nvoverlaysink" + descr = "udpsrc port=" + _stream_uri + " ! application/x-rtp,encoding-name=H264,payload=96 ! rtph264depay ! h264parse ! queue ! avdec_h264 ! nvvidconv ! nvoverlaysink sync=false async=false" ; }else{ descr = "rtspsrc location=" + _stream_uri + " latency=0 ! rtph264depay ! h264parse ! nvv4l2decoder ! nvoverlaysink sync=false async=false"; diff --git a/src/payloadSdkInterface.cpp b/src/payloadSdkInterface.cpp index 0787b33..33a1fcd 100644 --- a/src/payloadSdkInterface.cpp +++ b/src/payloadSdkInterface.cpp @@ -229,6 +229,9 @@ getPayloadCameraInformation(){ mavlink_msg_command_long_encode_chan(SYS_ID, COMP_ID, port->get_mav_channel(), &message, &msg); + message.sysid = SYS_ID; + message.compid = COMP_ID; + // -------------------------------------------------------------------------- // WRITE // -------------------------------------------------------------------------- diff --git a/src/third-party/mavlink/src/udp_port.cpp b/src/third-party/mavlink/src/udp_port.cpp index 6d0a46b..629b75b 100644 --- a/src/third-party/mavlink/src/udp_port.cpp +++ b/src/third-party/mavlink/src/udp_port.cpp @@ -56,7 +56,9 @@ #include "udp_port.h" - +#define UDP_SERVER 0 +#define UDP_CLIENT 1 +#define UDP_MODE UDP_SERVER // ---------------------------------------------------------------------------------- // UDP Port Manager Class // ---------------------------------------------------------------------------------- @@ -70,6 +72,9 @@ UDP_Port(const char *target_ip_, int udp_port_) initialize_defaults(); target_ip = target_ip_; rx_port = udp_port_; +#if (UDP_MODE == UDP_CLIENT) + tx_port = rx_port; +#endif is_open = false; } @@ -93,7 +98,9 @@ initialize_defaults() // Initialize attributes target_ip = "127.0.0.1"; rx_port = 14550; + tx_port = -1; + is_open = false; debug = false; sock = -1; @@ -111,84 +118,6 @@ initialize_defaults() // ------------------------------------------------------------------------------ // Read from UDP // ------------------------------------------------------------------------------ -#if 0 -int -UDP_Port:: -read_message(mavlink_message_t &message) -{ - uint8_t cp; - mavlink_status_t status; - uint8_t msgReceived = false; - - // -------------------------------------------------------------------------- - // READ FROM PORT - // -------------------------------------------------------------------------- - - // this function locks the port during read - int result = _read_port(cp); - - - // -------------------------------------------------------------------------- - // PARSE MESSAGE - // -------------------------------------------------------------------------- - if (result > 0) - { - // the parsing - msgReceived = mavlink_parse_char(MAVLINK_COMM_1, cp, &message, &status); - - // check for dropped packets - if ( (lastStatus.packet_rx_drop_count != status.packet_rx_drop_count) && debug ) - { - printf("ERROR: DROPPED %d PACKETS\n", status.packet_rx_drop_count); - unsigned char v=cp; - fprintf(stderr,"%02x ", v); - } - lastStatus = status; - } - - // Couldn't read from port - else - { - fprintf(stderr, "ERROR: Could not read, res = %d, errno = %d : %m\n", result, errno); - } - - // -------------------------------------------------------------------------- - // DEBUGGING REPORTS - // -------------------------------------------------------------------------- - if(msgReceived && debug) - { - // Report info - printf("Received message from UDP with ID #%d (sys:%d|comp:%d):\n", message.msgid, message.sysid, message.compid); - - fprintf(stderr,"Received UDP data: "); - unsigned int i; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - - // check message is write length - unsigned int messageLength = mavlink_msg_to_send_buffer(buffer, &message); - - // message length error - if (messageLength > MAVLINK_MAX_PACKET_LEN) - { - fprintf(stderr, "\nFATAL ERROR: MESSAGE LENGTH IS LARGER THAN BUFFER SIZE\n"); - } - - // print out the buffer - else - { - for (i=0; i &message) @@ -237,7 +166,6 @@ read_message(std::queue &message) return msgReceived; } -#endif // ------------------------------------------------------------------------------ // Write to UDP // ------------------------------------------------------------------------------ @@ -282,6 +210,8 @@ start() throw EXIT_FAILURE; } +#if (UDP_MODE == UDP_SERVER) + printf("\x1b[32m %s : %d \t Set UDP_Port as UDP_SERVER\r\n\x1b[0m",__func__,__LINE__); /* Bind the socket to rx_port - necessary to receive packets */ struct sockaddr_in addr; memset(&addr, 0, sizeof(addr)); @@ -300,19 +230,14 @@ start() sock = -1; throw EXIT_FAILURE; } - /*if (fcntl(sock, F_SETFL, O_NONBLOCK | O_ASYNC) < 0) - { - fprintf(stderr, "error setting nonblocking: %s\n", strerror(errno)); - close(sock); - sock = -1; - throw EXIT_FAILURE; - }*/ - // -------------------------------------------------------------------------- // CONNECTED! // -------------------------------------------------------------------------- - printf("Listening to %s:%i\n", target_ip, rx_port); + // printf("Listening to %s:%i\n", target_ip, rx_port); lastStatus.packet_rx_drop_count = 0; +#else + printf("\x1b[32m %s : %d \t Set UDP_Port as UDP_CLIENT\r\n\x1b[0m",__func__,__LINE__); +#endif is_open = true; @@ -372,19 +297,13 @@ _read_port(uint8_t &cp) // Lock pthread_mutex_lock(&lock); - - int result = -1; - // if(buff_ptr < buff_len){ - // cp=buff[buff_ptr]; - // buff_ptr++; - // result=1; - // }else{ + int result = -1; struct sockaddr_in addr; len = sizeof(struct sockaddr_in); result = recvfrom(sock, &buff, BUFF_LEN, 0, (struct sockaddr *)&addr, &len); // printf("%s %d \n", __func__, result); - + #if (UDP_MODE == UDP_SERVER) if(tx_port < 0){ if(strcmp(inet_ntoa(addr.sin_addr), target_ip) == 0){ tx_port = ntohs(addr.sin_port); @@ -393,14 +312,7 @@ _read_port(uint8_t &cp) printf("ERROR: Got packet from %s:%i but listening on %s\n", inet_ntoa(addr.sin_addr), ntohs(addr.sin_port), target_ip); } } - // if(result > 0){ - // buff_len=result; - // buff_ptr=0; - // cp=buff[buff_ptr]; - // buff_ptr++; - // //printf("recvfrom: %i %i\n", result, cp); - // } - // } + #endif // Unlock pthread_mutex_unlock(&lock); @@ -416,10 +328,6 @@ int UDP_Port:: _write_port(char *buf, unsigned len) { - - // Lock - // pthread_mutex_lock(&lock); - // Write packet via UDP link int bytesWritten = 0; if(tx_port > 0){ @@ -429,16 +337,11 @@ _write_port(char *buf, unsigned len) addr.sin_addr.s_addr = inet_addr(target_ip); addr.sin_port = htons(tx_port); bytesWritten = sendto(sock, buf, len, 0, (struct sockaddr*)&addr, sizeof(struct sockaddr_in)); - //printf("sendto: %i\n", bytesWritten); + // printf("sendto: %s \t %i\n",target_ip,tx_port); }else{ printf("ERROR: Sending before first packet received!\n"); bytesWritten = -1; } - - // Unlock - // pthread_mutex_unlock(&lock); - - return bytesWritten; }