Exemple #1
0
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;
}