2015-01-08 16:11:12 -04:00
|
|
|
/*
|
|
|
|
Servo controlled mount backend class
|
|
|
|
*/
|
2016-02-17 21:25:38 -04:00
|
|
|
#pragma once
|
2015-01-08 16:11:12 -04:00
|
|
|
|
2022-06-14 01:55:10 -03:00
|
|
|
#include "AP_Mount_Backend.h"
|
|
|
|
|
|
|
|
#ifndef HAL_MOUNT_SERVO_ENABLED
|
|
|
|
#define HAL_MOUNT_SERVO_ENABLED HAL_MOUNT_ENABLED
|
|
|
|
#endif
|
|
|
|
|
|
|
|
#if HAL_MOUNT_SERVO_ENABLED
|
|
|
|
|
2015-08-11 03:28:44 -03:00
|
|
|
#include <AP_Math/AP_Math.h>
|
|
|
|
#include <AP_Common/AP_Common.h>
|
2017-01-05 01:13:02 -04:00
|
|
|
#include <SRV_Channel/SRV_Channel.h>
|
2015-01-08 16:11:12 -04:00
|
|
|
|
|
|
|
class AP_Mount_Servo : public AP_Mount_Backend
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
2022-08-30 08:15:10 -03:00
|
|
|
AP_Mount_Servo(AP_Mount &frontend, AP_Mount_Params ¶ms, bool requires_stab, uint8_t instance):
|
|
|
|
AP_Mount_Backend(frontend, params, instance),
|
2022-08-30 05:20:19 -03:00
|
|
|
requires_stabilization(requires_stab),
|
2017-01-05 01:13:02 -04:00
|
|
|
_roll_idx(SRV_Channel::k_none),
|
|
|
|
_tilt_idx(SRV_Channel::k_none),
|
|
|
|
_pan_idx(SRV_Channel::k_none),
|
2018-07-23 00:47:53 -03:00
|
|
|
_open_idx(SRV_Channel::k_none)
|
2015-01-08 16:11:12 -04:00
|
|
|
{
|
|
|
|
}
|
|
|
|
|
|
|
|
// init - performs any required initialisation for this instance
|
2019-08-27 03:23:30 -03:00
|
|
|
void init() override;
|
2015-01-08 16:11:12 -04:00
|
|
|
|
|
|
|
// update mount position - should be called periodically
|
2018-11-07 20:49:14 -04:00
|
|
|
void update() override;
|
2015-01-08 16:11:12 -04:00
|
|
|
|
2022-06-23 00:39:10 -03:00
|
|
|
// returns true if this mount can control its pan (required for multicopters)
|
|
|
|
bool has_pan_control() const override;
|
2015-01-08 16:11:12 -04:00
|
|
|
|
2022-07-11 05:07:22 -03:00
|
|
|
protected:
|
|
|
|
|
|
|
|
// get attitude as a quaternion. returns true on success
|
|
|
|
bool get_attitude_quaternion(Quaternion& att_quat) override;
|
2015-01-08 16:11:12 -04:00
|
|
|
|
|
|
|
private:
|
|
|
|
|
2022-06-23 00:39:10 -03:00
|
|
|
// update body-frame angle outputs from earth-frame targets
|
|
|
|
void update_angle_outputs(const MountTarget& angle_rad);
|
2015-01-08 16:11:12 -04:00
|
|
|
|
2022-06-23 00:39:10 -03:00
|
|
|
// returns closest angle to 'angle' taking into account limits. all angles are in body-frame and degrees * 10
|
2015-01-08 16:11:12 -04:00
|
|
|
int16_t closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max);
|
|
|
|
|
2022-06-23 00:39:10 -03:00
|
|
|
/// moves servo with the given function id to the specified angle. all angles are in body-frame and degrees * 10
|
2015-01-08 16:11:12 -04:00
|
|
|
void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);
|
|
|
|
|
2022-08-30 05:20:19 -03:00
|
|
|
/// Servo gimbals require stabilization, BrushlessPWM gimbals self-stabilize
|
|
|
|
const bool requires_stabilization;
|
|
|
|
|
2017-01-05 01:13:02 -04:00
|
|
|
// SRV_Channel - different id numbers are used depending upon the instance number
|
|
|
|
SRV_Channel::Aux_servo_function_t _roll_idx; // SRV_Channel mount roll function index
|
|
|
|
SRV_Channel::Aux_servo_function_t _tilt_idx; // SRV_Channel mount tilt function index
|
|
|
|
SRV_Channel::Aux_servo_function_t _pan_idx; // SRV_Channel mount pan function index
|
|
|
|
SRV_Channel::Aux_servo_function_t _open_idx; // SRV_Channel mount open function index
|
2015-01-08 16:11:12 -04:00
|
|
|
|
2022-06-23 00:39:10 -03:00
|
|
|
MountTarget _angle_rad; // angle target
|
2015-01-15 01:11:17 -04:00
|
|
|
Vector3f _angle_bf_output_deg; // final body frame output angle in degrees
|
2015-01-08 16:11:12 -04:00
|
|
|
};
|
2022-06-14 01:55:10 -03:00
|
|
|
#endif // HAL_MOUNT_SERVO_ENABLED
|