PX4 Firmware
PX4 Autopilot Software http://px4.io
|
class OutputBase Base class for all driver output classes More...
#include <output.h>
Public Member Functions | |
OutputBase (const OutputConfig &output_config) | |
virtual | ~OutputBase () |
virtual int | initialize () |
virtual int | update (const ControlData *control_data)=0 |
Update the output. More... | |
virtual void | print_status ()=0 |
report status to stdout More... | |
void | publish () |
Publish _angle_outputs as a mount_orientation message. More... | |
Protected Member Functions | |
float | _calculate_pitch (double lon, double lat, float altitude, const vehicle_global_position_s &global_position) |
void | _set_angle_setpoints (const ControlData *control_data) |
set angle setpoints, speeds & stabilize flags More... | |
void | _handle_position_update (bool force_update=false) |
check if vehicle position changed and update the setpoint angles if in gps mode More... | |
void | _calculate_output_angles (const hrt_abstime &t) |
calculate the _angle_outputs (with speed) and stabilize if needed More... | |
int | _get_vehicle_attitude_sub () const |
Protected Attributes | |
map_projection_reference_s | _projection_reference = {} |
reference to convert (lon, lat) to local [m] More... | |
const OutputConfig & | _config |
const ControlData * | _cur_control_data = nullptr |
float | _angle_setpoints [3] = { 0.f, 0.f, 0.f } |
[rad] More... | |
float | _angle_speeds [3] = { 0.f, 0.f, 0.f } |
bool | _stabilize [3] = { false, false, false } |
float | _angle_outputs [3] = { 0.f, 0.f, 0.f } |
calculated output angles (roll, pitch, yaw) [rad] More... | |
hrt_abstime | _last_update |
Private Attributes | |
int | _vehicle_attitude_sub = -1 |
int | _vehicle_global_position_sub = -1 |
orb_advert_t | _mount_orientation_pub = nullptr |
class OutputBase Base class for all driver output classes
vmount::OutputBase::OutputBase | ( | const OutputConfig & | output_config | ) |
Definition at line 59 of file output.cpp.
References _last_update, and hrt_absolute_time().
|
virtual |
Definition at line 65 of file output.cpp.
References _mount_orientation_pub, _vehicle_attitude_sub, _vehicle_global_position_sub, orb_unadvertise(), and orb_unsubscribe().
|
protected |
calculate the _angle_outputs (with speed) and stabilize if needed
Definition at line 201 of file output.cpp.
References _angle_outputs, _angle_setpoints, _angle_speeds, _last_update, _stabilize, _vehicle_attitude_sub, dt, M_PI_F, orb_copy(), ORB_ID, and vehicle_attitude_s::q.
Referenced by vmount::OutputMavlink::update(), and vmount::OutputRC::update().
|
protected |
Definition at line 105 of file output.cpp.
References _projection_reference, vehicle_global_position_s::alt, vehicle_global_position_s::lat, vehicle_global_position_s::lon, map_projection_init(), map_projection_initialized(), and map_projection_project().
Referenced by _handle_position_update().
|
inlineprotected |
|
protected |
check if vehicle position changed and update the setpoint angles if in gps mode
Definition at line 156 of file output.cpp.
References _angle_setpoints, _calculate_pitch(), _cur_control_data, _vehicle_global_position_sub, vmount::ControlData::TypeData::TypeLonLat::altitude, get_bearing_to_next_waypoint(), vehicle_global_position_s::lat, vmount::ControlData::TypeData::TypeLonLat::lat, vehicle_global_position_s::lon, vmount::ControlData::TypeData::TypeLonLat::lon, vmount::ControlData::LonLat, vmount::ControlData::TypeData::lonlat, M_PI_F, orb_check(), orb_copy(), ORB_ID, vmount::ControlData::TypeData::TypeLonLat::pitch_angle_offset, vmount::ControlData::TypeData::TypeLonLat::pitch_fixed_angle, vmount::ControlData::TypeData::TypeLonLat::roll_angle, vmount::ControlData::type, vmount::ControlData::type_data, matrix::wrap_pi(), vehicle_global_position_s::yaw, and vmount::ControlData::TypeData::TypeLonLat::yaw_angle_offset.
Referenced by _set_angle_setpoints(), vmount::OutputMavlink::update(), and vmount::OutputRC::update().
|
protected |
set angle setpoints, speeds & stabilize flags
Definition at line 122 of file output.cpp.
References _angle_setpoints, _angle_speeds, _cur_control_data, _handle_position_update(), _stabilize, vmount::ControlData::Angle, vmount::ControlData::TypeData::angle, vmount::ControlData::TypeData::TypeAngle::angles, vmount::ControlData::TypeData::TypeAngle::is_speed, vmount::ControlData::LonLat, vmount::ControlData::Neutral, vmount::ControlData::stabilize_axis, vmount::ControlData::type, and vmount::ControlData::type_data.
Referenced by vmount::OutputMavlink::update(), and vmount::OutputRC::update().
|
virtual |
Definition at line 80 of file output.cpp.
References _vehicle_attitude_sub, _vehicle_global_position_sub, ORB_ID, and orb_subscribe().
Referenced by vmount_thread_main().
|
pure virtual |
report status to stdout
Implemented in vmount::OutputRC, and vmount::OutputMavlink.
Referenced by vmount_main().
void vmount::OutputBase::publish | ( | ) |
Publish _angle_outputs as a mount_orientation message.
Definition at line 93 of file output.cpp.
References _angle_outputs, _mount_orientation_pub, mount_orientation_s::attitude_euler_angle, ll40ls::instance, ORB_ID, ORB_PRIO_DEFAULT, and orb_publish_auto().
Referenced by vmount_thread_main().
|
pure virtual |
Update the output.
data | new command if non null |
Implemented in vmount::OutputRC, and vmount::OutputMavlink.
Referenced by vmount_thread_main().
calculated output angles (roll, pitch, yaw) [rad]
Definition at line 119 of file output.h.
Referenced by _calculate_output_angles(), publish(), vmount::OutputMavlink::update(), and vmount::OutputRC::update().
[rad]
Definition at line 112 of file output.h.
Referenced by _calculate_output_angles(), _handle_position_update(), and _set_angle_setpoints().
Definition at line 113 of file output.h.
Referenced by _calculate_output_angles(), and _set_angle_setpoints().
|
protected |
Definition at line 103 of file output.h.
Referenced by vmount::OutputMavlink::update(), and vmount::OutputRC::update().
|
protected |
Definition at line 111 of file output.h.
Referenced by _handle_position_update(), and _set_angle_setpoints().
|
protected |
Definition at line 120 of file output.h.
Referenced by _calculate_output_angles(), OutputBase(), vmount::OutputMavlink::update(), and vmount::OutputRC::update().
|
private |
Definition at line 128 of file output.h.
Referenced by publish(), and ~OutputBase().
|
protected |
reference to convert (lon, lat) to local [m]
Definition at line 101 of file output.h.
Referenced by _calculate_pitch().
|
protected |
Definition at line 114 of file output.h.
Referenced by _calculate_output_angles(), and _set_angle_setpoints().
|
private |
Definition at line 125 of file output.h.
Referenced by _calculate_output_angles(), initialize(), and ~OutputBase().
|
private |
Definition at line 126 of file output.h.
Referenced by _handle_position_update(), initialize(), and ~OutputBase().