ardupilot/libraries/AP_Mount/AP_Mount_Servo.h

79 lines
3.0 KiB
C++

/*
Servo controlled mount backend class
*/
#pragma once
#include <AP_Math/AP_Math.h>
#include <AP_Common/AP_Common.h>
#include <AP_GPS/AP_GPS.h>
#include <AP_AHRS/AP_AHRS.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <SRV_Channel/SRV_Channel.h>
#include "AP_Mount_Backend.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),
_last_check_servo_map_ms(0)
{
// init to no axis being controlled
_flags.roll_control = false;
_flags.tilt_control = false;
_flags.pan_control = false;
}
// init - performs any required initialisation for this instance
virtual void init(const AP_SerialManager& serial_manager);
// update mount position - should be called periodically
virtual void update();
// has_pan_control - returns true if this mount can control it's pan (required for multicopters)
virtual bool has_pan_control() const { return _flags.pan_control; }
// set_mode - sets mount's mode
virtual void set_mode(enum MAV_MOUNT_MODE mode);
// status_msg - called to allow mounts to send their status to GCS using the MOUNT_STATUS message
virtual void status_msg(mavlink_channel_t chan);
private:
// flags structure
struct {
bool roll_control :1; // true if mount has roll control
bool tilt_control :1; // true if mount has tilt control
bool pan_control :1; // true if mount has pan control
} _flags;
// check_servo_map - detects which axis we control (i.e. _flags) using the functions assigned to the servos in the SRV_Channel
// should be called periodically (i.e. 1hz or less)
void check_servo_map();
// stabilize - stabilizes the mount relative to the Earth's frame
void stabilize();
// closest_limit - returns closest angle to 'angle' taking into account limits. all angles are in degrees * 10
int16_t closest_limit(int16_t angle, int16_t angle_min, int16_t angle_max);
/// move_servo - moves servo with the given id to the specified angle. all angles are in 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
Vector3f _angle_bf_output_deg; // final body frame output angle in degrees
uint32_t _last_check_servo_map_ms; // system time of latest call to check_servo_map function
};