aruna::movement Namespace Reference

Namespaces
	anonymous_namespace{movement.cpp}

Classes
class	Actuator
class	ActuatorSet
struct	axis_t

Enumerations
enum	axis_mask_t : uint8_t {   axis_mask_t::X = (1 << 0), axis_mask_t::Y = (1 << 1), axis_mask_t::Z = (1 << 2), axis_mask_t::ROLL = (1 << 3),   axis_mask_t::YAW = (1 << 4), axis_mask_t::PITCH = (1 << 5), axis_mask_t::DIRECTION_PLUS = (1 << 6), axis_mask_t::DIRECTION_MIN = (1 << 7),   axis_mask_t::DIRECTION_BOTH = 192, axis_mask_t::ALL_AXIS = 63, axis_mask_t::MAX = 6, axis_mask_t::NONE = 0 }
enum	damping_t { damping_t::DISABLE = 0, damping_t::KEEP_VELOCITY = 0x01, damping_t::KEEP_DEGREE = 0x02 }
enum	status_t { status_t::RUNNING = 0, status_t::STOPPED = 1 }

Functions
status_t	start ()
	initialise movement and communicate with hardware for active modes. More...
status_t	get_status ()
	get the status of the movement unit. More...
void *	comm_handler_task (void *arg)
	task to handle all the incoming comm requests. More...
status_t	stop ()
	Stop all motors and free all processes. More...
err_t	register_driver (Actuator *driver)
	register a accelerator driver for use. More...
void	calibrate_sensors ()
	Calibarte the sensors, aka set 0 point. More...
uint8_t	test_sensor ()
	test whenever the sensors are connected, sensor should be horizontal and facing up. More...
void	set_speed (axis_mask_t axisMask, int16_t speed)
	Set the speed of the motors directly. More...
int16_t	get_speed (axis_mask_t single_axis)
	Get the speed of an axis. More...
int16_t	get_velocity (axis_mask_t single_axis)
	get the current velocity More...
int16_t	get_degree (axis_mask_t single_axis)
	get the angle of an axis (pitch, roll and yaw only) More...
void	set_degree (axis_mask_t axisMask, int16_t degree)
	set the degree of an axis (only applies to yaw, pitch and roll) More...
void	set_velocity (axis_mask_t axisMask, int16_t mm_per_second)
	Set the target velocity of the ROV. More...
axis_mask_t	get_active_axis ()
	Get active modus (X,Y,Z,yawn,pitch,roll) More...
damping_t	get_damping (axis_mask_t single_axis)
	get the damping of a single mode. More...
void	set_damping (axis_mask_t axisMask, damping_t damp)
	set the damping type. More...
err_t	unregister_driver (Actuator *driver)
	unregister a accelerator driver for use. More...

Enumeration Type Documentation

axis_mask_t

enum aruna::movement::axis_mask_t : uint8_t

strong

Enumerator
X
Y
Z
ROLL
YAW
PITCH
DIRECTION_PLUS
DIRECTION_MIN
DIRECTION_BOTH
ALL_AXIS
MAX
NONE

Definition at line 13 of file movementTypes.h.

                              : uint8_t {
            X = (1 << 0),
            Y = (1 << 1),
            Z = (1 << 2),
            ROLL = (1 << 3),
            YAW = (1 << 4),
            PITCH = (1 << 5),

            DIRECTION_PLUS = (1 << 6),
            DIRECTION_MIN = (1 << 7),

            DIRECTION_BOTH = 192,
            ALL_AXIS = 63,
            MAX = 6,
            NONE = 0,
        };

damping_t

enum aruna::movement::damping_t

strong

Enumerator
DISABLE
KEEP_VELOCITY
KEEP_DEGREE

Definition at line 30 of file movementTypes.h.

                             {
            DISABLE = 0,
            KEEP_VELOCITY = 0x01,
            KEEP_DEGREE = 0x02,
        };

status_t

enum aruna::movement::status_t

strong

Enumerator
RUNNING
STOPPED

Definition at line 37 of file movementTypes.h.

                            {
            RUNNING = 0,
            STOPPED = 1,
        };

Function Documentation

calibrate_sensors()

void aruna::movement::calibrate_sensors

(

)

Calibarte the sensors, aka set 0 point.

make sure that the device is standing still and is level for 400ms.

Definition at line 287 of file movement.cpp.

                         {
//   TODO
}

Here is the caller graph for this function:

comm_handler_task()

void * aruna::movement::comm_handler_task

(

void *

arg

)

task to handle all the incoming comm requests.

Parameters

arg,not

used.

Definition at line 60 of file movement.cpp.

                                   {
    enum comm_commands_t {
        NO_COMMAND = 0x00,

//      basic functionality
        SET_SPEED = 0x01,
        GET_SPEED = 0x02,
        SET_VELOCITY = 0x03,
        GET_VELOCITY = 0x04,
        SET_DEGREE = 0x05,
        GET_DEGREE = 0x06,
        GET_DIRECTION = 0x07,

//      closed loop configuration
        SET_DAMPING = 0x10,
        GET_DAMPING = 0x11,

//      debugging
        SET_RUNNING_STATE = 0x20,
        GET_RUNNING_STATE = 0x21,
        TEST_SENSORS = 0x22,
        CALIBRATE_SENSORS = 0x23,
        GET_SUPPORTED_AXIS = 0x24,
        SET_SENSOR_OFFSET = 0x25,
        GET_SENSOR_OFFSET = 0x26,
        ACTIVATE_MPU = 0x27,
        GET_MPU_STATUS = 0x28,

//      advanched automated control
        RETURN_HOME = 0x30,
        SET_HOME = 0x31,
        GOTO_CORDINATES = 0x32,
    };
//  initialize comms
    comm::transmitpackage_t request;
    uint8_t mask;
    uint8_t flags;
    int16_t data;
    uint8_t buffer[6];
    axis_mask_t active_axis;
    comm::channel_t movement_channel = {
            .port = 3,
    };
    int16_t (*get_value)(axis_mask_t mode);
    void (*set_value)(axis_mask_t mode, int16_t speed);
    comm_commands_t command;
    if((int) movement_channel.register_err) {
        log->error("failed to register comm channel: %s", err_to_char.at(movement_channel.register_err));
    }
    while (1) {
        if (movement_channel.receive(&request)) {
//          if (request.data_received_lenght < 2)
//              continue;
            flags = request.data_received[1];
            data = ((uint16_t) request.data_received[2] << 8) | request.data_received[3];
            get_value = nullptr;
            set_value = nullptr;
            command = NO_COMMAND;
            log->debug("comm command:%X, flag:%X, data:%d", request.data_received[0], request.data_received[1], data);
            switch (request.data_received[0]) {
//              basic functionality

                case GET_DEGREE:
                    if ((flags & ((uint8_t) axis_mask_t::X | (uint8_t) axis_mask_t::Y | (uint8_t) axis_mask_t::Z)))
                        break;
                    get_value = get_degree;
                    command = GET_DEGREE;
                case GET_VELOCITY:
                    if (command == NO_COMMAND) {
                        get_value = get_velocity;
                        command = GET_VELOCITY;
                    }
                case GET_SPEED:
                    if (command == NO_COMMAND) {
                        command = GET_SPEED;
                        get_value = get_speed;
                    }
                    mask = 1;
                    int16_t speed;
                    active_axis = get_active_axis();
                    for (int i = 0; i < (uint8_t) axis_mask_t::MAX; ++i) {
                        mask = 0b1 << i;
                        if (mask & flags & (uint8_t) active_axis) {
                            buffer[0] = command;
                            buffer[1] = mask;
                            speed = get_value((axis_mask_t) mask);
                            buffer[2] = (speed >> 8);
                            buffer[3] = speed & 0xff;
//                          log->debug("mask: %X flag: %X", mask, flags);
//                          log->debug("GET_COMMAND: %X, value: %d",command,  speed);
//                          TODO error check comm.send return value
                            movement_channel.send(request.from_port, buffer, 4);
                        }
                    }
                    break;
//              set target velocity yaw, roll, pitch, x, y, z
                case SET_DEGREE:
                    if ((flags & ((uint8_t) axis_mask_t::X | (uint8_t) axis_mask_t::Y | (uint8_t) axis_mask_t::Z)))
                        break;
                    set_value = set_degree;
                    command = SET_DEGREE;
                case SET_VELOCITY:
                    if (command == NO_COMMAND) {
                        set_value = set_velocity;
                        command = SET_VELOCITY;
                    }
                case SET_SPEED:
                    if (command == NO_COMMAND) {
                        set_value = set_speed;
                        command = SET_SPEED;
                    }
                    log->verbose("command: %X, axis: 0X%02X, speed: %i", command, flags, data);
                    set_value((axis_mask_t) flags, data);
                    break;
//              control

//              get damping
                case GET_DAMPING:
                    mask = 1;
                    active_axis = get_active_axis();
                    for (int j = 0; j < (uint8_t) axis_mask_t::MAX; ++j) {
                        mask = 1 << j;
                        if (mask & flags & (uint8_t) active_axis) {
                            buffer[0] = GET_DAMPING;
                            buffer[1] = mask;
                            buffer[2] = (uint8_t) get_damping((axis_mask_t) mask);
                            movement_channel.send(request.from_port, buffer, 3);
                        }
                    }
                    break;
//              set damping
                case SET_DAMPING:
                    set_damping((axis_mask_t) flags, (damping_t) data);
                    break;

//              debugging

//              get running state
                case GET_RUNNING_STATE:
                    buffer[0] = GET_RUNNING_STATE;
                    buffer[1] = (uint8_t) get_status();
                    movement_channel.send(request.from_port, buffer, 2);
                    break;
//              set running state
                case SET_RUNNING_STATE:
//                  TODO return success?
                    switch (data) {
                        case (uint8_t) status_t::RUNNING :
                            start();
                            break;
                        case (uint8_t) status_t::STOPPED:
                            stop();
                            break;
                        default:
                            break;
                    }
                    break;
//              test sensors
                case TEST_SENSORS:
                    uint8_t t;
                    t = test_sensor();
                    buffer[0] = TEST_SENSORS;
                    buffer[1] = t;
                    movement_channel.send(request.from_port, buffer, 2);
                    break;
//              calibrate sensors
                case CALIBRATE_SENSORS:
                    calibrate_sensors();
                    break;
//              get supported modes
                case GET_SUPPORTED_AXIS:
                    buffer[0] = GET_SUPPORTED_AXIS;
                    buffer[1] = (uint8_t) get_active_axis();
                    movement_channel.send(request.from_port, buffer, 2);
                    break;
                case SET_SENSOR_OFFSET:
                case GET_SENSOR_OFFSET:
                    break;
                case ACTIVATE_MPU:
//                  TODO disable support also?
                    break;
                case GET_MPU_STATUS:
                    buffer[0] = GET_MPU_STATUS;
                    movement_channel.send(request.from_port, buffer, 1);
                    break;
//              advanced automated control

//              return home
                case RETURN_HOME:
//              set home
                case SET_HOME:
//              goto cordinates
                case GOTO_CORDINATES:
//              protocol error
                default:
                    break;
            }
            movement_channel.receive_clear();
        }
    }
    pthread_exit(0);
}

Here is the call graph for this function:

Here is the caller graph for this function:

get_active_axis()

axis_mask_t aruna::movement::get_active_axis

(

)

Get active modus (X,Y,Z,yawn,pitch,roll)

Returns

movement_mode_t with a bit high on enabled modus. Use movement_mode_mask_t to decipher.

Definition at line 337 of file movement.cpp.

                              {
    uint8_t modes = 0;
    for (Actuator *d: drivers) {
        modes |= (uint8_t) d->get_axis();
    }
    return (axis_mask_t) modes;
}

Here is the caller graph for this function:

get_damping()

damping_t aruna::movement::get_damping

(

axis_mask_t

single_axis

)

get the damping of a single mode.

Parameters

single_axis

only 1 bit to high.

Returns

damping of that mode.

Definition at line 345 of file movement.cpp.

                                               {
    return currentDamping[(uint8_t) single_axis];
}

Here is the caller graph for this function:

get_degree()

int16_t aruna::movement::get_degree

(

axis_mask_t

single_axis

)

get the angle of an axis (pitch, roll and yaw only)

Parameters

single_axis

of target axis

Returns

degree of axis (32767 = 360 deg)

Definition at line 322 of file movement.cpp.

                                            {
    if ((uint8_t) single_axis & ((uint8_t) axis_mask_t::X | (uint8_t) axis_mask_t::Y | (uint8_t) axis_mask_t::Z))
        return 0;
    return currentDegree[(uint8_t) single_axis];
}

Here is the caller graph for this function:

get_speed()

int16_t aruna::movement::get_speed

(

axis_mask_t

single_axis

)

Get the speed of an axis.

Parameters

single_axis

one axis at a time

Returns

speed.

Definition at line 314 of file movement.cpp.

                                           {
    return currentSpeed[(uint8_t) single_axis];
}

Here is the caller graph for this function:

get_status()

status_t aruna::movement::get_status

(

)

get the status of the movement unit.

Returns

status enum, current running status.

Definition at line 56 of file movement.cpp.

                      {
    return movement_status;
}

Here is the caller graph for this function:

get_velocity()

int16_t aruna::movement::get_velocity

(

axis_mask_t

single_axis

)

get the current velocity

Parameters

single_axis

of target axis

Returns

mm_second how fast we are going

Definition at line 318 of file movement.cpp.

                                              {
    return currentVelocity[(uint8_t) single_axis];
}

Here is the caller graph for this function:

register_driver()

err_t aruna::movement::register_driver

(

Actuator *

driver

)

register a accelerator driver for use.

Parameters

driver

pointer to the driver

Returns

err_t::OK if it went well. err_t::FAIL if not.

Definition at line 278 of file movement.cpp.

                                        {
    if (drivers.find(driver) != drivers.end()) {
        return err_t::DRIVER_EXISTS;
    }
    if (!drivers.insert(driver).second)
        return err_t::BUFFER_OVERFLOW;
    return err_t::OK;
}

set_damping()

void aruna::movement::set_damping	(	axis_mask_t	axisMask,
		damping_t	damp
	)

set the damping type.

• err_t::DAMP_DISABLE will disable all damping for that axis
• err_t::DAMP_KEEP_VELOCITY ROV will try to keep the current velocity
• err_t::DAMP_KEEP_DEGREE ROV will try to keep the current degree (only row, yaw and pitch)

  Parameters

  damp	damping_t, mode to set damping to.
  axisMask	axis to apply damping to

Definition at line 349 of file movement.cpp.

                                                       {
    uint8_t j = 1;
    axis_mask_t active_axis = get_active_axis();
    for (uint8_t i = 0; i < (uint8_t) axis_mask_t::MAX; i++) {
        j=1<<i;
        if ((uint8_t) axisMask & j & (uint8_t) active_axis) {
            currentDamping[j] = damp;
        }
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

set_degree()

void aruna::movement::set_degree	(	axis_mask_t	axisMask,
		int16_t	degree
	)

set the degree of an axis (only applies to yaw, pitch and roll)

Parameters

axisMask	multiple axis to apply this to
degree	degree (65535 is maximum)

Definition at line 329 of file movement.cpp.

                                                      {
//  TODO
}

Here is the caller graph for this function:

set_speed()

void aruna::movement::set_speed	(	axis_mask_t	axisMask,
		int16_t	speed
	)

Set the speed of the motors directly.

Parameters

axisMask,multiple	axis to apply speed to.
speed,speed	of the motors

Definition at line 296 of file movement.cpp.

                                                    {
    uint8_t j = 1;
    err_t ret;
    axis_mask_t active_axis = get_active_axis();
    for (Actuator *d: drivers) {
        ret = d->set(axisMask, speed);
        if (ret != err_t::OK) {
            log->warning("setting speed of driver failed: %s", err_to_char.at(ret));
        }
    }
    for (int i = 0; i < (uint8_t) axis_mask_t::MAX; i++) {
        j= 1 << i;
        if ((uint8_t) axisMask & j & (uint8_t) active_axis) {
            currentSpeed[j] = speed;
        }
    }
}

Here is the call graph for this function:

Here is the caller graph for this function:

set_velocity()

void aruna::movement::set_velocity	(	axis_mask_t	axisMask,
		int16_t	mm_per_second
	)

Set the target velocity of the ROV.

Parameters

axisMask	axis to apply the velocity to
mm_per_second	how fast

Definition at line 333 of file movement.cpp.

                                                               {
// TODO
}

Here is the caller graph for this function:

start()

status_t aruna::movement::start

(

)

initialise movement and communicate with hardware for active modes.

start SIS report watcher thread

start reporter to report all SIS activiry to the watcher over comm channels

Returns

status_t, returns current status of the modulke

• RUNNING if it is running,
• STOPPED is it us currenty stopped.

Definition at line 30 of file movement.cpp.

                 {
//    check movement status.
    if (movement_status == status_t::RUNNING)
        return movement_status;

    log = new log::channel_t("Movement");
//    check if we got drivers
    if (drivers.empty()) {
        log->warning("Start failed: No drivers found!");
        return movement_status;
    }

//    start all drivers
    for (Actuator *d: drivers) {
        err_t stat = d->startup_error;
        if (stat != err_t::OK)
//            TODO print driver name?
            log->warning("Driver failed to start: %s", err_to_char.at(stat));
    }

//    start threads.
    pthread_create(&movement_comm_handler, NULL, comm_handler_task, NULL);
    movement_status = status_t::RUNNING;
    return movement_status;
}

Here is the call graph for this function:

Here is the caller graph for this function:

stop()

status_t aruna::movement::stop

(

)

Stop all motors and free all processes.

Returns

status_t, returns current status of the modulke

• RUNNING if it is running,
• STOPPED is it us currenty stopped.

Definition at line 263 of file movement.cpp.

                {
    if (movement_status == status_t::STOPPED)
        return movement_status;

    //    stop all drivers
    for (Actuator *d: drivers) {
        delete d;
    }

//    stop task
    pthread_cancel(movement_comm_handler);
    movement_status = status_t::STOPPED;
    return movement_status;
}

Here is the caller graph for this function:

test_sensor()

uint8_t aruna::movement::test_sensor

(

)

test whenever the sensors are connected, sensor should be horizontal and facing up.

Returns

1 if all is well; 0 if not.

Definition at line 291 of file movement.cpp.

                      {
// TODO
    return 1;
}

Here is the caller graph for this function:

unregister_driver()

err_t aruna::movement::unregister_driver

(

Actuator *

driver

)

unregister a accelerator driver for use.

Parameters

driver

pointer to the driver

Returns

err_t::OK if it went well. err_t::FAIL if not.