forked from Archive/PX4-Autopilot
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
parent
2a8402edb1
commit
56d12caa9a
|
|
@ -32,8 +32,8 @@
|
|||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file hello_example.cpp
|
||||
* Example for Linux
|
||||
* @file muorb_test_example.cpp
|
||||
* Example for muorb
|
||||
*
|
||||
* @author Mark Charlebois <charlebm@gmail.com>
|
||||
*/
|
||||
|
|
@ -49,176 +49,165 @@
|
|||
#include "px4_middleware.h"
|
||||
#include "px4_defines.h"
|
||||
#include <stdlib.h>
|
||||
//#include <fstream>
|
||||
//#include <iostream>
|
||||
|
||||
#include <drivers/drv_hrt.h>
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue