diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/delay.cpp b/ground/src/experimental/USB_UPLOAD_TOOL/delay.cpp
new file mode 100644
index 000000000..b8bb87265
--- /dev/null
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/delay.cpp
@@ -0,0 +1,2 @@
+#include "delay.h"
+
diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/delay.h b/ground/src/experimental/USB_UPLOAD_TOOL/delay.h
new file mode 100644
index 000000000..ccf14ee5b
--- /dev/null
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/delay.h
@@ -0,0 +1,13 @@
+#ifndef DELAY_H
+#define DELAY_H
+#include <QThread>
+
+class delay : public QThread
+{
+public:
+    static void msleep(unsigned long msecs)
+    {
+        QThread::msleep(msecs);
+    }
+};
+#endif // DELAY_H
diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/main.cpp b/ground/src/experimental/USB_UPLOAD_TOOL/main.cpp
index 91bb00196..47c7c8982 100644
--- a/ground/src/experimental/USB_UPLOAD_TOOL/main.cpp
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/main.cpp
@@ -184,6 +184,11 @@ int main(int argc, char *argv[])
             cout<<"Could not enter DFU mode\n";
             return -1;
         }
+        if (debug)
+        {
+            OP_DFU::Status ret=dfu.StatusRequest();
+            qDebug()<<dfu.StatusToString(ret);
+        }
         if(!(action==OP_DFU::actionStatusReq || action==OP_DFU::actionReset || action== OP_DFU::actionJump))
         {
             dfu.findDevices();
@@ -202,6 +207,7 @@ int main(int argc, char *argv[])
                     cout<<"FW CRC="<<dfu.devices[x].FW_CRC<<"\n";
 
                     int size=((OP_DFU::device)dfu.devices[x]).SizeOfDesc;
+                    dfu.enterDFU(x);
                     cout<<"Description:"<<dfu.DownloadDescription(size).toLatin1().data()<<"\n";
                     cout<<"\n";
                 }
@@ -274,10 +280,10 @@ int main(int argc, char *argv[])
 
         return 0;
     }
-    //OP_DFU dfu(false);
+    OP_DFU dfu(true);
     //dfu.findDevices();
-
-    //dfu.UploadFirmware("c:/openpilot.bin",true,0);
+    dfu.enterDFU(1);
+    dfu.UploadFirmware("c:/ahrs.bin",true,1);
     // dfu.UploadDescription("josemanuel");
     //  QString str=dfu.DownloadDescription(12);
     // dfu.JumpToApp();
diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp b/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp
index c39f6482c..8cfc61ddd 100644
--- a/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.cpp
@@ -5,17 +5,14 @@
 
 OP_DFU::OP_DFU(bool _debug): debug(_debug)
 {
-    QWaitCondition sleep;
-    QMutex mutex;
+    send_delay=10;
     int numDevices=0;
     cout<<"Please connect device now\n";
     int count=0;
     while(numDevices==0)
     {
         cout<<".";
-        mutex.lock();
-        sleep.wait(&mutex,500);
-        mutex.unlock();
+        delay::msleep(500);
         numDevices = hidHandle.open(1,0x20a0,0x4117,0,0); //0xff9c,0x0001);
         if(++count==10)
         {
@@ -91,8 +88,8 @@ bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & typ
     buf[9] = crc>>16;
     buf[10] = crc>>8;
     buf[11] = crc;
-
-    qDebug()<<"Number of packets:"<<numberOfPackets<<" Size of last packet:"<<lastPacketCount;
+    if(debug)
+        qDebug()<<"Number of packets:"<<numberOfPackets<<" Size of last packet:"<<lastPacketCount;
     int result = hidHandle.send(0,buf, BUF_LEN, 5000);
     if(debug)
         qDebug() << result << " bytes sent";
@@ -104,7 +101,6 @@ bool OP_DFU::StartUpload(qint32 const & numberOfBytes, TransferTypes const & typ
 }
 bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray  & data)
 {
-    qDebug()<<(int)data[0];
     int lastPacketCount;
     qint32 numberOfPackets=numberOfBytes/4/14;
     int pad=(numberOfBytes-numberOfPackets*4*14)/4;
@@ -135,6 +131,7 @@ bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray  & data)
             packetsize=lastPacketCount;
         else
             packetsize=14;
+       // qDebug()<<packetcount;
         buf[2] = packetcount>>24;//DFU Count
         buf[3] = packetcount>>16;//DFU Count
         buf[4] = packetcount>>8;//DFU Count
@@ -151,8 +148,9 @@ bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray  & data)
 
         //        }
         // qDebug()<<" Data0="<<(int)data[0]<<" Data0="<<(int)data[1]<<" Data0="<<(int)data[2]<<" Data0="<<(int)data[3]<<" buf6="<<(int)buf[6]<<" buf7="<<(int)buf[7]<<" buf8="<<(int)buf[8]<<" buf9="<<(int)buf[9];
-
+        delay::msleep(send_delay);
         int result = hidHandle.send(0,buf, BUF_LEN, 5000);
+     //   qDebug()<<"sent:"<<result;
         if(result<1)
         {
             return false;
@@ -162,10 +160,13 @@ bool OP_DFU::UploadData(qint32 const & numberOfBytes, QByteArray  & data)
 
     }
     cout<<"\n";
+    // while(true){}
     return true;
 }
 OP_DFU::Status OP_DFU::UploadDescription(QString  & description)
 {
+    if(debug)
+        qDebug()<<"Starting uploading description";
     if(description.length()%4!=0)
     {      
         int pad=description.length()/4;
@@ -446,6 +447,7 @@ bool OP_DFU::EndOperation()
 }
 OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify,int device)
 {
+    OP_DFU::Status ret;
     cout<<"Starting Firmware Uploading...\n";
     QFile file(sfile);
     //QFile file("in.txt");
@@ -467,42 +469,63 @@ OP_DFU::Status OP_DFU::UploadFirmware(const QString &sfile, const bool &verify,i
         pad=pad-arr.length();
         arr.append(QByteArray(pad,255));
     }
+    if(devices[device].SizeOfCode<arr.length())
+    {
+        cout<<"ERROR file to big for device\n";
+        return OP_DFU::abort;;
+    }
     quint32 crc=CRCFromQBArray(arr,devices[device].SizeOfCode);
+    cout<<"NEW FIRMWARE CRC="<<crc<<"\n";
     if(!StartUpload(arr.length(),FW,crc))
     {
         if(debug)
+        {
             qDebug()<<"StartUpload failed";
+            OP_DFU::Status ret=StatusRequest();
+            qDebug()<<"StartUpload returned:"<< StatusToString(ret);
+        }
         return OP_DFU::abort;
     }
     cout<<"Erasing memory";
+    delay::msleep(3000);
     for(int x=0;x<3;++x)
     {
         OP_DFU::Status ret=StatusRequest();
+        qDebug()<<"Erase returned:"<<StatusToString(ret);
         if (ret==OP_DFU::uploading)
-               break;
+            break;
         else
-             return OP_DFU::abort;
+            return OP_DFU::abort;
     }
     if(!UploadData(arr.length(),arr))
     {
         if(debug)
-            qDebug()<<"Upload failed";
+        {
+            qDebug()<<"Upload failed (upload data)";
+            OP_DFU::Status ret=StatusRequest();
+            qDebug()<<"StartUpload returned:"<<StatusToString(ret);
+        }
         return OP_DFU::abort;
     }
     if(!EndOperation())
     {
         if(debug)
-            qDebug()<<"Upload failed";
+        {
+            qDebug()<<"Upload failed (end operation)";
+            OP_DFU::Status ret=StatusRequest();
+            qDebug()<<"StartUpload returned:"<<StatusToString(ret);
+        }
         return OP_DFU::abort;
     }
-    OP_DFU::Status ret=StatusRequest();
+    ret=StatusRequest();
+  //  qDebug()<<"---------------------------"<<StatusToString(ret);
     if(ret==OP_DFU::Last_operation_Success)
     {
 
     }
     else
     {
-        return OP_DFU::abort;
+        return ret;
     }
     if(verify)
     {
@@ -614,6 +637,8 @@ QString OP_DFU::StatusToString(OP_DFU::Status const & status)
         return "Jmp to user FW failed";
     case abort:
         return "abort";
+    case uploadingStarting:
+        return "Uploading Starting";
     default:
         return "unknown";
 
diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.h b/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.h
index ccfc228cf..15867cf6a 100644
--- a/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.h
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/op_dfu.h
@@ -8,6 +8,7 @@
  #include <QCryptographicHash>
 #include <QList>
 #include <iostream>
+#include "delay.h"
 using namespace std;
 #define BUF_LEN 64
 
@@ -41,19 +42,20 @@ public:
 
     enum Status
     {
-        DFUidle,
-        uploading,
-        wrong_packet_received,
-        too_many_packets,
-        too_few_packets,
-        Last_operation_Success,
-        downloading,
-        idle,
-        Last_operation_failed,
-        outsideDevCapabilities,
-        CRC_Fail,
-        failed_jump,
-        abort
+        DFUidle,//0
+        uploading,//1
+        wrong_packet_received,//2
+        too_many_packets,//3
+        too_few_packets,//4
+        Last_operation_Success,//5
+        downloading,//6
+        idle,//7
+        Last_operation_failed,//8
+        uploadingStarting,//9
+        outsideDevCapabilities,//10
+        CRC_Fail,//11
+        failed_jump,//12
+        abort//13
 
     };
     enum Actions
@@ -121,6 +123,7 @@ public:
     quint32 CRC32WideFast(quint32 Crc, quint32 Size, quint32 *Buffer);
     quint32 CRCFromQBArray(QByteArray array, quint32 Size);
     void test();
+    int send_delay;
 private:
     bool debug;
     int RWFlags;
diff --git a/ground/src/experimental/USB_UPLOAD_TOOL/upload.pro b/ground/src/experimental/USB_UPLOAD_TOOL/upload.pro
index b89b2dbd1..f36bf6334 100644
--- a/ground/src/experimental/USB_UPLOAD_TOOL/upload.pro
+++ b/ground/src/experimental/USB_UPLOAD_TOOL/upload.pro
@@ -15,9 +15,11 @@ CONFIG   -= app_bundle
 TEMPLATE = app
 
 SOURCES += main.cpp \
-    op_dfu.cpp
+    op_dfu.cpp \
+    delay.cpp
 HEADERS += ../../plugins/rawhid/pjrc_rawhid.h \
-    op_dfu.h
+    op_dfu.h \
+    delay.h
 
 win32 {
     SOURCES += ../../plugins/rawhid/pjrc_rawhid_win.cpp