Permalink
Browse files

Added perror message to setupSocket

  • Loading branch information...
1 parent c1a498b commit 0cd04b06798afd076b931a6443032ebb3095204e @bmcdorman bmcdorman committed Mar 19, 2013
Showing with 35 additions and 8 deletions.
  1. +35 −8 src/ardrone.cpp
View
@@ -41,6 +41,8 @@
#include <stdint.h>
+#define ARDRONE_DEBUG
+
struct navdata_option_t
{
// Navdata block ('option') identifier
@@ -598,23 +600,40 @@ bool DroneController::fetchNavdata()
bool DroneController::fetchVideo()
{
- if(!m_videoSocket.isOpen()) return true;
+ if(!m_videoSocket.isOpen()) {
+#ifdef ARDRONE_DEBUG
+ std::cout << "Video socket is not open. fetchVideo will silently fail." << std::endl;
+#endif
+ return true;
+ }
ssize_t readLength = 0;
unsigned char data[UVLC_MAX_SIZE];
if((readLength = m_videoSocket.recv(data, sizeof(data))) < 0 && errno != EAGAIN) {
perror("recvfrom");
return false;
}
- if(readLength < 0) return true;
+ if(readLength < 0) {
+#ifdef ARDRONE_DEBUG
+ std::cout << "Didn't read any data from video stream." << std::endl;
+#endif
+ return true;
+ }
+#ifdef ARDRONE_DEBUG
std::cout << "Read " << readLength << " bytes from video stream" << std::endl;
+#endif
Private::UvlcVideoDecoder().decode(data, readLength, m_image);
return true;
}
bool DroneController::wakeupStream(Socket &socket, const Address &address)
{
- if(!socket.isOpen()) return false;
+ if(!socket.isOpen()) {
+#ifdef ARDRONE_DEBUG
+ std::cout << "Failed to wakeup stream using invalid socket." << std::endl;
+#endif
+ return false;
+ }
const static char dummy[4] = { 0x01, 0x00, 0x00, 0x00 };
if(socket.sendto(dummy, sizeof(dummy), address) != sizeof(dummy)) {
@@ -649,7 +668,12 @@ void DroneController::__enumerateOptions()
bool DroneController::sendCurrentCommand()
{
// Nothing to do.
- if(m_commandStack.empty() || !m_atSocket.isOpen()) return true;
+ if(m_commandStack.empty() || !m_atSocket.isOpen()) {
+#ifdef ARDRONE_DEBUG
+ std::cout << "Waiting on valid socket and non-empty command stack" << std::endl;
+#endif
+ return true;
+ }
char realCommand[ARDRONE_MAX_CMD_LENGTH];
sprintf(realCommand, m_commandStack.top().data, m_seq.next());
@@ -670,10 +694,13 @@ bool DroneController::setupSocket(Socket &socket, const unsigned short bindTo) c
socket = Socket::udp();
bool success = true;
- success &= socket.setBlocking(false);
- success &= socket.setReusable(true);
- if(bindTo) success &= socket.bind(bindTo);
- if(!success) socket.close();
+ if(success) success &= socket.setBlocking(false);
+ if(success) success &= socket.setReusable(true);
+ if(success && bindTo) success &= socket.bind(bindTo);
+ if(!success) {
+ perror("setupSocket");
+ socket.close();
+ }
return success;
}

0 comments on commit 0cd04b0

Please sign in to comment.