int init(){
const int self_node_id = 2;
/*
* Node initialization.
* Node ID and name are required; otherwise, the node will refuse to start.
* Version info is optional.
*/
auto& node = getNode();
node.setNodeID(self_node_id);
node.setName("org.kmti.cam.control");
/*
* Start the node.
* All returnable error codes are listed in the header file uavcan/error.hpp.
*/
const int node_start_res = node.start();
if (node_start_res < 0) {
printf("Node init: node failed to start with error %d", node_start_res);
return node_start_res;
}
uavcan::Subscriber<uavcan::equipment::camera::CameraCommand> camera_sub(node);
const int camera_sub_start_res =
camera_sub.start(
[&](const uavcan::ReceivedDataStructure<uavcan::equipment::camera::CameraCommand>& msg)
{
printf("Yay, we got a message!");
switch(msg.command_type) {
case msg.COMMAND_TAKE_PICTURE:
take_picture();
break;
case msg.COMMAND_TURN_ON:
turn_on_camera();
break;
case msg.COMMAND_TURN_OFF:
turn_off_camera();
break;
}
});
if (camera_sub_start_res < 0) {
printf("Camera subscriber: subscriber failed to start with error %d", camera_sub_start_res);
return camera_sub_start_res;
}
/*
* Informing other nodes that we're ready to work.
* Default mode is INITIALIZING.
*/
node.setModeOperational();
node_thread.start(NORMALPRIO);
return 0;
}
void stopCamera() {
turn_off_camera();
cameraOn = 0;
}