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

285
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,176 +49,165 @@
#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;
int MuorbTestExample::main() int MuorbTestExample::main()
{ {
int rc; int rc;
appState.setRunning(true); appState.setRunning(true);
rc = PingPongTest(); rc = PingPongTest();
//rc = FileReadTest(); rc = FileReadTest();
appState.setRunning( false ); rc = uSleepTest();
return rc; appState.setRunning(false);
return rc;
} }
int MuorbTestExample::DefaultTest() 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( &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; memset(&pwm, 0, sizeof(pwm_input_s));
pwm.error_count++; memset(&sc, 0, sizeof(sensor_combined_s));
sc.gyro_errcount++; PX4_WARN("Successful after memset... ");
//while (!appState.exitRequested() && i<5) { orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm);
while (!appState.exitRequested() && i < 10 ) {
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..."); 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 ) ++i;
{
volatile int x = 0;
++x;
}
++i;
} }
return 0;
return 0;
} }
int MuorbTestExample::PingPongTest() 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 )
{
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() ) { if (pub_id_esc_status == 0) {
PX4_ERR("error publishing esc_status");
return -1;
}
PX4_DEBUG("[%d] Doing work...", i ); if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) {
bool updated = false; PX4_ERR("[%d]Error publishing the esc_status message", i);
if( orb_check( sub_vc, &updated ) == 0 ) return -1;
{ }
if( updated )
{ int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
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 (sub_vc == PX4_ERROR) {
{ PX4_ERR("Error subscribing to vehicle_command topic");
PX4_ERR( "[%d]Error calling orb copy for vechicle... ", i ); return -1;
break; }
}
if( orb_publish( ORB_ID( esc_status ), pub_id_esc_status, &m_esc_status ) == PX4_ERROR ) while (!appState.exitRequested()) {
{
PX4_ERR( "[%d]Error publishing the esc_status message", i ); PX4_DEBUG("[%d] Doing work...", i);
break; bool updated = false;
}
} if (orb_check(sub_vc, &updated) == 0) {
else if (updated) {
{ PX4_WARN("[%d]vechile command status is updated... reading new value", i);
PX4_DEBUG( "[%d] vechicle command topic is not updated", i );
} if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
} PX4_ERR("[%d]Error calling orb copy for vechicle... ", i);
else break;
{ }
PX4_ERR( "[%d]Error checking the updated status for vehicle command ", 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);
// sleep for 1 sec. break;
usleep( 1000000 ); }
++i; } else {
} PX4_WARN("[%d] vechicle command topic is not updated", i);
return 0; }
} 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 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; size_t len = 0;
size_t len = 0; ssize_t read;
ssize_t read;
fp = fopen( TEST_FILE_PATH, "r" ); 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 );
}
}
/* if (fp == NULL) {
std::fstream fs( TEST_FILE_PATH, std::fstream::in ); PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH);
if( fs.is_open() ) rc = PX4_ERROR;
{
int i = 0; } else {
char line[1024]; int i = 0;
while( !fs.eof() ) while( ( read = getline( &line, &len, fp ) ) != -1 )
{ {
++i; ++i;
fs.getline( line, 1024 ); PX4_INFO( "LineNum[%d] LineLength[%d]", i, len );
PX4_WARN( "ReadLine[%d] Line[%s]", i, line ); PX4_INFO( "LineNum[%d] Line[%s]", i, line );
} }
fs.close(); PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH);
} fclose(fp);
else
{ if (line != NULL) {
PX4_WARN( "Unable to open file[%s] for reading", TEST_FILE_PATH ); free(line);
rc = PX4_ERROR; }
} }
*/
return rc; return rc;
} }

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

@ -40,18 +40,19 @@
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:
int DefaultTest();
int PingPongTest();
int FileReadTest();
struct esc_status_s m_esc_status; private:
struct vehicle_command_s m_vc; int DefaultTest();
int PingPongTest();
int FileReadTest();
int uSleepTest();
struct esc_status_s m_esc_status;
struct vehicle_command_s m_vc;
}; };