changeset 267:bf47051f7919 main

PixyCam control software.
author Bob Cook <bob@bobcookdev.com>
date Sun, 01 May 2016 14:22:14 -0700
parents 3aeab6193316
children 5a43fb1075a1 3b91c5d76887
files main/robots/odr/PixyApp.cpp main/robots/odr/PixyApp.h main/robots/odr/PixyReader.cpp main/robots/odr/PixyReader.h main/robots/odr/pixy_ubuntu.xml
diffstat 5 files changed, 439 insertions(+), 30 deletions(-) [+]
line wrap: on
line diff
--- a/main/robots/odr/PixyApp.cpp	Sun May 01 14:13:52 2016 -0700
+++ b/main/robots/odr/PixyApp.cpp	Sun May 01 14:22:14 2016 -0700
@@ -56,6 +56,28 @@
 
 // ----------------------------------------------------------------------------------------
 
+// incoming messages: start tracking, stop tracking
+// outgoing messages: heartbeat, track info
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::sendHeartbeat( Poco::Timer& timer )
+{
+    // if the PixyCam is not online then we don't send any messages
+
+    if ( ! PixyReader::isPixyAlive() )
+    {
+        return;
+    }
+
+    uint32_t heartbeatmsgid = can_build_message_id(
+                    can_node_sensor_pixy, can_node_broadcast, can_dataid_heartbeat );
+
+    CANMessage::QueueToSend( new CANMessage( heartbeatmsgid ) );
+}
+
+// ----------------------------------------------------------------------------------------
+
 void PixyApp::sendPixyDataUpdate( Poco::Timer& timer )
 {
     // if the PixyCam is not online then we don't send any messages
@@ -82,9 +104,190 @@
 
 // ----------------------------------------------------------------------------------------
 
+void PixyApp::recvSetPixyTracking( bool enabled )
+{
+    if ( enabled )
+    {
+        m_timerSendPixyDataUpdate.start(
+            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );
+    }
+    else
+    {
+        m_timerSendPixyDataUpdate.stop();
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvEmergency( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    sm_isEmergencyActive = true;
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            sm_DisplayEmergency = true;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = true;
+            break;
+
+        case can_node_odr_motion:
+            sm_MotionEmergency = true;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = true;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvAllClear( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            sm_DisplayEmergency = false;
+            break;
+
+        case can_node_sensor_gps:
+            sm_GpsEmergency = false;
+            break;
+
+        case can_node_odr_motion:
+            sm_MotionEmergency = false;
+            break;
+
+        case can_node_odr_sonar_front:
+            sm_SonarFrontEmergency = false;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::recvHeartbeat( uint8_t srcNode )
+{
+#if 0
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+
+    switch ( srcNode )
+    {
+        case can_node_odr_display:
+            m_timerDisplayTimeout.restart();
+            sm_isDisplayAlive = true;
+            break;
+
+        case can_node_sensor_gps:
+            m_timerGpsTimeout.restart();
+            sm_isGpsAlive = true;
+            break;
+
+        case can_node_odr_motion:
+            m_timerMotionTimeout.restart();
+            sm_isMotionAlive = true;
+            break;
+    }
+#endif
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyApp::runLoop( const std::string& loggerName )
+{
+    Poco::Logger& log = Poco::Logger::get( loggerName );
+
+    for ( ;; )
+    {
+        log.information( std::string( "PixyApp::runLoop() starting" ) );
+
+        try
+        {
+            for ( ;; )
+            {
+                CANMessage* msg = CANMessage::WaitDequeueReceived( 250 ); // 250 ms
+
+                if ( msg == 0 && m_quitEvent.tryWait( 0 ) )
+                {
+                    log.information( std::string( "PixyApp::runLoop() stopping" ) );
+                    return;
+                }
+
+                if ( msg == 0 )
+                {
+                    continue;
+                }
+
+                uint8_t  srcNode;
+                uint8_t  dstNode;
+                uint16_t dataId;
+
+                can_parse_message_id( 
+                        msg->msgIdentifier(), &srcNode, &dstNode, &dataId );
+
+                switch ( dataId )
+                {
+                    case can_dataid_emergency:
+                        recvEmergency( srcNode );
+                        break;
+
+                    case can_dataid_all_clear:
+                        recvAllClear( srcNode );
+                        break;
+
+                    case can_dataid_heartbeat:
+                        recvHeartbeat( srcNode );
+                        break;
+
+                    case can_dataid_pixy_start_tracking:
+                        recvSetPixyTracking( true );
+                        break;
+
+                    case can_dataid_pixy_stop_tracking:
+                        recvSetPixyTracking( false );
+                        break;
+                }
+            }
+        }
+        catch ( const Poco::Exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyApp::runLoop() caught Poco::Exception: $0", ex.displayText() ) );
+        }
+        catch ( const std::exception& ex )
+        {
+            log.error(
+                Poco::Logger::format(
+                    "PixyApp::runLoop() caught std::exception: $0", ex.what() ) );
+        }
+        catch ( ... )
+        {
+            log.error( "PixyApp::runLoop() caught unknown exception" );
+        }
+
+        // sleep for 1/2 second after exception processing, but don't exit
+        Poco::Thread::sleep( 500 );
+    }
+}
+
+// ----------------------------------------------------------------------------------------
+
 PixyApp::PixyApp()
     : m_helpRequested( false ),
-      m_runloopActive( false ),
+      m_quitEvent( false /* not auto-reset */ ),
+      m_timerSendHeartbeat(),
       m_timerSendPixyDataUpdate()
 {
 }
@@ -164,12 +367,13 @@
     PixyReader   pixyReader( logger().name() );
     pixyReaderThread.start( pixyReader );
 
+    m_timerSendHeartbeat.setPeriodicInterval( 750 /* milliseconds */ );
+    m_timerSendHeartbeat.start(
+            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendHeartbeat ) );
+
     m_timerSendPixyDataUpdate.setPeriodicInterval( 750 /* milliseconds */ );
-    m_timerSendPixyDataUpdate.start(
-            Poco::TimerCallback< PixyApp >( *this, &PixyApp::sendPixyDataUpdate ) );
 
-    m_runloopActive = true;
-    while ( m_runloopActive ) Poco::Thread::sleep( 5000 /* milliseconds */ );
+    runLoop( logger().name() );
 
     pixyReader.timeToQuit();
     pixyReaderThread.join();
--- a/main/robots/odr/PixyApp.h	Sun May 01 14:13:52 2016 -0700
+++ b/main/robots/odr/PixyApp.h	Sun May 01 14:22:14 2016 -0700
@@ -32,6 +32,7 @@
 #ifndef BCDRL_ROBOTS_ODR_PIXYAPP_H
 #define BCDRL_ROBOTS_ODR_PIXYAPP_H
 
+#include <Poco/Event.h>
 #include <Poco/Timer.h>
 #include <Poco/Util/ServerApplication.h>
 
@@ -43,6 +44,7 @@
     }
 }
 
+#include <stdint.h>
 #include <string>
 #include <vector>
 
@@ -62,11 +64,18 @@
         int  main( const std::vector< std::string >& args );
 
     private:
+        void sendHeartbeat( Poco::Timer& timer );
         void sendPixyDataUpdate( Poco::Timer& timer );
+        void recvEmergency( uint8_t srcNode );
+        void recvAllClear( uint8_t srcNode );
+        void recvHeartbeat( uint8_t srcNode );
+        void recvSetPixyTracking( bool enabled );
+        void runLoop( const std::string& loggerName );
 
     private:
         bool        m_helpRequested;
-        bool        m_runloopActive;
+        Poco::Event m_quitEvent;
+        Poco::Timer m_timerSendHeartbeat;
         Poco::Timer m_timerSendPixyDataUpdate;
 };
 
--- a/main/robots/odr/PixyReader.cpp	Sun May 01 14:13:52 2016 -0700
+++ b/main/robots/odr/PixyReader.cpp	Sun May 01 14:22:14 2016 -0700
@@ -57,8 +57,10 @@
 // ----------------------------------------------------------------------------------------
 
 Poco::RWLock    PixyReader::sm_rwLock;
-bool            PixyReader::sm_isPixyAlive;
+bool            PixyReader::sm_isPixyAlive = false;
 Poco::Timestamp PixyReader::sm_pixyLastUpdate;
+bool            PixyReader::sm_hasTarget = false;
+int8_t          PixyReader::sm_targetPosition;
 
 // ----------------------------------------------------------------------------------------
 
@@ -97,6 +99,60 @@
 
 // ----------------------------------------------------------------------------------------
 
+static const int8_t MAX_LUT_VALUE =  10;
+static const int8_t MIN_LUT_VALUE = -10;
+
+static int8_t XCoordinateToPosition( uint16_t xCoord )
+{
+    static std::vector< int8_t > s_positionLut;
+
+    if ( s_positionLut.size() == 0 )
+    {
+        s_positionLut.reserve( PIXY_MAX_X - PIXY_MIN_X + 1 );
+
+        double scale_factor = static_cast< double >( MAX_LUT_VALUE * 2 );
+        scale_factor /= static_cast< double >( PIXY_MAX_X - PIXY_MIN_X );
+
+        for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
+        {
+            double v = static_cast< double >( i );
+
+            if ( i < ( PIXY_MAX_X / 2 ) )
+            {
+                v = static_cast< double >( PIXY_MAX_X / 2 ) - v;
+                v *= -1.0;
+            }
+            else
+            {
+                v -= static_cast< double >( PIXY_MAX_X / 2 );
+            }
+
+            v *= scale_factor;
+
+            s_positionLut.push_back( static_cast< int8_t >( v ) );
+        }
+
+        s_positionLut[ 0 ] = MIN_LUT_VALUE; // force boundries
+    }
+
+    if ( s_positionLut.size() <= xCoord )
+    {
+        return MAX_LUT_VALUE;
+    }
+
+    return s_positionLut[ xCoord ];
+}
+
+// ----------------------------------------------------------------------------------------
+
+void PixyReader::pixyIsOnline()
+{
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
+    sm_isPixyAlive = true;
+}
+
+// ----------------------------------------------------------------------------------------
+
 void PixyReader::pixyIsOffline()
 {
     Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
@@ -128,6 +184,20 @@
 
 // ----------------------------------------------------------------------------------------
 
+bool PixyReader::hasTarget( int8_t* position )
+{
+    Poco::RWLock::ScopedReadLock lock( sm_rwLock );
+
+    if ( sm_hasTarget && position != NULL )
+    {
+        *position = sm_targetPosition;
+    }
+    
+    return sm_hasTarget;
+}
+
+// ----------------------------------------------------------------------------------------
+
 PixyReader::PixyReader( const std::string& loggerName )
     : Poco::Runnable(),
       m_loggerName( loggerName ),
@@ -164,17 +234,33 @@
 
     log.information( std::string( "PixyReader::configurePixyCam()" ) );
 
+#if 0
+    for ( uint16_t i = PIXY_MIN_X; i <= PIXY_MAX_X; i++ )
+    {
+        log.information(
+            Poco::Logger::format(
+                "PixyReader::configurePixyCam(): $0 = $1",
+                Poco::NumberFormatter::format( i ),
+                Poco::NumberFormatter::format( XCoordinateToPosition( i ) ) ) );
+    }
+#endif
+
     int result = pixy_init();
     if ( result != 0 )
     {
+        pixyIsOffline();
+
         log.error(
             Poco::Logger::format(
                 "PixyReader::configurePixyCam() failed to init: $0 ($1)",
                 PixyErrorCodeToString( result ),
                 Poco::NumberFormatter::format( result ) ) );
+
         return false;
     }
 
+    pixyIsOnline();
+
     uint16_t pixy_version_major;
     uint16_t pixy_version_minor;
     uint16_t pixy_version_build;
@@ -184,11 +270,14 @@
                                         &pixy_version_build );
     if ( result != 0 )
     {
+        pixyIsOffline();
+
         log.error(
             Poco::Logger::format(
                 "PixyReader::configurePixyCam() failed to read f/w version: $0 ($1)",
                 PixyErrorCodeToString( result ),
                 Poco::NumberFormatter::format( result ) ) );
+
         return false;
     }
 
@@ -220,6 +309,8 @@
 
     if ( numBlocks < 0 )
     {
+        pixyIsOffline();
+
         log.error(
             Poco::Logger::format(
                 "PixyReader::processPixyBlocks() failed to read blocks: $0 ($1)",
@@ -228,20 +319,95 @@
         return;
     }
 
-    // the returned block x,y is the center of the rectangle
-    // we should probably filter out things of the wrong aspect ratio
+    Poco::RWLock::ScopedWriteLock lock( sm_rwLock );
 
-#if 0
+    if ( numBlocks == 0 )
+    {
+        sm_hasTarget = false;
+        return; // weird but true
+    }
+
+    // compute the ratio of height to width and surface area for each block, rejecting
+    // blocks that are really the wrong ratio
+
+    double heightToWidthRatio[ PIXY_BLOCK_BUFFER_SIZE ];
+    int    surfaceArea[ PIXY_BLOCK_BUFFER_SIZE ];
+
     for ( int i = 0; i < numBlocks; ++i )
     {
+        if ( pixyBlocks[ i ].height < 10 || pixyBlocks[ i ].width < 10 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+            continue;
+        }
+
+        double ratio = (double)(pixyBlocks[ i ].height) / (double)(pixyBlocks[ i ].width);
+
+        if ( ratio < 1.0 || ratio > 2.0 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+            continue;
+        }
+
+        surfaceArea[ i ] = pixyBlocks[ i ].height * pixyBlocks[ i ].width;
+        heightToWidthRatio[ i ] = ratio;
+
+        if ( surfaceArea[ i ] < 1000 )
+        {
+            heightToWidthRatio[ i ] = 0;
+            surfaceArea[ i ] = 0;
+        }
+    }
+
+    // find the largest block
+
+    int largestSurfaceArea = surfaceArea[ 0 ];
+    int indexOfLargest = 0;
+
+    for ( int i = 1; i < numBlocks; ++i )
+    {
+        if ( surfaceArea[ i ] > largestSurfaceArea )
+        {
+            largestSurfaceArea = surfaceArea[ i ];
+            indexOfLargest = i;
+        }
+    }
+
+    for ( int i = 0; i < numBlocks; ++i )
+    {
+        if ( surfaceArea[ i ] == 0 )
+        {
+            continue;
+        }
+
+#if 1
         log.information(
-            Poco::Logger::format( "received pixy x: $0 y: $1 height: $2 width: $3",
+            Poco::Logger::format( "received pixy x: $0 y: $1 ratio: $2 area: $3",
                 Poco::NumberFormatter::format( pixyBlocks[ i ].x ),
                 Poco::NumberFormatter::format( pixyBlocks[ i ].y ),
-                Poco::NumberFormatter::format( pixyBlocks[ i ].height ),
-                Poco::NumberFormatter::format( pixyBlocks[ i ].width ) ) );
+                Poco::NumberFormatter::format( heightToWidthRatio[ i ], 6, 4 ),
+                Poco::NumberFormatter::format( surfaceArea[ i ] ) ) );
+#endif
     }
-#endif
+
+    if ( largestSurfaceArea > 0 )
+    {
+        sm_hasTarget      = true;
+        sm_targetPosition = XCoordinateToPosition( pixyBlocks[ indexOfLargest ].x );
+
+        log.information(
+            Poco::Logger::format( "choose candidate block $0 (ratio $1, area $2, pos $3)",
+                Poco::NumberFormatter::format( indexOfLargest ),
+                Poco::NumberFormatter::format( heightToWidthRatio[ indexOfLargest ], 6, 4 ),
+                Poco::NumberFormatter::format( surfaceArea[ indexOfLargest ] ),
+                Poco::NumberFormatter::format( sm_targetPosition ) ) );
+    }
+    else
+    {
+        sm_hasTarget = false;
+    }
 }
 
 // ----------------------------------------------------------------------------------------
@@ -264,6 +430,8 @@
                 continue;
             }
 
+            // read data from the PixyCam until told to quit
+
             for ( ;; )
             {
                 if ( m_quitEvent.tryWait( 0 ) )
--- a/main/robots/odr/PixyReader.h	Sun May 01 14:13:52 2016 -0700
+++ b/main/robots/odr/PixyReader.h	Sun May 01 14:22:14 2016 -0700
@@ -45,8 +45,8 @@
 class PixyReader : public Poco::Runnable
 {
     public:
-        static void   pixyIsOffline();
-        static bool   isPixyAlive();
+        static bool isPixyAlive();
+        static bool hasTarget( int8_t* position = NULL );
 
     public:
         PixyReader( const std::string& loggerName );
@@ -55,27 +55,19 @@
         void timeToQuit();
 
     private:
-        typedef enum
-        {
-            PARSE_STATE_LOOKING_FOR_START,
-            PARSE_STATE_START_SYMBOL_A,
-            PARSE_STATE_START_SYMBOL_N,
-            PARSE_STATE_START_SYMBOL_G,
-            PARSE_STATE_START_SYMBOL_COLON,
-            PARSE_STATE_READ_ROLL,
-            PARSE_STATE_READ_PITCH,
-            PARSE_STATE_READ_YAW
-
-        }   imu_parse_state_t;
+        static void pixyIsOnline();
+        static void pixyIsOffline();
 
     private:
         static Poco::RWLock    sm_rwLock;
         static bool            sm_isPixyAlive;
         static Poco::Timestamp sm_pixyLastUpdate;
+        static bool            sm_hasTarget;
+        static int8_t          sm_targetPosition;
 
     private:
-        std::string       m_loggerName;
-        Poco::Event       m_quitEvent;
+        std::string m_loggerName;
+        Poco::Event m_quitEvent;
 
     private:
         void closePixyCam();
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main/robots/odr/pixy_ubuntu.xml	Sun May 01 14:22:14 2016 -0700
@@ -0,0 +1,36 @@
+<config>
+    <logging>
+        <formatters>
+            <f1>
+                <class>PatternFormatter</class>
+                <pattern>%q %m/%d/%y-%H:%M:%S.%F %t</pattern>
+                <times>UTC</times>
+            </f1>
+        </formatters>
+        <channels>
+            <c1>
+                <class>FileChannel</class>
+                <formatter>f1</formatter>
+                <path>/home/bob/devroot/main/robots/odr/pixy.log</path>
+                <rotation>1M</rotation>
+                <archive>number</archive>
+                <purgeCount>4</purgeCount>
+            </c1>
+        </channels>
+        <loggers>
+            <primary>
+                <channel>c1</channel>
+                <level>information</level>
+            </primary>
+        </loggers>
+    </logging>
+    <application>
+        <logger>primary</logger>
+    </application>
+    <can>
+        <interfaceName>can0</interfaceName>
+        <logIncoming>no</logIncoming>
+        <logOutgoing>no</logOutgoing>
+    </can>
+</config>
+