-
Notifications
You must be signed in to change notification settings - Fork 4
Description
#include <ros/ros.h>
#include <std_msgs/UInt8.h>
#include <adnav_driver/an_packet_protocol.h>
#include <adnav_driver/ins_packets.h>
// GPIO Mode Constants
#define RAW_SENSOR_TRIGGER 0x01
#define EVENT_INPUT_MODE 0x02
ros::Publisher gpio_pub;
void setGPIOConfiguration()
{
gpio_configuration_packet_t gpio_configuration_packet;
gpio_configuration_packet.permanent = 1;
gpio_configuration_packet.gpio_function[0] = gpio_function_packet_trigger_system_state;
gpio_configuration_packet.gpio_function[1] = gpio_function_packet_trigger_raw_sensors;
gpio_configuration_packet.gpio_voltage_selection = gpio_voltage_3v3;
an_packet_t* packet = encode_gpio_configuration_packet(&gpio_configuration_packet);
if (packet != nullptr)
{
if (send_packet_to_device(packet))
{
ROS_INFO("GPIO configuration packet sent successfully.");
std_msgs::UInt8 msg;
msg.data = packet->data[1];
gpio_pub.publish(msg);
ROS_INFO("Setting GPIO Configuration: %d", msg.data);
}
else
{
ROS_ERROR("Failed to send GPIO configuration packet.");
}
}
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "certus_gpio_control");
ros::NodeHandle nh;
gpio_pub = nh.advertise<std_msgs::UInt8>("/an_device/gpio_config", 10);
//ros::Rate loop_rate(1);
while (ros::ok())
{
setGPIOConfiguration();
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
not working trigger mode