PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <VehicleAcceleration.hpp>
Public Member Functions | |
VehicleAcceleration () | |
virtual | ~VehicleAcceleration () |
void | Run () override |
bool | Start () |
void | Stop () |
void | PrintStatus () |
Private Member Functions | |
void | ParametersUpdate (bool force=false) |
void | SensorBiasUpdate (bool force=false) |
bool | SensorCorrectionsUpdate (bool force=false) |
Private Attributes | |
DEFINE_PARAMETERS((ParamInt< px4::params::SENS_BOARD_ROT >) _param_sens_board_rot,(ParamFloat< px4::params::SENS_BOARD_X_OFF >) _param_sens_board_x_off,(ParamFloat< px4::params::SENS_BOARD_Y_OFF >) _param_sens_board_y_off,(ParamFloat< px4::params::SENS_BOARD_Z_OFF >) _param_sens_board_z_off) uORB uORB::Subscription | _params_sub {ORB_ID(parameter_update)} |
parameter updates subscription More... | |
uORB::Subscription | _sensor_bias_sub {ORB_ID(sensor_bias)} |
sensor in-run bias correction subscription More... | |
uORB::Subscription | _sensor_correction_sub {ORB_ID(sensor_correction)} |
sensor thermal correction subscription More... | |
uORB::SubscriptionCallbackWorkItem | _sensor_selection_sub {this, ORB_ID(sensor_selection)} |
selected primary sensor subscription More... | |
uORB::SubscriptionCallbackWorkItem | _sensor_sub [MAX_SENSOR_COUNT] |
sensor data subscription More... | |
matrix::Dcmf | _board_rotation |
rotation matrix for the orientation that the board is mounted More... | |
matrix::Vector3f | _offset |
matrix::Vector3f | _scale |
matrix::Vector3f | _bias |
uint8_t | _selected_sensor {0} |
Static Private Attributes | |
static constexpr int | MAX_SENSOR_COUNT = 3 |
Definition at line 54 of file VehicleAcceleration.hpp.
VehicleAcceleration::VehicleAcceleration | ( | ) |
Definition at line 41 of file VehicleAcceleration.cpp.
|
virtual |
Definition at line 47 of file VehicleAcceleration.cpp.
References Stop().
|
private |
Definition at line 147 of file VehicleAcceleration.cpp.
References _board_rotation, _params_sub, uORB::Subscription::copy(), get_rot_matrix(), math::radians(), and uORB::Subscription::updated().
Referenced by Run(), and Start().
void VehicleAcceleration::PrintStatus | ( | ) |
Definition at line 205 of file VehicleAcceleration.cpp.
References _selected_sensor.
Referenced by Sensors::print_status().
|
override |
Definition at line 171 of file VehicleAcceleration.cpp.
References _bias, _board_rotation, _offset, _scale, _selected_sensor, _sensor_sub, uORB::SubscriptionInterval::copy(), hrt_absolute_time(), ParametersUpdate(), SensorBiasUpdate(), SensorCorrectionsUpdate(), sensor_accel_s::timestamp, vehicle_acceleration_s::timestamp_sample, sensor_accel_s::x, sensor_accel_s::y, and sensor_accel_s::z.
|
private |
Definition at line 84 of file VehicleAcceleration.cpp.
References _bias, _sensor_bias_sub, sensor_bias_s::accel_bias, uORB::Subscription::copy(), and uORB::Subscription::updated().
Referenced by Run(), and Start().
|
private |
Definition at line 97 of file VehicleAcceleration.cpp.
References _offset, _scale, _selected_sensor, _sensor_correction_sub, _sensor_sub, uORB::Subscription::copy(), MAX_SENSOR_COUNT, and uORB::Subscription::updated().
Referenced by Run(), and Start().
bool VehicleAcceleration::Start | ( | ) |
Definition at line 53 of file VehicleAcceleration.cpp.
References _bias, _offset, _scale, _sensor_selection_sub, ParametersUpdate(), uORB::SubscriptionCallback::registerCallback(), SensorBiasUpdate(), SensorCorrectionsUpdate(), and matrix::Matrix< Type, M, N >::zero().
Referenced by Sensors::Sensors().
void VehicleAcceleration::Stop | ( | ) |
Definition at line 71 of file VehicleAcceleration.cpp.
References _sensor_selection_sub, _sensor_sub, and uORB::SubscriptionCallback::unregisterCallback().
Referenced by Sensors::~Sensors(), and ~VehicleAcceleration().
|
private |
Definition at line 101 of file VehicleAcceleration.hpp.
Referenced by Run(), SensorBiasUpdate(), and Start().
|
private |
rotation matrix for the orientation that the board is mounted
Definition at line 97 of file VehicleAcceleration.hpp.
Referenced by ParametersUpdate(), and Run().
|
private |
Definition at line 99 of file VehicleAcceleration.hpp.
Referenced by Run(), SensorCorrectionsUpdate(), and Start().
|
private |
parameter updates subscription
Definition at line 86 of file VehicleAcceleration.hpp.
Referenced by ParametersUpdate().
|
private |
Definition at line 100 of file VehicleAcceleration.hpp.
Referenced by Run(), SensorCorrectionsUpdate(), and Start().
|
private |
Definition at line 103 of file VehicleAcceleration.hpp.
Referenced by PrintStatus(), Run(), and SensorCorrectionsUpdate().
|
private |
sensor in-run bias correction subscription
Definition at line 87 of file VehicleAcceleration.hpp.
Referenced by SensorBiasUpdate().
|
private |
sensor thermal correction subscription
Definition at line 88 of file VehicleAcceleration.hpp.
Referenced by SensorCorrectionsUpdate().
|
private |
selected primary sensor subscription
Definition at line 90 of file VehicleAcceleration.hpp.
|
private |
sensor data subscription
Definition at line 91 of file VehicleAcceleration.hpp.
Referenced by Run(), SensorCorrectionsUpdate(), and Stop().
|
staticprivate |
Definition at line 74 of file VehicleAcceleration.hpp.
Referenced by SensorCorrectionsUpdate().