48 #include <px4_platform_common/defines.h> 51 #include <mathlib/mathlib.h> 60 : _config(output_config)
98 for (
unsigned i = 0; i < 3; ++i) {
112 float x1, y1, x2, y2;
115 float dx = x1 - x2, dy = y1 - y2;
116 float target_distance = sqrtf(dx * dx + dy * dy);
117 float z = altitude - global_position.
alt;
119 return atan2f(z, target_distance);
126 for (
int i = 0; i < 3; ++i) {
131 switch (control_data->
type) {
133 for (
int i = 0; i < 3; ++i) {
158 bool need_update = force_update;
174 const double &vlat = vehicle_global_position.
lat;
175 const double &vlon = vehicle_global_position.
lon;
206 for (
int i = 0; i < 3; ++i) {
219 for (
int i = 0; i < 3; ++i) {
struct vmount::ControlData::TypeData::TypeLonLat lonlat
float yaw_angle_offset
angular offset for yaw [rad]
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
double lon
longitude in [deg]
const ControlData * _cur_control_data
float _calculate_pitch(double lon, double lat, float altitude, const vehicle_global_position_s &global_position)
int _vehicle_attitude_sub
int _vehicle_global_position_sub
Definition of geo / math functions to perform geodesic calculations.
bool map_projection_initialized(const struct map_projection_reference_s *ref)
Checks if projection given as argument was initialized.
void _handle_position_update(bool force_update=false)
check if vehicle position changed and update the setpoint angles if in gps mode
float pitch_angle_offset
angular offset for pitch [rad]
float pitch_fixed_angle
ignored if < -pi, otherwise use a fixed pitch angle instead of the altitude
void _calculate_output_angles(const hrt_abstime &t)
calculate the _angle_outputs (with speed) and stabilize if needed
float altitude
altitude in [m]
orb_advert_t _mount_orientation_pub
float roll_angle
roll is set to a fixed angle.
int orb_subscribe(const struct orb_metadata *meta)
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
float attitude_euler_angle[3]
double lat
latitude in [deg]
OutputBase(const OutputConfig &output_config)
int orb_unsubscribe(int handle)
float angles[3]
attitude angles (roll, pitch, yaw) in rad, [-pi, +pi] if is_speed[i] == false
float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
Returns the bearing to the next waypoint in radians.
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
This defines the common API between an input and an output of the vmount driver.
Type wrap_pi(Type x)
Wrap value in range [-π, π)
void _set_angle_setpoints(const ControlData *control_data)
set angle setpoints, speeds & stabilize flags
float _angle_setpoints[3]
[rad]
float _angle_outputs[3]
calculated output angles (roll, pitch, yaw) [rad]
bool is_speed[3]
if true, the angle is the angular speed in rad/s
Quaternion< float > Quatf
void publish()
Publish _angle_outputs as a mount_orientation message.
int map_projection_project(const struct map_projection_reference_s *ref, double lat, double lon, float *x, float *y)
int orb_check(int handle, bool *updated)
int orb_unadvertise(orb_advert_t handle)
control the roll, pitch & yaw angle directly
struct vmount::ControlData::TypeData::TypeAngle angle
map_projection_reference_s _projection_reference
reference to convert (lon, lat) to local [m]
int map_projection_init(struct map_projection_reference_s *ref, double lat_0, double lon_0)
Initializes the map transformation given by the argument and sets the timestamp to now...
bool stabilize_axis[3]
whether the vmount driver should stabilize an axis (if the output supports it, this can also be done ...
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
static int orb_publish_auto(const struct orb_metadata *meta, orb_advert_t *handle, const void *data, int *instance, int priority)
Advertise as the publisher of a topic.
union vmount::ControlData::TypeData type_data