ardupilot/libraries/AP_Mount/AP_Mount_Servo.h

66 lines
2.4 KiB
C++

/*
Servo controlled mount backend class
*/
#pragma once
#include "AP_Mount_Backend.h"
#ifndef HAL_MOUNT_SERVO_ENABLED
#define HAL_MOUNT_SERVO_ENABLED HAL_MOUNT_ENABLED
#endif
#if HAL_MOUNT_SERVO_ENABLED
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <SRV_Channel/SRV_Channel.h>
class AP_Mount_Servo : public AP_Mount_Backend
{
public:
// Constructor
AP_Mount_Servo(AP_Mount &frontend, AP_Mount::mount_state &state, uint8_t instance):
AP_Mount_Backend(frontend, state, instance),
_roll_idx(SRV_Channel::k_none),
_tilt_idx(SRV_Channel::k_none),
_pan_idx(SRV_Channel::k_none),
_open_idx(SRV_Channel::k_none)
{
}
// init - performs any required initialisation for this instance
void init() override;
// update mount position - should be called periodically
void update() override;
// returns true if this mount can control its pan (required for multicopters)
bool has_pan_control() const override;
// send_mount_status - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
void send_mount_status(mavlink_channel_t chan) override;
private:
// update body-frame angle outputs from earth-frame targets
void update_angle_outputs(const MountTarget& angle_rad);
// returns closest angle to 'angle' taking into account limits. all angles are in body-frame and degrees * 10
int16_t closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max);
/// moves servo with the given function id to the specified angle. all angles are in body-frame and degrees * 10
void move_servo(uint8_t rc, int16_t angle, int16_t angle_min, int16_t angle_max);
// 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
MountTarget _angle_rad; // angle target
Vector3f _angle_bf_output_deg; // final body frame output angle in degrees
};
#endif // HAL_MOUNT_SERVO_ENABLED