ardupilot/libraries/AP_Motors/AP_Motors6DOF.h

78 lines
2.8 KiB
C++

/// @file AP_Motors6DOF.h
/// @brief Motor control class for ROVs with direct control over 6DOF (or fewer) in movement
#pragma once
#include <AP_Common/AP_Common.h>
#include <AP_Math/AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <RC_Channel/RC_Channel.h> // RC Channel Library
#include "AP_MotorsMatrix.h"
/// @class AP_MotorsMatrix
class AP_Motors6DOF : public AP_MotorsMatrix {
public:
AP_Motors6DOF(uint16_t loop_rate, uint16_t speed_hz = AP_MOTORS_SPEED_DEFAULT) :
AP_MotorsMatrix(loop_rate, speed_hz) {
AP_Param::setup_object_defaults(this, var_info);
};
// Supported frame types
typedef enum {
SUB_FRAME_BLUEROV1,
SUB_FRAME_VECTORED,
SUB_FRAME_VECTORED_6DOF,
SUB_FRAME_VECTORED_6DOF_90DEG,
SUB_FRAME_SIMPLEROV_3,
SUB_FRAME_SIMPLEROV_4,
SUB_FRAME_SIMPLEROV_5,
SUB_FRAME_CUSTOM
} sub_frame_t;
// Override parent
void setup_motors(motor_frame_class frame_class, motor_frame_type frame_type) override;
// Override parent
void output_min() override;
// Map thrust input -1~1 to pwm output 1100~1900
int16_t calc_thrust_to_pwm(float thrust_in) const;
// output_to_motors - sends minimum values out to the motors
void output_to_motors() override;
// returns a vector with roll, pitch, and yaw contributions
Vector3f get_motor_angular_factors(int motor_number);
// returns true if motor is enabled
bool motor_is_enabled(int motor_number);
bool set_reversed(int motor_number, bool reversed);
// var_info for holding Parameter information
static const struct AP_Param::GroupInfo var_info[];
protected:
// return current_limit as a number from 0 ~ 1 in the range throttle_min to throttle_max
float get_current_limit_max_throttle() override;
//Override MotorsMatrix method
void add_motor_raw_6dof(int8_t motor_num, float roll_fac, float pitch_fac, float yaw_fac, float climb_fac, float forward_fac, float lat_fac, uint8_t testing_order);
void output_armed_stabilizing() override;
void output_armed_stabilizing_vectored();
void output_armed_stabilizing_vectored_6dof();
// Parameters
AP_Int8 _motor_reverse[AP_MOTORS_MAX_NUM_MOTORS];
AP_Float _forwardVerticalCouplingFactor;
float _throttle_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to throttle (climb/descent)
float _forward_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to forward/backward
float _lateral_factor[AP_MOTORS_MAX_NUM_MOTORS]; // each motors contribution to lateral (left/right)
// current limiting
float _output_limited = 1.0f;
float _batt_current_last = 0.0f;
};