diff --git a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp index d934e1b9b7..7e93bf1149 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.cpp +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.cpp @@ -32,8 +32,8 @@ ****************************************************************************/ /** - * @file hello_example.cpp - * Example for Linux + * @file muorb_test_example.cpp + * Example for muorb * * @author Mark Charlebois */ @@ -49,176 +49,165 @@ #include "px4_middleware.h" #include "px4_defines.h" #include -//#include -//#include - +#include px4::AppState MuorbTestExample::appState; int MuorbTestExample::main() { - int rc; + int rc; appState.setRunning(true); - rc = PingPongTest(); - //rc = FileReadTest(); - appState.setRunning( false ); - return rc; + rc = PingPongTest(); + rc = FileReadTest(); + rc = uSleepTest(); + 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; - } - + struct pwm_input_s pwm; + struct sensor_combined_s sc; - int i=0; - pwm.error_count++; - sc.gyro_errcount++; - //while (!appState.exitRequested() && i<5) { - while (!appState.exitRequested() && i < 10 ) { + memset(&pwm, 0, sizeof(pwm_input_s)); + memset(&sc, 0, sizeof(sensor_combined_s)); + PX4_WARN("Successful 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; + pwm.error_count++; + sc.gyro_errcount++; + + while (!appState.exitRequested() && i < 10) { 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; + orb_publish(ORB_ID(pwm_input), pub_fd, &pwm); + orb_publish(ORB_ID(sensor_combined), pub_sc, &sc); + + sleep(1); + ++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; - } + int i = 0; + orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status); - while (!appState.exitRequested() ) { + if (pub_id_esc_status == 0) { + PX4_ERR("error publishing esc_status"); + return -1; + } - 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; + 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_WARN("[%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_WARN("[%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::uSleepTest() +{ + PX4_WARN("before usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); + usleep(1000000); + PX4_INFO("After usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time()); + + for (int i = 0; i < 10; ++i) { + PX4_INFO("In While Loop: B4 Sleep for[%d] seconds [%" PRIu64 "]" , i + 1, hrt_absolute_time()); + usleep((i + 1) * 1000000); + PX4_INFO("In While Loop: After Sleep for[%d] seconds [%" PRIu64 "]" , i + 1 , hrt_absolute_time()); + } + + PX4_INFO("exiting sleep test..."); + 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; + int rc = OK; + 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 ); - } - } + fp = fopen(TEST_FILE_PATH, "r"); -/* - 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; + 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_INFO( "LineNum[%d] LineLength[%d]", i, len ); + PX4_INFO( "LineNum[%d] Line[%s]", i, line ); + } + PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH); + fclose(fp); + + if (line != NULL) { + free(line); + } + } + + 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 5d6f70a45e..266b0a2b42 100644 --- a/src/platforms/qurt/tests/muorb/muorb_test_example.h +++ b/src/platforms/qurt/tests/muorb/muorb_test_example.h @@ -40,18 +40,19 @@ class MuorbTestExample { public: MuorbTestExample() {}; - ~MuorbTestExample() {}; 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; +private: + int DefaultTest(); + int PingPongTest(); + int FileReadTest(); + int uSleepTest(); + + struct esc_status_s m_esc_status; + struct vehicle_command_s m_vc; };