Project

General

Profile

rotorcraft component

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 (function).


set_sensor_rate (function)

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

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

      • long sec

      • long nsec

    • 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_calibration_param (attribute)

Outputs
  • struct ::rotorcraft::ids::calibration_param_s calib_param

    • double motion_tolerance Tolerance factor of the standstill detector

Get IMU calibration parameters. See set_calibration_param (attribute).


set_calibration_param (attribute)

Inputs
  • struct ::rotorcraft::ids::calibration_param_s calib_param

    • double motion_tolerance (default "10") Tolerance factor of the standstill detector

Set IMU calibration parameters.

The motion_tolerance is a multiplicative factor the controls the sensitivity of the standstill detector used by calibrate_imu (activity). motion_tolerance must be greater than 1.0. The closer it is to 1.0, the more the detector will be sensitive to slight motion, making the calibration procedure practically cumbersome but hopefully more precise. A good tradeoff is 10.0, which is the default.


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]

    • double temp Average IMU temperature (°C)

Get current gyroscopes and accelerometer calibration data.


set_imu_calibration (function)

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]

    • double temp (default "21") Average IMU temperature (°C)

Set current gyroscopes, accelerometer and magnetometer 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


get_imu_temp (attribute)

Outputs
  • double imu_temp IMU temperature (°C)

Get current IMU temperature.


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.

serial is the device special file to open, at baud speed. If one or more connections are already open, they are all closed first.

See pconnect (activity) to deal with multiple hardware connections.


pconnect (activity)

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

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

  • boolean imu (default "1") Use IMU

  • boolean mag (default "1") Use magnetometer

  • boolean motor (default "1") Use motors

  • unsigned short offset (default "0") Motor id offset

Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_baddev

    • string<256> dev

Context

Connect to multiple hardware devices.

This works like connect (activity), except that existing connections are not closed, so that multiple devices can be connected simultaneously.

serial is the device special file to open, at baud speed. If a connection with the same serial is already open, it is closed and replaced by a new one with updated parameters.

imu, mag and motor flags indicates whether the corresponding part should be used (TRUE) or ignored (FALSE).

offset is a small integer added to the motor ids reported by the hardware, so that the rotor_measure (out) or rotor_input (in) arrays present an aggregated view of the different devices. For instance, to connect two devices that both have 4 motors numbered from 1 to 4, set the first offset to 0 and the second to 4, so that rotor_measure (out) (resp. rotor_input (in)) will present (resp. use) motor ids from 1 to 8 with the first half directed to the first device and the second half to the second device.


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


set_pid (function)

Inputs
  • unsigned short motor (default "1") Motor id

  • double Kp (default "0") Proportional gain

  • double Ki (default "0") Integral gain

  • double Kd (default "0") Derivative gain

  • double f (default "0") Derivative filtering pole

Throws
  • exception ::rotorcraft::e_baddev

    • string<256> dev

Update the hardware PID controller parameters, on hardware that implements this.


calibrate_imu (activity)

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

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

  • string<64> path (default "") Log file name (or empty string for no log)

Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_connection

Context

Calibrate accelerometers, gyroscopes and magnetometer.

This service computes the 3×3 scaling matrices and 3D bias vector for gyroscopes, accelerometers and magnetometers 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). The duration of motion between two standstill positions must not exceed thirty times the tstill parameter (60 seconds by default).

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 (function) when you later restart.

If a log file name has been specified in path, the file is filled with all samples acquired during calibration, corrected with the newly estimated calibration parameters. In addition, a boolean indicates for each sample if it is considered as part of a standstill position or not, so that the quality of the calibration can be visually assessed.

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.

calibrate_mag (activity)

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

  • string<64> path (default "") Log file name (or empty string for no log)

Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

  • exception ::rotorcraft::e_connection

Context

Calibrate magnetometer.

This service computes the 3×3 scaling matrices and 3D bias vector for the magnetometers. This is a stripped down version of calibrate_imu (activity), which see.

Two standstill positions are required, with a motion not exceeding thirty times the tsill parameter (2 by default, so 60s of motion). The motion should cover the full SO(3) space for best results.


set_zero (activity)

Inputs
  • double duration (default "10") Averaging time (s)

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.


set_zero_velocity (activity)

Inputs
  • double duration (default "10") Averaging time (s)

Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

Context

Reset gyroscopes bias.

This service updates the 3D bias vector for gyroscopes so that the the gyroscopes return a 0 angular velocity on each axis.

While running this service, the platform should be perfectly standstill. This is a subset of what set_zero (activity) does.

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


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_stopped

    • unsigned short id

  • exception ::rotorcraft::e_rate

    • string<8> what

  • 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_rotor_stopped

    • unsigned short id

  • exception ::rotorcraft::e_input

Context

Control the propellers according to the velocities published in rotor_input (in).

The activity will run until it is stopped by stop (activity) or set_velocity (function). It will also stop and raise the e_input exception if there are no published velocities in rotor_input (in) or if they are older than 500ms.


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 (activity)

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

Context
  • In task main (frequency 1000.0 Hz)

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


get_sensor_average (activity)

Inputs
  • double duration (default "10") Averaging time (s)

Outputs
  • struct ::or::t3d::avel gyr

    • double wx

    • double wy

    • double wz

  • struct ::or::t3d::acc acc

    • double ax

    • double ay

    • double az

  • struct ::or::t3d::pos mag

    • double x

    • double y

    • double z

Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what

Context

Compute gyroscopes, accelerometers and magnetomers average.


Tasks

main

Context

comm

Context
Throws
  • exception ::rotorcraft::e_sys

    • short code

    • string<128> what