Skip to content

Commit

Permalink
[src] Update Events
Browse files Browse the repository at this point in the history
  • Loading branch information
ThomasLeMezo committed Mar 10, 2018
1 parent 3f8bba7 commit 22f5a70
Show file tree
Hide file tree
Showing 4 changed files with 84 additions and 42 deletions.
59 changes: 46 additions & 13 deletions src/ellipse.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -15,19 +15,19 @@ void Ellipse::connect(){

// Set the parameters of the Interface (port, baud_rate)
errorCode = sbgInterfaceSerialCreate(&m_sbgInterface, m_uartPortName.c_str(), m_uartBaudRate);
if (errorCode != SBG_NO_ERROR){ROS_WARN("sbgInterfaceSerialCreate Error : %s", sbgErrorCodeToString(errorCode));}
if (errorCode != SBG_NO_ERROR){ROS_WARN("SBG DRIVER - sbgInterfaceSerialCreate Error : %s", sbgErrorCodeToString(errorCode));}

// Init the SBG
errorCode = sbgEComInit(&m_comHandle, &m_sbgInterface);
if (errorCode != SBG_NO_ERROR){ROS_WARN("sbgEComInit Error : %s", sbgErrorCodeToString(errorCode));}
if (errorCode != SBG_NO_ERROR){ROS_WARN("SBG DRIVER - sbgEComInit Error : %s", sbgErrorCodeToString(errorCode));}

// Get Infos
read_GetInfo(&m_comHandle);
}

void Ellipse::init_callback(){
SbgErrorCode errorCode = sbgEComSetReceiveLogCallback(&m_comHandle, onLogReceived, this);
if (errorCode != SBG_NO_ERROR){ROS_WARN("sbgEComSetReceiveLogCallback Error : %s", sbgErrorCodeToString(errorCode));}
if (errorCode != SBG_NO_ERROR){ROS_WARN("SBG DRIVER - sbgEComSetReceiveLogCallback Error : %s", sbgErrorCodeToString(errorCode));}
}

void Ellipse::init_publishers(){
Expand All @@ -45,7 +45,10 @@ void Ellipse::init_publishers(){
m_sbgGpsHdt_pub = m_node->advertise<sbg_driver::SbgGpsHdt>("gps_hdt",10);
m_sbgGpsRaw_pub = m_node->advertise<sbg_driver::SbgGpsRaw>("gps_raw",10);
m_sbgOdoVel_pub = m_node->advertise<sbg_driver::SbgOdoVel>("odo_vel",10);
m_sbgEvent_pub = m_node->advertise<sbg_driver::SbgEvent>("event",10);
m_sbgEventA_pub = m_node->advertise<sbg_driver::SbgEvent>("eventA",10);
m_sbgEventB_pub = m_node->advertise<sbg_driver::SbgEvent>("eventB",10);
m_sbgEventC_pub = m_node->advertise<sbg_driver::SbgEvent>("eventC",10);
m_sbgEventD_pub = m_node->advertise<sbg_driver::SbgEvent>("eventD",10);
m_sbgPressure_pub = m_node->advertise<sbg_driver::SbgPressure>("pressure",10);
}

Expand All @@ -67,12 +70,12 @@ void Ellipse::configure(){

if(change){
// // SAVE AND REBOOT
ROS_INFO("The configuration of the Ellipse was updated according to the configuration file");
ROS_INFO("SBG DRIVER - The configuration of the Ellipse was updated according to the configuration file");
SbgErrorCode errorCode = sbgEComCmdSettingsAction(&m_comHandle, SBG_ECOM_SAVE_SETTINGS);
if (errorCode != SBG_NO_ERROR)
ROS_WARN("sbgEComCmdSettingsAction (Saving) Error : %s", sbgErrorCodeToString(errorCode));
ROS_WARN("SBG DRIVER - sbgEComCmdSettingsAction (Saving) Error : %s", sbgErrorCodeToString(errorCode));
else
ROS_INFO("SAVED & REBOOT");
ROS_INFO("SBG DRIVER - SAVED & REBOOT");
}
}

Expand Down Expand Up @@ -187,7 +190,7 @@ void Ellipse::load_param(){

void Ellipse::publish(){
SbgErrorCode errorCode = sbgEComHandle(&m_comHandle);
if (errorCode != SBG_NO_ERROR){ROS_WARN("sbgEComHandle Error : %s", sbgErrorCodeToString(errorCode));}
if (errorCode != SBG_NO_ERROR){ROS_WARN("SBG DRIVER - sbgEComHandle Error : %s", sbgErrorCodeToString(errorCode));}

if(m_new_sbgStatus){
m_new_sbgStatus = false;
Expand Down Expand Up @@ -259,9 +262,24 @@ void Ellipse::publish(){
m_sbgOdoVel_pub.publish(m_sbgOdoVel_msg);
}

if(m_new_sbgEvent){
m_new_sbgEvent = false;
m_sbgEvent_pub.publish(m_sbgEvent_msg);
if(m_new_sbgEventA){
m_new_sbgEventA = false;
m_sbgEventA_pub.publish(m_sbgEventA_msg);
}

if(m_new_sbgEventB){
m_new_sbgEventB = false;
m_sbgEventB_pub.publish(m_sbgEventB_msg);
}

if(m_new_sbgEventC){
m_new_sbgEventC = false;
m_sbgEventC_pub.publish(m_sbgEventC_msg);
}

if(m_new_sbgEventD){
m_new_sbgEventD = false;
m_sbgEventD_pub.publish(m_sbgEventD_msg);
}

if(m_new_sbgPressure){
Expand Down Expand Up @@ -345,8 +363,23 @@ SbgErrorCode onLogReceived(SbgEComHandle *pHandle, SbgEComClass msgClass, SbgECo
break;

case SBG_ECOM_LOG_EVENT_A:
read_ecom_log_event(e->m_sbgEvent_msg, pLogData);
e->m_new_sbgEvent = true;
read_ecom_log_event(e->m_sbgEventA_msg, pLogData);
e->m_new_sbgEventA = true;
break;

case SBG_ECOM_LOG_EVENT_B:
read_ecom_log_event(e->m_sbgEventB_msg, pLogData);
e->m_new_sbgEventB = true;
break;

case SBG_ECOM_LOG_EVENT_C:
read_ecom_log_event(e->m_sbgEventC_msg, pLogData);
e->m_new_sbgEventC = true;
break;

case SBG_ECOM_LOG_EVENT_D:
read_ecom_log_event(e->m_sbgEventD_msg, pLogData);
e->m_new_sbgEventD = true;
break;

case SBG_ECOM_LOG_PRESSURE:
Expand Down
15 changes: 12 additions & 3 deletions src/ellipse.h
Original file line number Diff line number Diff line change
Expand Up @@ -60,7 +60,10 @@ class Ellipse
sbg_driver::SbgGpsHdt m_sbgGpsHdt_msg;
sbg_driver::SbgGpsRaw m_sbgGpsRaw_msg;
sbg_driver::SbgOdoVel m_sbgOdoVel_msg;
sbg_driver::SbgEvent m_sbgEvent_msg;
sbg_driver::SbgEvent m_sbgEventA_msg;
sbg_driver::SbgEvent m_sbgEventB_msg;
sbg_driver::SbgEvent m_sbgEventC_msg;
sbg_driver::SbgEvent m_sbgEventD_msg;
sbg_driver::SbgPressure m_sbgPressure_msg;

public:
Expand All @@ -78,7 +81,10 @@ class Ellipse
bool m_new_sbgGpsHdt;
bool m_new_sbgGpsRaw;
bool m_new_sbgOdoVel;
bool m_new_sbgEvent;
bool m_new_sbgEventA;
bool m_new_sbgEventB;
bool m_new_sbgEventC;
bool m_new_sbgEventD;
bool m_new_sbgPressure;

int m_rate_frequency;
Expand All @@ -98,7 +104,10 @@ class Ellipse
ros::Publisher m_sbgGpsHdt_pub;
ros::Publisher m_sbgGpsRaw_pub;
ros::Publisher m_sbgOdoVel_pub;
ros::Publisher m_sbgEvent_pub;
ros::Publisher m_sbgEventA_pub;
ros::Publisher m_sbgEventB_pub;
ros::Publisher m_sbgEventC_pub;
ros::Publisher m_sbgEventD_pub;
ros::Publisher m_sbgPressure_pub;

// *************** SBG TOOLS *************** //
Expand Down
22 changes: 11 additions & 11 deletions src/ellipse_msg.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -197,7 +197,7 @@ void read_ecom_log_mag(sbg_driver::SbgMag &msg, const SbgBinaryLogData *pLogData

void read_ecom_log_mag_calib(sbg_driver::SbgMagCalib &msg, const SbgBinaryLogData *pLogData){
msg.header.stamp = ros::Time::now();
ROS_INFO("SbgMagCalib message not implemented");
ROS_INFO("SBG DRIVER - SbgMagCalib message not implemented");
}

void read_gps_vel_status(sbg_driver::SbgGpsVelStatus &msg, const uint32 &val){
Expand Down Expand Up @@ -305,14 +305,14 @@ void read_ecom_log_pressure(sbg_driver::SbgPressure &msg, const SbgBinaryLogData
void read_GetInfo(SbgEComHandle *comHandle){
SbgEComDeviceInfo *pInfo;
SbgErrorCode errorCode = sbgEComCmdGetInfo(comHandle, pInfo);
if (errorCode != SBG_NO_ERROR){ROS_WARN("sbgEComCmdGetInfo Error : %s", sbgErrorCodeToString(errorCode));}

ROS_INFO("SBG productCode = %s", pInfo->productCode);
ROS_INFO("SBG serialNumber = %i", pInfo->serialNumber);
ROS_INFO("SBG calibationRev = %i", pInfo->calibationRev);
ROS_INFO("SBG calibrationYear = %i", pInfo->calibrationYear);
ROS_INFO("SBG calibrationMonth = %i", pInfo->calibrationMonth);
ROS_INFO("SBG calibrationDay = %i", pInfo->calibrationDay);
ROS_INFO("SBG hardwareRev = %i", pInfo->hardwareRev);
ROS_INFO("SBG firmwareRev = %i", pInfo->firmwareRev);
if (errorCode != SBG_NO_ERROR){ROS_WARN("SBG DRIVER - sbgEComCmdGetInfo Error : %s", sbgErrorCodeToString(errorCode));}

ROS_INFO("SBG DRIVER - productCode = %s", pInfo->productCode);
ROS_INFO("SBG DRIVER - serialNumber = %i", pInfo->serialNumber);
ROS_INFO("SBG DRIVER - calibationRev = %i", pInfo->calibationRev);
ROS_INFO("SBG DRIVER - calibrationYear = %i", pInfo->calibrationYear);
ROS_INFO("SBG DRIVER - calibrationMonth = %i", pInfo->calibrationMonth);
ROS_INFO("SBG DRIVER - calibrationDay = %i", pInfo->calibrationDay);
ROS_INFO("SBG DRIVER - hardwareRev = %i", pInfo->hardwareRev);
ROS_INFO("SBG DRIVER - firmwareRev = %i", pInfo->firmwareRev);
}
30 changes: 15 additions & 15 deletions src/main.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -7,29 +7,29 @@ int main(int argc, char **argv)
ros::init(argc, argv, "sbg_ellipse");
ros::NodeHandle n;

ROS_INFO("Init node & load params");
ROS_INFO("SBG DRIVER - Init node & load params");
Ellipse ellipse(&n);

ellipse.init_publishers();

// ROS_INFO("Ellipse connect");
// ellipse.connect();
ROS_INFO("SBG DRIVER - Ellipse connect");
ellipse.connect();

// ROS_INFO("Ellipse configure");
// ellipse.configure();
ROS_INFO("SBG DRIVER - Ellipse configure");
ellipse.configure();

// ROS_INFO("Init callback");
// ellipse.init_callback();
ROS_INFO("SBG DRIVER - Init callback");
ellipse.init_callback();

// ROS_INFO("START RECEIVING DATA");
// ros::Rate loop_rate(ellipse.m_rate_frequency);
// while (ros::ok())
// {
// ellipse.publish();
ROS_INFO("SBG DRIVER - START RECEIVING DATA");
ros::Rate loop_rate(ellipse.m_rate_frequency);
while (ros::ok())
{
ellipse.publish();

// ros::spinOnce();
// loop_rate.sleep();
// }
ros::spinOnce();
loop_rate.sleep();
}

return 0;
}

0 comments on commit 22f5a70

Please sign in to comment.