Skip to content
Browse files

Emergency fix to create_stop

  • Loading branch information...
1 parent ee558d6 commit 070cef9909c89a5c863e6ae77a012e62cb0fd379 @bmcdorman bmcdorman committed Mar 9, 2013
Showing with 217 additions and 50 deletions.
  1. +1 −1 include/kovan/ardrone.hpp
  2. +5 −1 include/kovan/create.hpp
  3. +127 −44 src/ardrone.cpp
  4. +79 −4 src/ardrone_constants_p.hpp
  5. +5 −0 test/ardrone/ardrone.cpp
View
2 include/kovan/ardrone.hpp
@@ -30,7 +30,7 @@ class ARDrone
{
Disconnected = 0,
Landed,
- TakenOff
+ Flying
};
~ARDrone();
View
6 include/kovan/create.hpp
@@ -298,7 +298,11 @@ class EXPORT_SYM Create
void drive(const short& velocity, const short& radius);
void driveDirect(const short& left, const short& right);
inline void driveStraight(const short& speed) { driveDirect(speed, speed); }
- inline void stop() { driveStraight(0); }
+ inline void stop()
+ {
+ flush();
+ driveStraight(0);
+ }
/*!
* A very accurate turn method based on time rather than the create's own readings, which are often less than accurate.
View
171 src/ardrone.cpp
@@ -29,7 +29,7 @@ struct navdata_option_t
// set this to the size of this structure
uint16_t size;
- uint8_t data[1];
+ uint8_t data[];
} __attribute__((packed));
struct navdata_t {
@@ -41,9 +41,9 @@ struct navdata_t {
// Sequence number, incremented for each sent packet
uint32_t sequence;
- bool vision_defined;
+ uint32_t vision_defined;
- navdata_option_t options[1];
+ navdata_option_t options[];
} __attribute__((packed));
struct navdata_demo_t
@@ -100,7 +100,7 @@ DroneSequencer::DroneSequencer()
void DroneSequencer::wait(const unsigned times)
{
sequence_t current = m_current;
- while(current + times < m_current) msleep(10);
+ while(current + times >= m_current) msleep(10);
}
DroneSequencer::sequence_t DroneSequencer::next()
@@ -149,15 +149,22 @@ class DroneController : public Thread
void stop();
bool isStopped() const;
- void control(const unsigned param);
+ void control();
+ void sendMagic();
void setOwner(const char *const mac);
- void resetWatchdog();
+ bool isControlWatchdog() const;
+ bool isLowBatt() const;
+
+
+ void resetControlWatchdog();
+ void sendCommWatchdog();
std::string currentConfig();
- navdata_demo_t latestNavdata() const;
+ const navdata_t *latestNavdata() const;
+ bool writeNavdata(const char *const path) const;
private:
void pushCommand(const char *const command);
@@ -168,17 +175,22 @@ class DroneController : public Thread
bool update();
bool fetchNavdata();
+ void __enumerateOptions();
+
sockaddr_in m_addr;
int m_fd;
Mutex m_mutex;
+ Mutex m_control;
std::stack<DroneCommand> m_commandStack;
DroneSequencer m_seq;
bool m_stop;
+ unsigned char m_navdata[65507];
};
DroneController::DroneController()
: m_fd(-1)
{
+ memset(m_navdata, 0, sizeof(m_navdata));
}
DroneController::~DroneController()
@@ -240,18 +252,14 @@ void DroneController::move(const float x, const float y, const float z, const fl
bool DroneController::requestNavdata()
{
- sockaddr_in si_other;
- memset(&si_other, 0, sizeof(si_other));
- si_other.sin_family = AF_INET;
- si_other.sin_port = htons(ARDRONE_AT_PORT);
- if(inet_aton("192.168.1.1", &si_other.sin_addr) <= 0) {
- perror("inet_aton");
- return false;
- }
+ if(m_fd < 0) return true;
- const char *const dummy = "braden";
- if(sendto(m_fd, dummy, strlen(dummy), 0,
- (const sockaddr *)&si_other, sizeof(si_other)) < 0) {
+ sockaddr_in navport = m_addr;
+ navport.sin_port = htons(ARDRONE_NAVDATA_PORT);
+
+ const static char dummy[4] = { 0x01, 0x00, 0x00, 0x00 };
+ if(sendto(m_fd, dummy, sizeof(dummy), 0,
+ (const sockaddr *)&navport, sizeof(navport)) != sizeof(dummy)) {
perror("DroneController::requestNavdata -> sendto");
return false;
}
@@ -262,20 +270,31 @@ bool DroneController::requestNavdata()
void DroneController::config(const char *const cmd, const char *const value)
{
char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u,\"%s\",\"%s\",\"%s\"\r", ARDRONE_AT_CONFIG_IDS,
+ ARDRONE_SESSION_ID,ARDRONE_USER_ID, ARDRONE_APPLICATION_ID);
+ oneTimeCommand(command);
+
sprintf(command, "%s%%u,\"%s\",\"%s\"\r", ARDRONE_AT_CONFIG, cmd, value);
- printf("sending command: %s\n", command);
oneTimeCommand(command);
}
void DroneController::run()
{
unsigned long ticks = 0;
while(!m_stop) {
+ if(ticks % 100 == 0) {
+ // sendCommWatchdog();
+ requestNavdata();
+ }
m_mutex.lock();
bool success = true;
- if(++ticks % 11 == 0) success &= update();
+
+ // Slower update sub-loop
+ if(++ticks % 11 == 0) {
+ __enumerateOptions();
+ success &= update();
+ }
success &= fetchNavdata();
- requestNavdata();
m_mutex.unlock();
if(!success) break;
msleep(3);
@@ -296,19 +315,51 @@ bool DroneController::isStopped() const
return m_stop;
}
-void DroneController::control(const unsigned param)
+void DroneController::control()
{
char command[ARDRONE_MAX_CMD_LENGTH];
- sprintf(command, "%s%%u,%u,%u\r", ARDRONE_AT_CTRL, param, 0);
+ sprintf(command, "%s%%u,%u\r", ARDRONE_AT_CTRL, 0);
oneTimeCommand(command);
}
+void DroneController::sendMagic()
+{
+ char command[ARDRONE_MAX_CMD_LENGTH];
+ sprintf(command, "%s%%u,%u\r", ARDRONE_AT_PMODE, 2);
+ oneTimeCommand(command);
+ msleep(100);
+ sprintf(command, "%s%%u,%u,%u,%u,%u\r", ARDRONE_AT_MISC, 2, 20, 2000, 3000);
+ oneTimeCommand(command);
+ msleep(100);
+}
+
void DroneController::setOwner(const char *const mac)
{
config(ARDRONE_NETWORK_OWNER_MAC, mac);
}
-void DroneController::resetWatchdog()
+bool DroneController::isControlWatchdog() const
+{
+ const navdata_t *const latest = latestNavdata();
+ if(!latest) return false;
+
+ return latest->ardrone_state & ARDRONE_CTRL_WATCHDOG_MASK;
+}
+
+bool DroneController::isLowBatt() const
+{
+ const navdata_t *const latest = latestNavdata();
+ if(!latest) return false;
+
+ return latest->ardrone_state & ARDRONE_VBAT_LOW;
+}
+
+void DroneController::resetControlWatchdog()
+{
+ // control(ARDRONE_ACK_CONTROL_MODE);
+}
+
+void DroneController::sendCommWatchdog()
{
char command[ARDRONE_MAX_CMD_LENGTH];
sprintf(command, "%s%%u\r", ARDRONE_AT_COMWDG);
@@ -351,9 +402,20 @@ std::string DroneController::currentConfig()
return ret;
}
-navdata_demo_t DroneController::latestNavdata() const
+const navdata_t *DroneController::latestNavdata() const
{
- return navdata_demo_t();
+ const navdata_t *const ret = reinterpret_cast<const navdata_t *>(m_navdata);
+ if(!ret || ret->header != ARDRONE_NAVDATA_HEADER) return 0;
+ return ret;
+}
+
+bool DroneController::writeNavdata(const char *const path) const
+{
+ FILE *fp = fopen(path, "w");
+ if(!fp) return false;
+ fwrite(m_navdata, 1, sizeof(m_navdata), fp);
+ fclose(fp);
+ return true;
}
void DroneController::pushCommand(const char *const command)
@@ -376,35 +438,49 @@ void DroneController::popCommand()
void DroneController::oneTimeCommand(const char *const command)
{
pushCommand(command);
- m_seq.wait(10);
+ m_seq.wait(2);
popCommand();
}
bool DroneController::fetchNavdata()
{
if(m_fd < 0) return true;
- char data[512];
ssize_t readLength = 0;
- if((readLength = recv(m_fd, &data, 512, 0)) < 0 && errno != EAGAIN) {
+ char data[sizeof(m_navdata)];
+ if((readLength = recv(m_fd, data, sizeof(data), 0)) < 0 && errno != EAGAIN) {
perror("recvfrom");
return false;
}
- std::cout << "Read length: " << readLength << std::endl;
- if(readLength > 0) {
- std::cout << "Read length: " << readLength << " navdata_t size: " << sizeof(navdata_t) << std::endl;
- std::cout << data << std::endl;
- }
+ if(readLength < 0) return true;
+ std::cout << "readLength: " << readLength << std::endl;
+
+ memcpy(m_navdata, data, sizeof(m_navdata));
return true;
}
+void DroneController::__enumerateOptions()
+{
+ const navdata_t *const navdata = latestNavdata();
+ if(!navdata || navdata->header != ARDRONE_NAVDATA_HEADER) return;
+
+ const navdata_option_t *option = navdata->options;
+ // std::cout << "Got tag: " << (int)option->tag << std::endl;
+ // std::cout << "\tsize: " << (int)option->size << " (navdata_demo_t size: " << sizeof(navdata_demo_t) << ")" << std::endl;
+ for(int i = 0; i < 3; option += option->size, ++i) {
+ std::cout << "Got tag: " << (int)option->tag << std::endl;
+ std::cout << "\tsize: " << (int)option->size << " (navdata_demo_t size: " << sizeof(navdata_demo_t) << ")" << std::endl;
+ }
+}
+
bool DroneController::update()
{
// Nothing to do.
if(m_commandStack.empty() || m_fd < 0) return true;
char realCommand[ARDRONE_MAX_CMD_LENGTH];
sprintf(realCommand, m_commandStack.top().data, m_seq.next());
+
printf("Sending %s\n", realCommand);
if(sendto(m_fd, realCommand, strlen(realCommand), 0,
(const sockaddr *)&m_addr, sizeof(m_addr)) < 0) {
@@ -429,6 +505,7 @@ bool ARDrone::connect(const char *ip)
fcntl(m_fd, F_SETFL, flags | O_NONBLOCK);
sockaddr_in bindAddr;
+ memset(&bindAddr, 0, sizeof(bindAddr));
bindAddr.sin_family = AF_INET;
bindAddr.sin_addr.s_addr = htonl(INADDR_ANY);
bindAddr.sin_port = htons(ARDRONE_NAVDATA_PORT);
@@ -438,6 +515,9 @@ bool ARDrone::connect(const char *ip)
return false;
}
+ const int yes = 1;
+ setsockopt(m_fd, SOL_SOCKET, SO_REUSEADDR, &yes, sizeof(yes));
+
sockaddr_in si_other;
memset(&si_other, 0, sizeof(si_other));
si_other.sin_family = AF_INET;
@@ -449,19 +529,13 @@ bool ARDrone::connect(const char *ip)
}
m_controller->setup(si_other, m_fd);
-
- // m_controller->resetWatchdog();
-
- m_controller->requestNavdata();
- // m_controller->config(ARDRONE_NAVDATA_DEMO, ARDRONE_TRUE);
- // m_controller->control(ARDRONE_ACK_CONTROL_MODE);
+ m_controller->sendMagic();
+ m_controller->config(ARDRONE_NAVDATA_DEMO, ARDRONE_TRUE);
+ m_controller->control();
// Set ourself as the owner
// m_controller->config("network:ssid_single_player","myArdroneNetwork");
- // m_controller->control(ARDRONE_ACK_CONTROL_MODE);
// m_controller->setOwner("14:10:9f:e3:6c:fd");
- // m_controller->control(ARDRONE_ACK_CONTROL_MODE);
-
// std::cout << m_controller->currentConfig() << std::endl;
@@ -510,7 +584,16 @@ void ARDrone::move(const float x, const float y, const float z, const float yaw)
ARDrone::State ARDrone::state() const
{
// TODO: Stub
- return ARDrone::Disconnected;
+ const navdata_t *const navdata = m_controller->latestNavdata();
+ if(!navdata) return ARDrone::Disconnected;
+
+ static int i = 0;
+ char buffer[32];
+ sprintf(buffer, "navdata_%d.bin", i++);
+ m_controller->writeNavdata(buffer);
+
+ if(navdata->ardrone_state & ARDRONE_FLY_MASK) return ARDrone::Flying;
+ return ARDrone::Landed;
}
ARDrone *ARDrone::instance()
View
83 src/ardrone_constants_p.hpp
@@ -8,10 +8,13 @@
#define ARDRONE_AT_PCMD (ARDRONE_AT_PREFIX "PCMD" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_FTRIM (ARDRONE_AT_PREFIX "FTRIM" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_CONFIG (ARDRONE_AT_PREFIX "CONFIG" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_CONFIG_IDS (ARDRONE_AT_PREFIX "CONFIG_IDS" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_LED (ARDRONE_AT_PREFIX "LED" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_ANIM (ARDRONE_AT_PREFIX "ANIM" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_COMWDG (ARDRONE_AT_PREFIX "COMWDG" ARDRONE_AT_SUFFIX)
#define ARDRONE_AT_CTRL (ARDRONE_AT_PREFIX "CTRL" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_PMODE (ARDRONE_AT_PREFIX "PMODE" ARDRONE_AT_SUFFIX)
+#define ARDRONE_AT_MISC (ARDRONE_AT_PREFIX "MISC" ARDRONE_AT_SUFFIX)
#define ARDRONE_CMD_SEPARATOR ((char)10)
#define ARDRONE_MAX_CMD_LENGTH (1024)
@@ -21,15 +24,87 @@
#define ARDRONE_NAVDATA_HEADER (0x55667788)
-#define ARDRONE_TRUE "TRUE"
-#define ARDRONE_FALSE "FALSE"
+#define ARDRONE_TRUE ("TRUE")
+#define ARDRONE_FALSE ("FALSE")
#define ARDRONE_CFG_GET_CONTROL_MODE (4)
#define ARDRONE_ACK_CONTROL_MODE (5)
-#define ARDRONE_NAVDATA_DEMO "general:navdata_demo"
-#define ARDRONE_NETWORK_OWNER_MAC "network:owner_mac"
+#define ARDRONE_NAVDATA_DEMO ("general:navdata_demo")
+#define ARDRONE_NETWORK_OWNER_MAC ("network:owner_mac")
+
+#define ARDRONE_NAVDATA_DEMO_TAG (1)
+#define ARDRONE_NAVDATA_TIME_TAG (2)
+#define ARDRONE_NAVDATA_RAW_MEASURES_TAG (3)
+#define ARDRONE_NAVDATA_PHYS_MEASURES_TAG (4)
+#define ARDRONE_NAVDATA_GYROS_OFFSETS_TAG (5)
+#define ARDRONE_NAVDATA_EULER_ANGLES_TAG (6)
+#define ARDRONE_NAVDATA_REFERENCES_TAG (7)
+#define ARDRONE_NAVDATA_TRIMS_TAG (8)
+#define ARDRONE_NAVDATA_RC_REFERENCES_TAG (9)
+#define ARDRONE_NAVDATA_PWM_TAG (10)
+#define ARDRONE_NAVDATA_ALTITUDE_TAG (11)
+#define ARDRONE_NAVDATA_VISION_RAW_TAG (12)
+#define ARDRONE_NAVDATA_VISION_OF_TAG (13)
+#define ARDRONE_NAVDATA_VISION_TAG (14)
+#define ARDRONE_NAVDATA_VISION_PERF_TAG (15)
+#define ARDRONE_NAVDATA_TRACKERS_SEND_TAG (16)
+#define ARDRONE_NAVDATA_VISION_DETECT_TAG (17)
+#define ARDRONE_NAVDATA_WATCHDOG_TAG (18)
+#define ARDRONE_NAVDATA_ADC_DATA_FRAME_TAG (19)
+#define ARDRONE_NAVDATA_VIDEO_STREAM_TAG (20)
+#define ARDRONE_NAVDATA_GAMES_TAG (21)
+#define ARDRONE_NAVDATA_PRESSURE_RAW_TAG (22)
+#define ARDRONE_NAVDATA_MAGNETO_TAG (23)
+#define ARDRONE_NAVDATA_WIND_TAG (24)
+#define ARDRONE_NAVDATA_KALMAN_PRESSURE_TAG (25)
+#define ARDRONE_NAVDATA_HDVIDEO_STREAM_TAG (26)
+#define ARDRONE_NAVDATA_WIFI_TAG (27)
+#define ARDRONE_NAVDATA_CKS_TAG (0xFFFF)
+
+typedef enum {
+ ARDRONE_FLY_MASK = 1U << 0, /*!< FLY MASK : (0) ardrone is landed, (1) ardrone is flying */
+ ARDRONE_VIDEO_MASK = 1U << 1, /*!< VIDEO MASK : (0) video disable, (1) video enable */
+ ARDRONE_VISION_MASK = 1U << 2, /*!< VISION MASK : (0) vision disable, (1) vision enable */
+ ARDRONE_CONTROL_MASK = 1U << 3, /*!< CONTROL ALGO : (0) euler angles control, (1) angular speed control */
+ ARDRONE_ALTITUDE_MASK = 1U << 4, /*!< ALTITUDE CONTROL ALGO : (0) altitude control inactive (1) altitude control active */
+ ARDRONE_USER_FEEDBACK_START = 1U << 5, /*!< USER feedback : Start button state */
+ ARDRONE_COMMAND_MASK = 1U << 6, /*!< Control command ACK : (0) None, (1) one received */
+ ARDRONE_CAMERA_MASK = 1U << 7, /*!< CAMERA MASK : (0) camera not ready, (1) Camera ready */
+ ARDRONE_TRAVELLING_MASK = 1U << 8, /*!< Travelling mask : (0) disable, (1) enable */
+ ARDRONE_USB_MASK = 1U << 9, /*!< USB key : (0) usb key not ready, (1) usb key ready */
+ ARDRONE_NAVDATA_DEMO_MASK = 1U << 10, /*!< Navdata demo : (0) All navdata, (1) only navdata demo */
+ ARDRONE_NAVDATA_BOOTSTRAP = 1U << 11, /*!< Navdata bootstrap : (0) options sent in all or demo mode, (1) no navdata options sent */
+ ARDRONE_MOTORS_MASK = 1U << 12, /*!< Motors status : (0) Ok, (1) Motors problem */
+ ARDRONE_COM_LOST_MASK = 1U << 13, /*!< Communication Lost : (1) com problem, (0) Com is ok */
+ ARDRONE_SOFTWARE_FAULT = 1U << 14, /*!< Software fault detected - user should land as quick as possible (1) */
+ ARDRONE_VBAT_LOW = 1U << 15, /*!< VBat low : (1) too low, (0) Ok */
+ ARDRONE_USER_EL = 1U << 16, /*!< User Emergency Landing : (1) User EL is ON, (0) User EL is OFF*/
+ ARDRONE_TIMER_ELAPSED = 1U << 17, /*!< Timer elapsed : (1) elapsed, (0) not elapsed */
+ ARDRONE_MAGNETO_NEEDS_CALIB = 1U << 18, /*!< Magnetometer calibration state : (0) Ok, no calibration needed, (1) not ok, calibration needed */
+ ARDRONE_ANGLES_OUT_OF_RANGE = 1U << 19, /*!< Angles : (0) Ok, (1) out of range */
+ ARDRONE_WIND_MASK = 1U << 20, /*!< WIND MASK: (0) ok, (1) Too much wind */
+ ARDRONE_ULTRASOUND_MASK = 1U << 21, /*!< Ultrasonic sensor : (0) Ok, (1) deaf */
+ ARDRONE_CUTOUT_MASK = 1U << 22, /*!< Cutout system detection : (0) Not detected, (1) detected */
+ ARDRONE_PIC_VERSION_MASK = 1U << 23, /*!< PIC Version number OK : (0) a bad version number, (1) version number is OK */
+ ARDRONE_ATCODEC_THREAD_ON = 1U << 24, /*!< ATCodec thread ON : (0) thread OFF (1) thread ON */
+ ARDRONE_NAVDATA_THREAD_ON = 1U << 25, /*!< Navdata thread ON : (0) thread OFF (1) thread ON */
+ ARDRONE_VIDEO_THREAD_ON = 1U << 26, /*!< Video thread ON : (0) thread OFF (1) thread ON */
+ ARDRONE_ACQ_THREAD_ON = 1U << 27, /*!< Acquisition thread ON : (0) thread OFF (1) thread ON */
+ ARDRONE_CTRL_WATCHDOG_MASK = 1U << 28, /*!< CTRL watchdog : (1) delay in control execution (> 5ms), (0) control is well scheduled */
+ ARDRONE_ADC_WATCHDOG_MASK = 1U << 29, /*!< ADC Watchdog : (1) delay in uart2 dsr (> 5ms), (0) uart2 is good */
+ ARDRONE_COM_WATCHDOG_MASK = 1U << 30, /*!< Communication Watchdog : (1) com problem, (0) Com is ok */
+#if defined(__ARMCC_VERSION)
+ ARDRONE_EMERGENCY_MASK = (int)0x80000000 /*!< Emergency landing : (0) no emergency, (1) emergency */
+#else
+ ARDRONE_EMERGENCY_MASK = 1U << 31 /*!< Emergency landing : (0) no emergency, (1) emergency */
+#endif
+} def_ardrone_state_mask_t;
#define ARDRONE_NAVDATA_PORT (5554)
+#define ARDRONE_APPLICATION_ID ("d87f7e0c")
+#define ARDRONE_USER_ID ("be27e2e4")
+#define ARDRONE_SESSION_ID ("d2e081a3")
+
#endif
View
5 test/ardrone/ardrone.cpp
@@ -1,5 +1,6 @@
#include "kovan/kovan.hpp"
#include <cstdlib>
+#include <iostream>
int main(int argc, char *argv[])
{
@@ -11,9 +12,13 @@ int main(int argc, char *argv[])
drone->flatTrim();
msleep(1000);
+ std::cout << "flying? " << (drone->state() == ARDrone::Flying) << std::endl;
+
drone->takeoff();
msleep(5000);
+ std::cout << "flying? " << (drone->state() == ARDrone::Flying) << std::endl;
+
// drone->move(0.0f, 0.0f, 0.0f, 1.0f);
// msleep(10000);

0 comments on commit 070cef9

Please sign in to comment.
Something went wrong with that request. Please try again.