diff --git a/src/platforms/qurt/tests/muorb/module.mk b/src/platforms/qurt/tests/muorb/module.mk index 128d894f87..e21ee3c60b 100644 --- a/src/platforms/qurt/tests/muorb/module.mk +++ b/src/platforms/qurt/tests/muorb/module.mk @@ -37,7 +37,12 @@ MODULE_COMMAND = muorb_test -SRCS = muorb_test_main.cpp \ +SRCS = \ muorb_test_start_qurt.cpp \ muorb_test_example.cpp +INCLUDE_DIRS += $(PX4_BASE)/src/modules/uORB \ + $(PX4_BASE)/src/platforms \ + $(PX4_BASE)/src/modules + + diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp index fdf46c6260..d934e1b9b7 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -42,19 +42,183 @@ #include #include #include +#include +#include "uORB/topics/sensor_combined.h" +#include "uORB/topics/pwm_input.h" +#include "uORB.h" +#include "px4_middleware.h" +#include "px4_defines.h" +#include +//#include +//#include + px4::AppState MuorbTestExample::appState; int MuorbTestExample::main() { + int rc; appState.setRunning(true); + rc = PingPongTest(); + //rc = FileReadTest(); + appState.setRunning( false ); + return rc; +} + +int MuorbTestExample::DefaultTest() +{ + struct pwm_input_s pwm; + struct sensor_combined_s sc; + //sc = new sensor_combined_s; + memset( &pwm, 0, sizeof(pwm_input_s) ); + memset( &sc, 0, sizeof(sensor_combined_s) ); + PX4_WARN( "Suucessful after memset... " ); + orb_advert_t pub_fd = orb_advertise( ORB_ID( pwm_input ), &pwm ); + if( pub_fd == nullptr ) + { + PX4_WARN( "Error: advertizing pwm_input topic" ); + return -1; + } + orb_advert_t pub_sc = orb_advertise( ORB_ID( sensor_combined ), &sc ); + if( pub_sc == nullptr ) + { + PX4_WARN( "Error: advertizing sensor_combined topic" ); + return -1; + } + int i=0; - while (!appState.exitRequested() && i<5) { + pwm.error_count++; + sc.gyro_errcount++; + //while (!appState.exitRequested() && i<5) { + while (!appState.exitRequested() && i < 10 ) { - PX4_DEBUG(" Doing work..."); - ++i; + PX4_INFO(" Doing work..."); + orb_publish( ORB_ID( pwm_input), pub_fd, &pwm ); + orb_publish( ORB_ID( sensor_combined ), pub_sc, &sc ); + //px4::usleep( 1000000 ); + //sleep( 1 ); + for( int64_t j = 0; j < 0x80; ++j ) + { + volatile int x = 0; + ++x; + } + ++i; } - - return 0; + return 0; +} + +int MuorbTestExample::PingPongTest() +{ + int i=0; + orb_advert_t pub_id_esc_status = orb_advertise( ORB_ID( esc_status ), & m_esc_status ); + if( pub_id_esc_status == 0 ) + { + PX4_ERR( "error publishing esc_status" ); + return -1; + } + if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc_status message", i ); + return -1; + } + int sub_vc = orb_subscribe( ORB_ID( vehicle_command ) ); + if ( sub_vc == PX4_ERROR ) + { + PX4_ERR( "Error subscribing to vehicle_command topic" ); + return -1; + } + + while (!appState.exitRequested() ) { + + PX4_DEBUG("[%d] Doing work...", i ); + bool updated = false; + if( orb_check( sub_vc, &updated ) == 0 ) + { + if( updated ) + { + PX4_DEBUG( "[%d]vechile command status is updated... reading new value", i ); + if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) + { + PX4_ERR( "[%d]Error calling orb copy for vechicle... ", i ); + break; + } + if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR ) + { + PX4_ERR( "[%d]Error publishing the esc_status message", i ); + break; + } + } + else + { + PX4_DEBUG( "[%d] vechicle command topic is not updated", i ); + } + } + else + { + PX4_ERR( "[%d]Error checking the updated status for vehicle command ", i ); + break; + } + // sleep for 1 sec. + usleep( 1000000 ); + + ++i; + } + return 0; +} + +int MuorbTestExample::FileReadTest() +{ + int rc = OK; + //static const char TEST_FILE_PATH[] = "/home/linaro/test.txt"; + static const char TEST_FILE_PATH[] = "./test.txt"; + FILE* fp; + char* line = NULL; + size_t len = 0; + ssize_t read; + + fp = fopen( TEST_FILE_PATH, "r" ); + if( fp == NULL ) + { + PX4_WARN( "unable to open file[%s] for reading", TEST_FILE_PATH ); + rc = PX4_ERROR; + } + else + { + int i = 0; + //while( ( read = getline( &line, &len, fp ) ) != -1 ) + //{ + // ++i; + // PX4_WARN( "LineNum[%d] LineLength[%d]", i, len ); + // PX4_WARN( "LineNum[%d] Line[%s]", i, line ); + //} + PX4_WARN( "Successfully opened file [%s]", TEST_FILE_PATH ); + fclose( fp ); + if( line != NULL ) + { + free( line ); + } + } + +/* + std::fstream fs( TEST_FILE_PATH, std::fstream::in ); + if( fs.is_open() ) + { + int i = 0; + char line[1024]; + while( !fs.eof() ) + { + ++i; + fs.getline( line, 1024 ); + PX4_WARN( "ReadLine[%d] Line[%s]", i, line ); + } + fs.close(); + } + else + { + PX4_WARN( "Unable to open file[%s] for reading", TEST_FILE_PATH ); + rc = PX4_ERROR; + } +*/ + return rc; } diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.h b/src/platforms/qurt/tests/muorb/muorb_test_example.h index 304b8464e0..5d6f70a45e 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -34,6 +34,8 @@ #pragma once #include +#include "uORB/topics/esc_status.h" +#include "uORB/topics/vehicle_command.h" class MuorbTestExample { public: @@ -44,4 +46,12 @@ public: int main(); static px4::AppState appState; /* track requests to terminate app */ +private: + int DefaultTest(); + int PingPongTest(); + int FileReadTest(); + + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; + }; diff --git a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp index 2ded8976c6..8f8b7640e2 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_main.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_main.cpp @@ -42,7 +42,9 @@ #include #include "muorb_test_example.h" -int PX4_MAIN(int argc, char **argv) +extern "C" __EXPORT int muorb_test_entry( int argc, char** argv ); + +int muorb_test_entry(int argc, char **argv) { px4::init(argc, argv, "muorb_test"); diff --git a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp index f063806766..6d64cfec8b 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp @@ -50,6 +50,18 @@ static int daemon_task; /* Handle of deamon task / thread */ extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]); +int muorb_test_entry(int argc, char **argv) +{ + //px4::init(argc, argv, "muorb_test"); + + PX4_INFO("muorb_test entry....."); + MuorbTestExample hello; + hello.main(); + + PX4_INFO("goodbye"); + return 0; +} + static void usage() { PX4_DEBUG("usage: muorb_test {start|stop|status}"); @@ -68,12 +80,14 @@ int muorb_test_main(int argc, char *argv[]) /* this is not an error */ return 0; } + + PX4_INFO( "before starting the muorb_test_entry task" ); daemon_task = px4_task_spawn_cmd("muorb_test", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 16000, - PX4_MAIN, + 8192, + muorb_test_entry, (char* const*)argv); return 0;