Control multi-rotor UAVs using the telekyb3 protocol.

Ports

rotor_input (in)

Data structure

  • struct ::or_rotorcraft::input rotor_input

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • enum ::or_rotorcraft::control_type control ∈ { velocity, throttle }

    • sequence< double, 8 > desired

rotor_measure (out)

Data structure

  • struct ::or_rotorcraft::output rotor_measure

    • sequence< struct ::or_rotorcraft::rotor_state, 8 > rotor

      • struct ::or::time::ts ts

        • long sec

        • long nsec

      • boolean emerg

      • boolean spinning

      • boolean starting

      • boolean disabled

      • double velocity

      • double throttle

      • double consumption

      • double energy_level

imu (out)

Data structure

  • struct ::or_pose_estimator::state imu

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::pos_cov > pos_cov

      • double cov[6]

    • optional< struct ::or::t3d::att_cov > att_cov

      • double cov[10]

    • optional< struct ::or::t3d::att_pos_cov > att_pos_cov

      • double cov[12]

    • optional< struct ::or::t3d::vel_cov > vel_cov

      • double cov[6]

    • optional< struct ::or::t3d::avel_cov > avel_cov

      • double cov[6]

    • optional< struct ::or::t3d::acc_cov > acc_cov

      • double cov[6]

    • optional< struct ::or::t3d::aacc_cov > aacc_cov

      • double cov[6]

Provides current gyroscopes and accelerometer measurements.

According to the nature of data, the port is filled with the imu data timestamp ts, intrinsic true, no position (pos and pos_cov are absent) and linear velocities vx, vy, vz set to NaN. All other elements are always present.

mag (out)

Data structure

  • struct ::or_pose_estimator::state mag

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • boolean intrinsic

    • optional< struct ::or::t3d::pos > pos

      • double x

      • double y

      • double z

    • optional< struct ::or::t3d::att > att

      • double qw

      • double qx

      • double qy

      • double qz

    • optional< struct ::or::t3d::vel > vel

      • double vx

      • double vy

      • double vz

    • optional< struct ::or::t3d::avel > avel

      • double wx

      • double wy

      • double wz

    • optional< struct ::or::t3d::acc > acc

      • double ax

      • double ay

      • double az

    • optional< struct ::or::t3d::aacc > aacc

      • double awx

      • double awy

      • double awz

    • optional< struct ::or::t3d::pos_cov > pos_cov

      • double cov[6]

    • optional< struct ::or::t3d::att_cov > att_cov

      • double cov[10]

    • optional< struct ::or::t3d::att_pos_cov > att_pos_cov

      • double cov[12]

    • optional< struct ::or::t3d::vel_cov > vel_cov

      • double cov[6]

    • optional< struct ::or::t3d::avel_cov > avel_cov

      • double cov[6]

    • optional< struct ::or::t3d::acc_cov > acc_cov

      • double cov[6]

    • optional< struct ::or::t3d::aacc_cov > aacc_cov

      • double cov[6]

Provides current magnetometer measurements.

Services

get_sensor_rate (attribute)

Outputs

  • struct ::rotorcraft::ids::sensor_time_s::rate_s rate

    • double imu Accelerometer and gyroscopes measurement frequency

    • double mag Magnetometer measurement frequency

    • double motor Various motor data measurement frequency

    • double battery Battery level measurement frequency

  • struct ::rotorcraft::ids::sensor_time_s::rate_s measured_rate

    • double imu Accelerometer and gyroscopes effective frequency

    • double mag Magnetometer measurement frequency

    • double motor Various motor data effective frequency

    • double battery Battery level effective frequency

Get hardware sensor data publishing rate, see set_sensor_rate (attribute).

set_sensor_rate (attribute)

Inputs

  • struct ::rotorcraft::ids::sensor_time_s::rate_s rate

    • double imu (default "1000") Accelerometer and gyroscopes measurement frequency

    • double mag (default "100") Magnetometer measurement frequency

    • double motor (default "50") Various motor data measurement frequency

    • double battery (default "1") Battery level measurement frequency

Set hardware sensor data publishing rate, in Hz

imu and mag control the update frequency of port imu (out) and mag (out) respectively, while motor and battery indirectly control the port rotor_measure (out).

Caution
 The hardware may not be able to achieve the desired frequency, especially for motor data when many motors are controlled. In this case, no error will be reported, but the ports update rate may be lower than expected and the servo (activity) service will smoothly stop the motors.

get_battery (attribute)

Outputs

  • struct ::rotorcraft::ids::battery_s battery

    • double min Minimum acceptable battery voltage

    • double max Full battery voltage

    • double level Current battery voltage

Get current battery voltage and limits.

set_battery_limits (attribute)

Inputs

  • double min (default "14") Minimum acceptable battery voltage

  • double max (default "16.7") Full battery voltage
Throws

  • exception ::rotorcraft::e_range

Set battery minimum and full voltage

This controls the computed energy left percentage in the port rotor_measure (out).

get_imu_calibration (attribute)

Outputs

  • struct ::rotorcraft::ids::imu_calibration_s imu_calibration

    • double gscale[9] Gyroscopes 3×3 scaling matrix (row major)

    • double gbias[3] Gyroscopes bias vector

    • double gstddev[3] Gyroscopes measurement noise

    • double ascale[9] Accelerometers 3×3 scaling matrix (row major)

    • double abias[3] Accelerometers bias vector

    • double astddev[3] Accelerometers measurement noise

    • double mscale[9]

    • double mbias[3]

    • double mstddev[3]

Get current gyroscopes and accelerometer calibration data.

set_imu_calibration (attribute)

Inputs

  • struct ::rotorcraft::ids::imu_calibration_s imu_calibration

    • double gscale[9] Gyroscopes 3×3 scaling matrix (row major)

    • double gbias[3] Gyroscopes bias vector

    • double gstddev[3] Gyroscopes measurement noise

    • double ascale[9] Accelerometers 3×3 scaling matrix (row major)

    • double abias[3] Accelerometers bias vector

    • double astddev[3] Accelerometers measurement noise

    • double mscale[9]

    • double mbias[3]

    • double mstddev[3]

Set current gyroscopes and accelerometer calibration data.

Calling this service is mandatory after each component start, in order to obtain precise IMU measurements.

Input parameters are typically those returned by a call to get_imu_calibration (attribute) after a successful calibrate_imu (activity) (which see).

get_imu_filter (function)

Outputs

  • double gfc[3] Gyroscope X,Y,Z cut-off frequencies

  • double afc[3] Accelerometer X,Y,Z cut-off frequencies

  • double mfc[3] Magnetometer X,Y,Z cut-off frequencies

set_imu_filter (function)

Inputs

  • double gfc[3] Gyroscope X,Y,Z cut-off frequencies

  • double afc[3] Accelerometer X,Y,Z cut-off frequencies

  • double mfc[3] Magnetometer X,Y,Z cut-off frequencies

set_timeout (attribute)

Inputs

  • double timeout (default "30") Startup timeout (s)

Set motor startup timeout

set_ramp (attribute)

Inputs

  • double ramp

connect (activity)

Inputs

  • string<64> serial (default "/dev/ttyUSB0") Serial device

  • unsigned long baud (default "0") Baud rate (0 = don’t change)
Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_baddev

    • string<256> dev
Context

Connect to the hardware

disconnect (activity)

Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what
Context

Disconnect from the hardware

monitor (activity)

Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what
Context

Monitor connection status

disable_motor (function)

Inputs

  • unsigned short motor

Disable checking a motor status when it is disconnected

enable_motor (function)

Inputs

  • unsigned short motor

Disable checking a motor status when it is disconnected

calibrate_imu (activity)

Inputs

  • double tstill (default "2") Duration in seconds of standstill positions

  • unsigned short nposes (default "10") Number of different standstill positions
Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_connection
Context

Calibrate accelerometers and gyroscopes.

This service computes the 3×3 scaling matrices and 3D bias vector for both gyroscopes and accelerometers so that all data is returned in a consistent, orthogonal frame of reference. This is done by implementing the paper ‘A robust and easy to implement method for IMU calibration without external equipments, ICRA 2014’. It requires no external sensor and a minimum of 10 static poses spanning the whole SO(3) space, with moderate motion in between. The standard deviation of the sensor noise is also estimated.

The tstill parameter controls the time after which a standstill position is detected (2 seconds is fine), while nposes sets the required number of such standstill positions (minimum 10).

While running the calibration, a progress indication will be reported to the standard output of the component. You should first set the platform in the first standstill orientation, then start the service. The service will report stay still until it has acquired the first pose, then report acquired pose 1. You can then move to the next standstill orientation, leave it until you read the same messages again, and so on for all the nposes orientations.

For the calibration to be precise, all the orientations have to be as different as possible one from each other. Also, when moving from one orientation to another, try to perform a motion such that the angular velocities on all 3 axis are not zero.

If you don’t read stay still after moving to a new pose, this means that the platform may be vibrating or slightly moving, and the standstill detection cannot work. After some time, the service will eventually abort and also report it on the standard output.

Once all orientations have been acquired, the results are set for the current running instance, and available with get_imu_calibration (attribute). Make sure to save the results somewhere before stopping the component, so that you can load them with set_imu_calibration (attribute) when you later restart.

Caution
 This procedure does not set any particular vertical axis and the IMU will typically end up calibrated but not aligned with the gravity. Use set_zero (activity) (after calibration) to align the IMU.

set_zero (activity)

Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what
Context

Align IMU frame with the gravity vector and reset gyroscopes bias.

This service updates the 3×3 scaling matrices and 3D bias vector for both gyroscopes and accelerometers so that the current accelerometer measurements are only on the Z axis and the gyroscopes return a 0 angular velocity on each axis.

While running this service, the platform should be perfectly standstill and in a horizontal configuration (i.e. it’s roll and pitch angles are considered zero).

After completion, the current calibration results are updated and can be retrieved with get_imu_calibration (attribute).

This service should be called quite often, as the gyroscopes bias are much dependent on the temperature, so it is important to estimate them well.

start (activity)

Throws

  • exception ::rotorcraft::e_connection

  • exception ::rotorcraft::e_started

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_rotor_failure

    • unsigned short id

  • exception ::rotorcraft::e_rotor_not_disabled

    • unsigned short id
Context

Spin propellers at the lowest velocity

servo (activity)

Throws

  • exception ::rotorcraft::e_connection

  • exception ::rotorcraft::e_rotor_failure

    • unsigned short id

  • exception ::rotorcraft::e_rate

  • exception ::rotorcraft::e_input
Context

Control the propellers according to the given velocities

set_velocity (function)

Inputs

  • sequence< double, 8 > desired Propeller velocities
Throws

  • exception ::rotorcraft::e_connection

  • exception ::rotorcraft::e_rotor_failure

    • unsigned short id
Context

Set the given propeller velocity, once

set_throttle (function)

Inputs

  • sequence< double, 8 > desired Propeller throttles
Throws

  • exception ::rotorcraft::e_connection

  • exception ::rotorcraft::e_rotor_failure

    • unsigned short id
Context

Set the given propeller voltage

stop (activity)

Context

Stop all propellers

log (function)

Inputs

  • string<64> path (default "/tmp/rotorcraft.log") Log file name

  • unsigned long decimation (default "1") Reduced logging frequency
Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

Log IMU and commanded wrench

log_stop (function)

Stop logging

log_info (function)

Outputs

  • unsigned long miss Missed log entries

  • unsigned long total Total log entries

Show missed log entries

Tasks

main

Context

comm

Context
Throws

  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what