dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Updates to QuRT muorb tests 

Added usleep test and fixed code format errors.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2015-07-09 21:32:13 -07:00
parent 2a8402edb1
commit 56d12caa9a
2 changed files with 145 additions and 155 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

145
src/platforms/qurt/tests/muorb/muorb_test_example.cpp
View File

@ -32,8 +32,8 @@
****************************************************************************/ ****************************************************************************/
/** /**
* @file hello_example.cpp * @file muorb_test_example.cpp
* Example for Linux * Example for muorb
* *
* @author Mark Charlebois <charlebm@gmail.com> * @author Mark Charlebois <charlebm@gmail.com>
*/ */
@ -49,9 +49,7 @@
#include "px4_middleware.h" #include "px4_middleware.h"
#include "px4_defines.h" #include "px4_defines.h"
#include <stdlib.h> #include <stdlib.h>
//#include <fstream> #include <drivers/drv_hrt.h>
//#include <iostream>
px4::AppState MuorbTestExample::appState; px4::AppState MuorbTestExample::appState;
@ -60,7 +58,8 @@ int MuorbTestExample::main()
int rc; int rc;
appState.setRunning(true); appState.setRunning(true);
rc = PingPongTest(); rc = PingPongTest();
//rc = FileReadTest(); rc = FileReadTest();
rc = uSleepTest();
appState.setRunning(false); appState.setRunning(false);
return rc; return rc;
} }
@ -69,42 +68,38 @@ int MuorbTestExample::DefaultTest()
{ {
struct pwm_input_s pwm; struct pwm_input_s pwm;
struct sensor_combined_s sc; struct sensor_combined_s sc;
//sc = new sensor_combined_s;
memset(&pwm, 0, sizeof(pwm_input_s)); memset(&pwm, 0, sizeof(pwm_input_s));
memset(&sc, 0, sizeof(sensor_combined_s)); memset(&sc, 0, sizeof(sensor_combined_s));
PX4_WARN( "Suucessful after memset... " ); PX4_WARN("Successful after memset... ");
orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm); orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm);
if( pub_fd == nullptr )
{ if (pub_fd == nullptr) {
PX4_WARN("Error: advertizing pwm_input topic"); PX4_WARN("Error: advertizing pwm_input topic");
return -1; return -1;
} }
orb_advert_t pub_sc = orb_advertise(ORB_ID(sensor_combined), &sc); orb_advert_t pub_sc = orb_advertise(ORB_ID(sensor_combined), &sc);
if( pub_sc == nullptr )
{ if (pub_sc == nullptr) {
PX4_WARN("Error: advertizing sensor_combined topic"); PX4_WARN("Error: advertizing sensor_combined topic");
return -1; return -1;
} }
int i = 0; int i = 0;
pwm.error_count++; pwm.error_count++;
sc.gyro_errcount++; sc.gyro_errcount++;
//while (!appState.exitRequested() && i<5) {
while (!appState.exitRequested() && i < 10) { while (!appState.exitRequested() && i < 10) {
PX4_INFO(" Doing work..."); PX4_INFO(" Doing work...");
orb_publish(ORB_ID(pwm_input), pub_fd, &pwm); orb_publish(ORB_ID(pwm_input), pub_fd, &pwm);
orb_publish(ORB_ID(sensor_combined), pub_sc, &sc); orb_publish(ORB_ID(sensor_combined), pub_sc, &sc);
//px4::usleep( 1000000 );
//sleep( 1 ); sleep(1);
for( int64_t j = 0; j < 0x80; ++j )
{
volatile int x = 0;
++x;
}
++i; ++i;
} }
return 0; return 0;
} }
@ -112,19 +107,20 @@ int MuorbTestExample::PingPongTest()
{ {
int i = 0; int i = 0;
orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status); orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status);
if( pub_id_esc_status == 0 )
{ if (pub_id_esc_status == 0) {
PX4_ERR("error publishing esc_status"); PX4_ERR("error publishing esc_status");
return -1; return -1;
} }
if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR )
{ 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); PX4_ERR("[%d]Error publishing the esc_status message", i);
return -1; return -1;
} }
int sub_vc = orb_subscribe(ORB_ID(vehicle_command)); int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
if ( sub_vc == PX4_ERROR )
{ if (sub_vc == PX4_ERROR) {
PX4_ERR("Error subscribing to vehicle_command topic"); PX4_ERR("Error subscribing to vehicle_command topic");
return -1; return -1;
} }
@ -133,44 +129,58 @@ int MuorbTestExample::PingPongTest()
PX4_DEBUG("[%d] Doing work...", i); PX4_DEBUG("[%d] Doing work...", i);
bool updated = false; bool updated = false;
if( orb_check( sub_vc, &updated ) == 0 )
{ if (orb_check(sub_vc, &updated) == 0) {
if( updated ) if (updated) {
{ PX4_WARN("[%d]vechile command status is updated... reading new value", i);
PX4_DEBUG( "[%d]vechile command status is updated... reading new value", i );
if( orb_copy( ORB_ID( vehicle_command ), sub_vc, &m_vc ) != 0 ) if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
{
PX4_ERR("[%d]Error calling orb copy for vechicle... ", i); PX4_ERR("[%d]Error calling orb copy for vechicle... ", i);
break; break;
} }
if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR )
{ 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); PX4_ERR("[%d]Error publishing the esc_status message", i);
break; break;
} }
} else {
PX4_WARN("[%d] vechicle command topic is not updated", i);
} }
else
{ } else {
PX4_DEBUG( "[%d] vechicle command topic is not updated", i );
}
}
else
{
PX4_ERR("[%d]Error checking the updated status for vehicle command ", i); PX4_ERR("[%d]Error checking the updated status for vehicle command ", i);
break; break;
} }
// sleep for 1 sec. // sleep for 1 sec.
usleep(1000000); usleep(1000000);
++i; ++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; return 0;
} }
int MuorbTestExample::FileReadTest() int MuorbTestExample::FileReadTest()
{ {
int rc = OK; int rc = OK;
//static const char TEST_FILE_PATH[] = "/home/linaro/test.txt";
static const char TEST_FILE_PATH[] = "./test.txt"; static const char TEST_FILE_PATH[] = "./test.txt";
FILE *fp; FILE *fp;
char *line = NULL; char *line = NULL;
@ -178,47 +188,26 @@ int MuorbTestExample::FileReadTest()
ssize_t read; ssize_t read;
fp = fopen(TEST_FILE_PATH, "r"); fp = fopen(TEST_FILE_PATH, "r");
if( fp == NULL )
{ if (fp == NULL) {
PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH); PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH);
rc = PX4_ERROR; rc = PX4_ERROR;
}
else } else {
{
int i = 0; int i = 0;
//while( ( read = getline( &line, &len, fp ) ) != -1 ) 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 )
{ {
++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); 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; return rc;
} }

3
src/platforms/qurt/tests/muorb/muorb_test_example.h
View File

@ -40,16 +40,17 @@
class MuorbTestExample { class MuorbTestExample {
public: public:
MuorbTestExample() {}; MuorbTestExample() {};
~MuorbTestExample() {}; ~MuorbTestExample() {};
int main(); int main();
static px4::AppState appState; /* track requests to terminate app */ static px4::AppState appState; /* track requests to terminate app */
private: private:
int DefaultTest(); int DefaultTest();
int PingPongTest(); int PingPongTest();
int FileReadTest(); int FileReadTest();
int uSleepTest();
struct esc_status_s m_esc_status; struct esc_status_s m_esc_status;
struct vehicle_command_s m_vc; struct vehicle_command_s m_vc;