Copter: combined tri, single, coax and multicopter into a single build
this allows copter to be just 2 builds, one for heli, and one for everything else
This commit is contained in:
parent
5cf1c0869d
commit
0f6d0c5ba9
@ -261,7 +261,7 @@ void Copter::fast_loop()
|
||||
read_AHRS();
|
||||
|
||||
// run low level rate controllers that only require IMU data
|
||||
attitude_control.rate_controller_run();
|
||||
attitude_control->rate_controller_run();
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
update_heli_control_dynamics();
|
||||
@ -362,7 +362,7 @@ void Copter::update_batt_compass(void)
|
||||
|
||||
if(g.compass_enabled) {
|
||||
// update compass with throttle value - used for compassmot
|
||||
compass.set_throttle(motors.get_throttle());
|
||||
compass.set_throttle(motors->get_throttle());
|
||||
compass.read();
|
||||
// log compass information
|
||||
if (should_log(MASK_LOG_COMPASS) && !ahrs.have_ekf_logging()) {
|
||||
@ -378,11 +378,11 @@ void Copter::ten_hz_logging_loop()
|
||||
// log attitude data if we're not already logging at the higher rate
|
||||
if (should_log(MASK_LOG_ATTITUDE_MED) && !should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
|
||||
DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
||||
if (should_log(MASK_LOG_PID)) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
|
||||
}
|
||||
}
|
||||
@ -405,7 +405,7 @@ void Copter::ten_hz_logging_loop()
|
||||
DataFlash.Log_Write_Vibration(ins);
|
||||
}
|
||||
if (should_log(MASK_LOG_CTUN)) {
|
||||
attitude_control.control_monitor_log();
|
||||
attitude_control->control_monitor_log();
|
||||
Log_Write_Proximity();
|
||||
Log_Write_Beacon();
|
||||
}
|
||||
@ -425,11 +425,11 @@ void Copter::twentyfive_hz_logging()
|
||||
#if HIL_MODE == HIL_MODE_DISABLED
|
||||
if (should_log(MASK_LOG_ATTITUDE_FAST)) {
|
||||
Log_Write_Attitude();
|
||||
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
|
||||
DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
||||
if (should_log(MASK_LOG_PID)) {
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control.get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control.get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control.get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDR_MSG, attitude_control->get_rate_roll_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDP_MSG, attitude_control->get_rate_pitch_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDY_MSG, attitude_control->get_rate_yaw_pid().get_pid_info());
|
||||
DataFlash.Log_Write_PID(LOG_PIDA_MSG, g.pid_accel_z.get_pid_info() );
|
||||
}
|
||||
}
|
||||
@ -484,18 +484,18 @@ void Copter::one_hz_loop()
|
||||
|
||||
update_arming_checks();
|
||||
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
// make it possible to change ahrs orientation at runtime during initial config
|
||||
ahrs.set_orientation();
|
||||
|
||||
update_using_interlock();
|
||||
|
||||
// check the user hasn't updated the frame class or type
|
||||
motors.set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
|
||||
motors->set_frame_class_and_type((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// set all throttle channel settings
|
||||
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
|
||||
motors->set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -1,6 +1,6 @@
|
||||
#include "Copter.h"
|
||||
|
||||
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw
|
||||
// get_smoothing_gain - returns smoothing gain to be passed into attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw
|
||||
// result is a number from 2 to 12 with 2 being very sluggish and 12 being very crisp
|
||||
float Copter::get_smoothing_gain()
|
||||
{
|
||||
@ -107,7 +107,7 @@ void Copter::update_throttle_hover()
|
||||
{
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// if not armed or landed exit
|
||||
if (!motors.armed() || ap.land_complete) {
|
||||
if (!motors->armed() || ap.land_complete) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -117,17 +117,17 @@ void Copter::update_throttle_hover()
|
||||
}
|
||||
|
||||
// do not update while climbing or descending
|
||||
if (!is_zero(pos_control.get_desired_velocity().z)) {
|
||||
if (!is_zero(pos_control->get_desired_velocity().z)) {
|
||||
return;
|
||||
}
|
||||
|
||||
// get throttle output
|
||||
float throttle = motors.get_throttle();
|
||||
float throttle = motors->get_throttle();
|
||||
|
||||
// calc average throttle if we are in a level hover
|
||||
if (throttle > 0.0f && abs(climb_rate) < 60 && labs(ahrs.roll_sensor) < 500 && labs(ahrs.pitch_sensor) < 500) {
|
||||
// Can we set the time constant automatically
|
||||
motors.update_throttle_hover(0.01f);
|
||||
motors->update_throttle_hover(0.01f);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
@ -136,7 +136,7 @@ void Copter::update_throttle_hover()
|
||||
void Copter::set_throttle_takeoff()
|
||||
{
|
||||
// tell position controller to reset alt target and reset I terms
|
||||
pos_control.init_takeoff();
|
||||
pos_control->init_takeoff();
|
||||
}
|
||||
|
||||
// transform pilot's manual throttle input to make hover throttle mid stick
|
||||
@ -146,7 +146,7 @@ void Copter::set_throttle_takeoff()
|
||||
float Copter::get_pilot_desired_throttle(int16_t throttle_control, float thr_mid)
|
||||
{
|
||||
if (thr_mid <= 0.0f) {
|
||||
thr_mid = motors.get_throttle_hover();
|
||||
thr_mid = motors->get_throttle_hover();
|
||||
}
|
||||
|
||||
int16_t mid_stick = channel_throttle->get_control_mid();
|
||||
@ -218,7 +218,7 @@ float Copter::get_pilot_desired_climb_rate(float throttle_control)
|
||||
// get_non_takeoff_throttle - a throttle somewhere between min and mid throttle which should not lead to a takeoff
|
||||
float Copter::get_non_takeoff_throttle()
|
||||
{
|
||||
return MAX(0,motors.get_throttle_hover()/2.0f);
|
||||
return MAX(0,motors->get_throttle_hover()/2.0f);
|
||||
}
|
||||
|
||||
// get_surface_tracking_climb_rate - hold copter at the desired distance above the ground
|
||||
@ -240,7 +240,7 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
|
||||
last_call_ms = now;
|
||||
|
||||
// adjust rangefinder target alt if motors have not hit their limits
|
||||
if ((target_rate<0 && !motors.limit.throttle_lower) || (target_rate>0 && !motors.limit.throttle_upper)) {
|
||||
if ((target_rate<0 && !motors->limit.throttle_lower) || (target_rate>0 && !motors->limit.throttle_upper)) {
|
||||
target_rangefinder_alt += target_rate * dt;
|
||||
}
|
||||
|
||||
@ -286,9 +286,9 @@ float Copter::get_surface_tracking_climb_rate(int16_t target_rate, float current
|
||||
void Copter::set_accel_throttle_I_from_pilot_throttle()
|
||||
{
|
||||
// get last throttle input sent to attitude controller
|
||||
float pilot_throttle = constrain_float(attitude_control.get_throttle_in(), 0.0f, 1.0f);
|
||||
float pilot_throttle = constrain_float(attitude_control->get_throttle_in(), 0.0f, 1.0f);
|
||||
// shift difference between pilot's throttle and hover throttle into accelerometer I
|
||||
g.pid_accel_z.set_integrator((pilot_throttle-motors.get_throttle_hover()) * 1000.0f);
|
||||
g.pid_accel_z.set_integrator((pilot_throttle-motors->get_throttle_hover()) * 1000.0f);
|
||||
}
|
||||
|
||||
// updates position controller's maximum altitude using fence and EKF limits
|
||||
@ -312,7 +312,7 @@ void Copter::update_poscon_alt_max()
|
||||
}
|
||||
|
||||
// pass limit to pos controller
|
||||
pos_control.set_alt_max(alt_limit_cm);
|
||||
pos_control->set_alt_max(alt_limit_cm);
|
||||
}
|
||||
|
||||
// rotate vector from vehicle's perspective to North-East frame
|
||||
|
@ -28,7 +28,6 @@ Copter::Copter(void) :
|
||||
FUNCTOR_BIND_MEMBER(&Copter::verify_command_callback, bool, const AP_Mission::Mission_Command &),
|
||||
FUNCTOR_BIND_MEMBER(&Copter::exit_mission, void)),
|
||||
control_mode(STABILIZE),
|
||||
motors(MAIN_LOOP_RATE),
|
||||
scaleLongDown(1),
|
||||
wp_bearing(0),
|
||||
home_bearing(0),
|
||||
@ -66,12 +65,6 @@ Copter::Copter(void) :
|
||||
condition_start(0),
|
||||
G_Dt(MAIN_LOOP_SECONDS),
|
||||
inertial_nav(ahrs),
|
||||
attitude_control(ahrs, aparm, motors, MAIN_LOOP_SECONDS),
|
||||
pos_control(ahrs, inertial_nav, motors, attitude_control,
|
||||
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
|
||||
g.p_pos_xy, g.pi_vel_xy),
|
||||
wp_nav(inertial_nav, ahrs, pos_control, attitude_control),
|
||||
circle_nav(inertial_nav, ahrs, pos_control),
|
||||
pmTest1(0),
|
||||
fast_loopTimer(0),
|
||||
mainLoop_count(0),
|
||||
|
@ -326,21 +326,13 @@ private:
|
||||
} sensor_health;
|
||||
|
||||
// Motor Output
|
||||
#if FRAME_CONFIG == QUAD_FRAME || FRAME_CONFIG == HEXA_FRAME || FRAME_CONFIG == Y6_FRAME || FRAME_CONFIG == OCTA_FRAME || FRAME_CONFIG == OCTA_QUAD_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsMatrix
|
||||
#elif FRAME_CONFIG == TRI_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsTri
|
||||
#elif FRAME_CONFIG == HELI_FRAME
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsHeli_Single
|
||||
#elif FRAME_CONFIG == SINGLE_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsSingle
|
||||
#elif FRAME_CONFIG == COAX_FRAME
|
||||
#define MOTOR_CLASS AP_MotorsCoax
|
||||
#else
|
||||
#error Unrecognised frame type
|
||||
#define MOTOR_CLASS AP_MotorsMulticopter
|
||||
#endif
|
||||
|
||||
MOTOR_CLASS motors;
|
||||
MOTOR_CLASS *motors;
|
||||
|
||||
// GPS variables
|
||||
// Sometimes we need to remove the scaling for distance calcs
|
||||
@ -491,14 +483,10 @@ private:
|
||||
|
||||
// Attitude, Position and Waypoint navigation objects
|
||||
// To-Do: move inertial nav up or other navigation variables down here
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
AC_AttitudeControl_Heli attitude_control;
|
||||
#else
|
||||
AC_AttitudeControl_Multi attitude_control;
|
||||
#endif
|
||||
AC_PosControl pos_control;
|
||||
AC_WPNav wp_nav;
|
||||
AC_Circle circle_nav;
|
||||
AC_AttitudeControl *attitude_control;
|
||||
AC_PosControl *pos_control;
|
||||
AC_WPNav *wp_nav;
|
||||
AC_Circle *circle_nav;
|
||||
|
||||
// Performance monitoring
|
||||
int16_t pmTest1;
|
||||
@ -1085,6 +1073,7 @@ private:
|
||||
void check_usb_mux(void);
|
||||
bool should_log(uint32_t mask);
|
||||
void set_default_frame_class();
|
||||
void allocate_motors(void);
|
||||
uint8_t get_frame_mav_type();
|
||||
const char* get_frame_string();
|
||||
bool current_mode_has_user_takeoff(bool must_navigate);
|
||||
|
@ -71,7 +71,7 @@ NOINLINE void Copter::send_heartbeat(mavlink_channel_t chan)
|
||||
#endif
|
||||
|
||||
// we are armed if we are not initialising
|
||||
if (motors.armed()) {
|
||||
if (motors->armed()) {
|
||||
base_mode |= MAV_MODE_FLAG_SAFETY_ARMED;
|
||||
}
|
||||
|
||||
@ -161,7 +161,7 @@ void NOINLINE Copter::send_location(mavlink_channel_t chan)
|
||||
|
||||
void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
|
||||
{
|
||||
const Vector3f &targets = attitude_control.get_att_target_euler_cd();
|
||||
const Vector3f &targets = attitude_control->get_att_target_euler_cd();
|
||||
mavlink_msg_nav_controller_output_send(
|
||||
chan,
|
||||
targets.x / 1.0e2f,
|
||||
@ -169,7 +169,7 @@ void NOINLINE Copter::send_nav_controller_output(mavlink_channel_t chan)
|
||||
targets.z / 1.0e2f,
|
||||
wp_bearing / 1.0e2f,
|
||||
wp_distance / 1.0e2f,
|
||||
pos_control.get_alt_error() / 1.0e2f,
|
||||
pos_control->get_alt_error() / 1.0e2f,
|
||||
0,
|
||||
0);
|
||||
}
|
||||
@ -197,7 +197,7 @@ void NOINLINE Copter::send_vfr_hud(mavlink_channel_t chan)
|
||||
gps.ground_speed(),
|
||||
ahrs.groundspeed(),
|
||||
(ahrs.yaw_sensor / 100) % 360,
|
||||
(int16_t)(motors.get_throttle() * 100),
|
||||
(int16_t)(motors->get_throttle() * 100),
|
||||
current_loc.alt / 100.0f,
|
||||
climb_rate / 100.0f);
|
||||
}
|
||||
@ -279,7 +279,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
|
||||
{
|
||||
const Vector3f &gyro = ahrs.get_gyro();
|
||||
if (g.gcs_pid_mask & 1) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_roll_pid().get_pid_info();
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_roll_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_ROLL,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.x),
|
||||
@ -292,7 +292,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
|
||||
}
|
||||
}
|
||||
if (g.gcs_pid_mask & 2) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_pitch_pid().get_pid_info();
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_pitch_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_PITCH,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.y),
|
||||
@ -305,7 +305,7 @@ void Copter::send_pid_tuning(mavlink_channel_t chan)
|
||||
}
|
||||
}
|
||||
if (g.gcs_pid_mask & 4) {
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control.get_rate_yaw_pid().get_pid_info();
|
||||
const DataFlash_Class::PID_Info &pid_info = attitude_control->get_rate_yaw_pid().get_pid_info();
|
||||
mavlink_msg_pid_tuning_send(chan, PID_TUNING_YAW,
|
||||
pid_info.desired*0.01f,
|
||||
degrees(gyro.z),
|
||||
@ -348,7 +348,7 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
|
||||
// if we don't have at least 250 micros remaining before the main loop
|
||||
// wants to fire then don't send a mavlink message. We want to
|
||||
// prioritise the main flight control loop over communications
|
||||
if (copter.scheduler.time_available_usec() < 250 && copter.motors.armed()) {
|
||||
if (copter.scheduler.time_available_usec() < 250 && copter.motors->armed()) {
|
||||
copter.gcs_out_of_time = true;
|
||||
return false;
|
||||
}
|
||||
@ -683,7 +683,7 @@ GCS_MAVLINK_Copter::data_stream_send(void)
|
||||
return;
|
||||
}
|
||||
|
||||
if (!copter.in_mavlink_delay && !copter.motors.armed()) {
|
||||
if (!copter.in_mavlink_delay && !copter.motors->armed()) {
|
||||
handle_log_send(copter.DataFlash);
|
||||
}
|
||||
|
||||
@ -1089,7 +1089,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
// param3 : unused
|
||||
// param4 : unused
|
||||
if (packet.param2 > 0.0f) {
|
||||
copter.wp_nav.set_speed_xy(packet.param2 * 100.0f);
|
||||
copter.wp_nav->set_speed_xy(packet.param2 * 100.0f);
|
||||
result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
result = MAV_RESULT_FAILED;
|
||||
@ -1182,7 +1182,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
break;
|
||||
|
||||
case MAV_CMD_MISSION_START:
|
||||
if (copter.motors.armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
|
||||
if (copter.motors->armed() && copter.set_mode(AUTO, MODE_REASON_GCS_COMMAND)) {
|
||||
copter.set_auto_armed(true);
|
||||
if (copter.mission.state() != AP_Mission::MISSION_RUNNING) {
|
||||
copter.mission.start_or_resume();
|
||||
@ -1193,7 +1193,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
|
||||
case MAV_CMD_PREFLIGHT_CALIBRATION:
|
||||
// exit immediately if armed
|
||||
if (copter.motors.armed()) {
|
||||
if (copter.motors->armed()) {
|
||||
result = MAV_RESULT_FAILED;
|
||||
break;
|
||||
}
|
||||
@ -1449,7 +1449,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
break;
|
||||
}
|
||||
|
||||
if (!copter.motors.armed()) {
|
||||
if (!copter.motors->armed()) {
|
||||
// if disarmed, arm motors
|
||||
copter.init_arm_motors(true);
|
||||
} else if (copter.ap.land_complete) {
|
||||
@ -1472,7 +1472,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
break;
|
||||
}
|
||||
|
||||
if (copter.motors.armed()) {
|
||||
if (copter.motors->armed()) {
|
||||
if (copter.ap.land_complete) {
|
||||
// if landed, disarm motors
|
||||
copter.init_disarm_motors();
|
||||
@ -1543,10 +1543,10 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
climb_rate_cms = 0.0f;
|
||||
} else if (packet.thrust > 0.5f) {
|
||||
// climb at up to WPNAV_SPEED_UP
|
||||
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav.get_speed_up();
|
||||
climb_rate_cms = (packet.thrust - 0.5f) * 2.0f * copter.wp_nav->get_speed_up();
|
||||
} else {
|
||||
// descend at up to WPNAV_SPEED_DN
|
||||
climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav.get_speed_down());
|
||||
climb_rate_cms = (0.5f - packet.thrust) * 2.0f * -fabsf(copter.wp_nav->get_speed_down());
|
||||
}
|
||||
|
||||
// if the body_yaw_rate field is ignored, use the commanded yaw position
|
||||
@ -1797,13 +1797,13 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg)
|
||||
copter.in_log_download = true;
|
||||
/* no break */
|
||||
case MAVLINK_MSG_ID_LOG_REQUEST_LIST:
|
||||
if (!copter.in_mavlink_delay && !copter.motors.armed()) {
|
||||
if (!copter.in_mavlink_delay && !copter.motors->armed()) {
|
||||
handle_log_message(msg, copter.DataFlash);
|
||||
}
|
||||
break;
|
||||
case MAVLINK_MSG_ID_LOG_REQUEST_END:
|
||||
copter.in_log_download = false;
|
||||
if (!copter.in_mavlink_delay && !copter.motors.armed()) {
|
||||
if (!copter.in_mavlink_delay && !copter.motors->armed()) {
|
||||
handle_log_message(msg, copter.DataFlash);
|
||||
}
|
||||
break;
|
||||
|
@ -271,9 +271,9 @@ struct PACKED log_Nav_Tuning {
|
||||
// Write an Nav Tuning packet
|
||||
void Copter::Log_Write_Nav_Tuning()
|
||||
{
|
||||
const Vector3f &pos_target = pos_control.get_pos_target();
|
||||
const Vector3f &vel_target = pos_control.get_vel_target();
|
||||
const Vector3f &accel_target = pos_control.get_accel_target();
|
||||
const Vector3f &pos_target = pos_control->get_pos_target();
|
||||
const Vector3f &vel_target = pos_control->get_vel_target();
|
||||
const Vector3f &accel_target = pos_control->get_accel_target();
|
||||
const Vector3f &position = inertial_nav.get_position();
|
||||
const Vector3f &velocity = inertial_nav.get_velocity();
|
||||
|
||||
@ -325,17 +325,17 @@ void Copter::Log_Write_Control_Tuning()
|
||||
struct log_Control_Tuning pkt = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_CONTROL_TUNING_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
throttle_in : attitude_control.get_throttle_in(),
|
||||
angle_boost : attitude_control.angle_boost(),
|
||||
throttle_out : motors.get_throttle(),
|
||||
throttle_hover : motors.get_throttle_hover(),
|
||||
desired_alt : pos_control.get_alt_target() / 100.0f,
|
||||
throttle_in : attitude_control->get_throttle_in(),
|
||||
angle_boost : attitude_control->angle_boost(),
|
||||
throttle_out : motors->get_throttle(),
|
||||
throttle_hover : motors->get_throttle_hover(),
|
||||
desired_alt : pos_control->get_alt_target() / 100.0f,
|
||||
inav_alt : inertial_nav.get_altitude() / 100.0f,
|
||||
baro_alt : baro_alt,
|
||||
desired_rangefinder_alt : (int16_t)target_rangefinder_alt,
|
||||
rangefinder_alt : rangefinder_state.alt_cm,
|
||||
terr_alt : terr_alt,
|
||||
target_climb_rate : (int16_t)pos_control.get_vel_target_z(),
|
||||
target_climb_rate : (int16_t)pos_control->get_vel_target_z(),
|
||||
climb_rate : climb_rate
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt, sizeof(pkt));
|
||||
@ -373,7 +373,7 @@ void Copter::Log_Write_Performance()
|
||||
// Write an attitude packet
|
||||
void Copter::Log_Write_Attitude()
|
||||
{
|
||||
Vector3f targets = attitude_control.get_att_target_euler_cd();
|
||||
Vector3f targets = attitude_control->get_att_target_euler_cd();
|
||||
targets.z = wrap_360_cd(targets.z);
|
||||
DataFlash.Log_Write_Attitude(ahrs, targets);
|
||||
|
||||
@ -405,10 +405,10 @@ void Copter::Log_Write_MotBatt()
|
||||
struct log_MotBatt pkt_mot = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_MOTBATT_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
lift_max : (float)(motors.get_lift_max()),
|
||||
bat_volt : (float)(motors.get_batt_voltage_filt()),
|
||||
bat_res : (float)(motors.get_batt_resistance()),
|
||||
th_limit : (float)(motors.get_throttle_limit())
|
||||
lift_max : (float)(motors->get_lift_max()),
|
||||
bat_volt : (float)(motors->get_batt_voltage_filt()),
|
||||
bat_res : (float)(motors->get_batt_resistance()),
|
||||
th_limit : (float)(motors->get_throttle_limit())
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt_mot, sizeof(pkt_mot));
|
||||
#endif
|
||||
@ -637,8 +637,8 @@ void Copter::Log_Write_Heli()
|
||||
struct log_Heli pkt_heli = {
|
||||
LOG_PACKET_HEADER_INIT(LOG_HELI_MSG),
|
||||
time_us : AP_HAL::micros64(),
|
||||
desired_rotor_speed : motors.get_desired_rotor_speed(),
|
||||
main_rotor_speed : motors.get_main_rotor_speed(),
|
||||
desired_rotor_speed : motors->get_desired_rotor_speed(),
|
||||
main_rotor_speed : motors->get_main_rotor_speed(),
|
||||
};
|
||||
DataFlash.WriteBlock(&pkt_heli, sizeof(pkt_heli));
|
||||
}
|
||||
|
@ -18,8 +18,8 @@ AP_AdvancedFailsafe_Copter::AP_AdvancedFailsafe_Copter(AP_Mission &_mission, AP_
|
||||
void AP_AdvancedFailsafe_Copter::terminate_vehicle(void)
|
||||
{
|
||||
// stop motors
|
||||
copter.motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
copter.motors.output();
|
||||
copter.motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
copter.motors->output();
|
||||
|
||||
// disarm as well
|
||||
copter.init_disarm_motors();
|
||||
@ -47,8 +47,8 @@ void AP_AdvancedFailsafe_Copter::setup_IO_failsafe(void)
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// setup AP_Motors outputs for failsafe
|
||||
uint16_t mask = copter.motors.get_motor_mask();
|
||||
hal.rcout->set_failsafe_pwm(mask, copter.motors.get_pwm_output_min());
|
||||
uint16_t mask = copter.motors->get_motor_mask();
|
||||
hal.rcout->set_failsafe_pwm(mask, copter.motors->get_pwm_output_min());
|
||||
#endif
|
||||
}
|
||||
|
||||
|
@ -34,7 +34,7 @@ bool Copter::all_arming_checks_passing(bool arming_from_gcs)
|
||||
bool Copter::pre_arm_checks(bool display_failure)
|
||||
{
|
||||
// exit immediately if already armed
|
||||
if (motors.armed()) {
|
||||
if (motors->armed()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -360,7 +360,7 @@ bool Copter::parameter_checks(bool display_failure)
|
||||
}
|
||||
|
||||
// acro balance parameter check
|
||||
if ((g.acro_balance_roll > attitude_control.get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control.get_angle_pitch_p().kP())) {
|
||||
if ((g.acro_balance_roll > attitude_control->get_angle_roll_p().kP()) || (g.acro_balance_pitch > attitude_control->get_angle_pitch_p().kP())) {
|
||||
if (display_failure) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: ACRO_BAL_ROLL/PITCH");
|
||||
}
|
||||
@ -379,7 +379,7 @@ bool Copter::parameter_checks(bool display_failure)
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// check helicopter parameters
|
||||
if (!motors.parameter_check(display_failure)) {
|
||||
if (!motors->parameter_check(display_failure)) {
|
||||
return false;
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
@ -410,7 +410,7 @@ bool Copter::parameter_checks(bool display_failure)
|
||||
bool Copter::motor_checks(bool display_failure)
|
||||
{
|
||||
// check motors initialised correctly
|
||||
if (!motors.initialised_ok()) {
|
||||
if (!motors->initialised_ok()) {
|
||||
if (display_failure) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"PreArm: check firmware or FRAME_CLASS");
|
||||
}
|
||||
@ -746,7 +746,7 @@ bool Copter::arm_checks(bool display_failure, bool arming_from_gcs)
|
||||
|
||||
// if we are using motor interlock switch and it's enabled, fail to arm
|
||||
// skip check in Throw mode which takes control of the motor interlock
|
||||
if (ap.using_interlock && motors.get_interlock()) {
|
||||
if (ap.using_interlock && motors->get_interlock()) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL,"Arm: Motor Interlock Enabled");
|
||||
return false;
|
||||
}
|
||||
|
@ -172,9 +172,9 @@ bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle
|
||||
// get best vector away from obstacle
|
||||
Vector3f velocity_neu;
|
||||
if (should_climb) {
|
||||
velocity_neu.z = copter.wp_nav.get_speed_up();
|
||||
velocity_neu.z = copter.wp_nav->get_speed_up();
|
||||
} else {
|
||||
velocity_neu.z = -copter.wp_nav.get_speed_down();
|
||||
velocity_neu.z = -copter.wp_nav->get_speed_down();
|
||||
// do not descend if below RTL alt
|
||||
if (copter.current_loc.alt < copter.g.rtl_altitude) {
|
||||
velocity_neu.z = 0.0f;
|
||||
@ -205,8 +205,8 @@ bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstac
|
||||
// re-normalise
|
||||
velocity_neu.normalize();
|
||||
// convert horizontal components to velocities
|
||||
velocity_neu.x *= copter.wp_nav.get_speed_xy();
|
||||
velocity_neu.y *= copter.wp_nav.get_speed_xy();
|
||||
velocity_neu.x *= copter.wp_nav->get_speed_xy();
|
||||
velocity_neu.y *= copter.wp_nav->get_speed_xy();
|
||||
// send target velocity
|
||||
copter.avoid_adsb_set_velocity(velocity_neu);
|
||||
return true;
|
||||
@ -227,13 +227,13 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs
|
||||
Vector3f velocity_neu;
|
||||
if (get_vector_perpendicular(obstacle, velocity_neu)) {
|
||||
// convert horizontal components to velocities
|
||||
velocity_neu.x *= copter.wp_nav.get_speed_xy();
|
||||
velocity_neu.y *= copter.wp_nav.get_speed_xy();
|
||||
velocity_neu.x *= copter.wp_nav->get_speed_xy();
|
||||
velocity_neu.y *= copter.wp_nav->get_speed_xy();
|
||||
// use up and down waypoint speeds
|
||||
if (velocity_neu.z > 0.0f) {
|
||||
velocity_neu.z *= copter.wp_nav.get_speed_up();
|
||||
velocity_neu.z *= copter.wp_nav->get_speed_up();
|
||||
} else {
|
||||
velocity_neu.z *= copter.wp_nav.get_speed_down();
|
||||
velocity_neu.z *= copter.wp_nav->get_speed_down();
|
||||
// do not descend if below RTL alt
|
||||
if (copter.current_loc.alt < copter.g.rtl_altitude) {
|
||||
velocity_neu.z = 0.0f;
|
||||
|
@ -2,7 +2,7 @@
|
||||
|
||||
void Copter::update_ground_effect_detector(void)
|
||||
{
|
||||
if(!g2.gndeffect_comp_enabled || !motors.armed()) {
|
||||
if(!g2.gndeffect_comp_enabled || !motors->armed()) {
|
||||
// disarmed - disable ground effect and return
|
||||
gndeffect_state.takeoff_expected = false;
|
||||
gndeffect_state.touchdown_expected = false;
|
||||
@ -15,10 +15,10 @@ void Copter::update_ground_effect_detector(void)
|
||||
uint32_t tnow_ms = millis();
|
||||
float xy_des_speed_cms = 0.0f;
|
||||
float xy_speed_cms = 0.0f;
|
||||
float des_climb_rate_cms = pos_control.get_desired_velocity().z;
|
||||
float des_climb_rate_cms = pos_control->get_desired_velocity().z;
|
||||
|
||||
if (pos_control.is_active_xy()) {
|
||||
Vector3f vel_target = pos_control.get_vel_target();
|
||||
if (pos_control->is_active_xy()) {
|
||||
Vector3f vel_target = pos_control->get_vel_target();
|
||||
vel_target.z = 0.0f;
|
||||
xy_des_speed_cms = vel_target.length();
|
||||
}
|
||||
@ -32,7 +32,7 @@ void Copter::update_ground_effect_detector(void)
|
||||
// takeoff logic
|
||||
|
||||
// if we are armed and haven't yet taken off
|
||||
if (motors.armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
|
||||
if (motors->armed() && ap.land_complete && !gndeffect_state.takeoff_expected) {
|
||||
gndeffect_state.takeoff_expected = true;
|
||||
}
|
||||
|
||||
@ -50,13 +50,13 @@ void Copter::update_ground_effect_detector(void)
|
||||
}
|
||||
|
||||
// landing logic
|
||||
Vector3f angle_target_rad = attitude_control.get_att_target_euler_cd() * radians(0.01f);
|
||||
Vector3f angle_target_rad = attitude_control->get_att_target_euler_cd() * radians(0.01f);
|
||||
bool small_angle_request = cosf(angle_target_rad.x)*cosf(angle_target_rad.y) > cosf(radians(7.5f));
|
||||
bool xy_speed_low = (position_ok() || optflow_position_ok()) && xy_speed_cms <= 125.0f;
|
||||
bool xy_speed_demand_low = pos_control.is_active_xy() && xy_des_speed_cms <= 125.0f;
|
||||
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control.is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
|
||||
bool xy_speed_demand_low = pos_control->is_active_xy() && xy_des_speed_cms <= 125.0f;
|
||||
bool slow_horizontal = xy_speed_demand_low || (xy_speed_low && !pos_control->is_active_xy()) || (control_mode == ALT_HOLD && small_angle_request);
|
||||
|
||||
bool descent_demanded = pos_control.is_active_z() && des_climb_rate_cms < 0.0f;
|
||||
bool descent_demanded = pos_control->is_active_z() && des_climb_rate_cms < 0.0f;
|
||||
bool slow_descent_demanded = descent_demanded && des_climb_rate_cms >= -100.0f;
|
||||
bool z_speed_low = abs(inertial_nav.get_velocity_z()) <= 60.0f;
|
||||
bool slow_descent = (slow_descent_demanded || (z_speed_low && descent_demanded));
|
||||
|
@ -16,7 +16,7 @@ void Copter::update_home_from_EKF()
|
||||
}
|
||||
|
||||
// special logic if home is set in-flight
|
||||
if (motors.armed()) {
|
||||
if (motors->armed()) {
|
||||
set_home_to_current_location_inflight();
|
||||
} else {
|
||||
// move home to current ekf location (this will set home_state to HOME_SET)
|
||||
|
@ -336,7 +336,7 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
|
||||
// if no delay set the waypoint as "fast"
|
||||
if (loiter_time_max == 0 ) {
|
||||
wp_nav.set_fast_waypoint(true);
|
||||
wp_nav->set_fast_waypoint(true);
|
||||
}
|
||||
}
|
||||
|
||||
@ -414,7 +414,7 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd)
|
||||
if (target_loc.lat == 0 && target_loc.lng == 0) {
|
||||
// To-Do: make this simpler
|
||||
Vector3f temp_pos;
|
||||
wp_nav.get_wp_stopping_point_xy(temp_pos);
|
||||
wp_nav->get_wp_stopping_point_xy(temp_pos);
|
||||
Location_Class temp_loc(temp_pos);
|
||||
target_loc.lat = temp_loc.lat;
|
||||
target_loc.lng = temp_loc.lng;
|
||||
@ -646,7 +646,7 @@ void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd)
|
||||
bool Copter::verify_takeoff()
|
||||
{
|
||||
// have we reached our target altitude?
|
||||
return wp_nav.reached_wp_destination();
|
||||
return wp_nav->reached_wp_destination();
|
||||
}
|
||||
|
||||
// verify_land - returns true if landing has been completed
|
||||
@ -657,9 +657,9 @@ bool Copter::verify_land()
|
||||
switch (land_state) {
|
||||
case LandStateType_FlyToLocation:
|
||||
// check if we've reached the location
|
||||
if (wp_nav.reached_wp_destination()) {
|
||||
if (wp_nav->reached_wp_destination()) {
|
||||
// get destination so we can use it for loiter target
|
||||
Vector3f dest = wp_nav.get_wp_destination();
|
||||
Vector3f dest = wp_nav->get_wp_destination();
|
||||
|
||||
// initialise landing controller
|
||||
auto_land_start(dest);
|
||||
@ -703,7 +703,7 @@ bool Copter::verify_payload_place()
|
||||
const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed
|
||||
const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed
|
||||
|
||||
const float current_throttle_level = motors.get_throttle();
|
||||
const float current_throttle_level = motors->get_throttle();
|
||||
const uint32_t now = AP_HAL::millis();
|
||||
|
||||
// if we discover we've landed then immediately release the load:
|
||||
@ -729,11 +729,11 @@ bool Copter::verify_payload_place()
|
||||
|
||||
switch (nav_payload_place.state) {
|
||||
case PayloadPlaceStateType_FlyToLocation:
|
||||
if (!wp_nav.reached_wp_destination()) {
|
||||
if (!wp_nav->reached_wp_destination()) {
|
||||
return false;
|
||||
}
|
||||
// we're there; set loiter target
|
||||
auto_payload_place_start(wp_nav.get_wp_destination());
|
||||
auto_payload_place_start(wp_nav->get_wp_destination());
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
// no break
|
||||
case PayloadPlaceStateType_Calibrating_Hover_Start:
|
||||
@ -751,7 +751,7 @@ bool Copter::verify_payload_place()
|
||||
}
|
||||
// we have a valid calibration. Hopefully.
|
||||
nav_payload_place.hover_throttle_level = current_throttle_level;
|
||||
const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors.get_throttle_hover());
|
||||
const float hover_throttle_delta = fabsf(nav_payload_place.hover_throttle_level - motors->get_throttle_hover());
|
||||
gcs_send_text_fmt(MAV_SEVERITY_INFO, "hover throttle delta: %f", static_cast<double>(hover_throttle_delta));
|
||||
nav_payload_place.state = PayloadPlaceStateType_Descending_Start;
|
||||
}
|
||||
@ -831,7 +831,7 @@ bool Copter::verify_payload_place()
|
||||
}
|
||||
// no break
|
||||
case PayloadPlaceStateType_Ascending:
|
||||
if (!wp_nav.reached_wp_destination()) {
|
||||
if (!wp_nav->reached_wp_destination()) {
|
||||
return false;
|
||||
}
|
||||
nav_payload_place.state = PayloadPlaceStateType_Done;
|
||||
@ -852,7 +852,7 @@ bool Copter::verify_payload_place()
|
||||
bool Copter::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we have reached the waypoint
|
||||
if( !wp_nav.reached_wp_destination() ) {
|
||||
if( !wp_nav->reached_wp_destination() ) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -882,7 +882,7 @@ bool Copter::verify_loiter_unlimited()
|
||||
bool Copter::verify_loiter_time()
|
||||
{
|
||||
// return immediately if we haven't reached our destination
|
||||
if (!wp_nav.reached_wp_destination()) {
|
||||
if (!wp_nav->reached_wp_destination()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -900,7 +900,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we've reached the edge
|
||||
if (auto_mode == Auto_CircleMoveToEdge) {
|
||||
if (wp_nav.reached_wp_destination()) {
|
||||
if (wp_nav->reached_wp_destination()) {
|
||||
Vector3f curr_pos = inertial_nav.get_position();
|
||||
Vector3f circle_center = pv_location_to_vector(cmd.content.location);
|
||||
|
||||
@ -922,7 +922,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd)
|
||||
}
|
||||
|
||||
// check if we have completed circling
|
||||
return fabsf(circle_nav.get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
|
||||
return fabsf(circle_nav->get_angle_total()/M_2PI) >= LOWBYTE(cmd.p1);
|
||||
}
|
||||
|
||||
// verify_RTL - handles any state changes required to implement RTL
|
||||
@ -937,7 +937,7 @@ bool Copter::verify_RTL()
|
||||
bool Copter::verify_spline_wp(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
// check if we have reached the waypoint
|
||||
if( !wp_nav.reached_wp_destination() ) {
|
||||
if( !wp_nav->reached_wp_destination() ) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -1082,7 +1082,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd)
|
||||
void Copter::do_change_speed(const AP_Mission::Mission_Command& cmd)
|
||||
{
|
||||
if (cmd.content.speed.target_ms > 0) {
|
||||
wp_nav.set_speed_xy(cmd.content.speed.target_ms * 100.0f);
|
||||
wp_nav->set_speed_xy(cmd.content.speed.target_ms * 100.0f);
|
||||
}
|
||||
}
|
||||
|
||||
|
@ -134,14 +134,14 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||
// enable motors and pass through throttle
|
||||
init_rc_out();
|
||||
enable_motor_output();
|
||||
motors.armed(true);
|
||||
motors->armed(true);
|
||||
|
||||
// initialise run time
|
||||
last_run_time = millis();
|
||||
last_send_time = millis();
|
||||
|
||||
// main run while there is no user input and the compass is healthy
|
||||
while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors.armed()) {
|
||||
while (command_ack_start == command_ack_counter && compass.healthy(compass.get_primary()) && motors->armed()) {
|
||||
// 50hz loop
|
||||
if (millis() - last_run_time < 20) {
|
||||
// grab some compass values
|
||||
@ -155,7 +155,7 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||
read_radio();
|
||||
|
||||
// pass through throttle to motors
|
||||
motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
|
||||
|
||||
// read some compass values
|
||||
compass.read();
|
||||
@ -237,8 +237,8 @@ MAV_RESULT Copter::mavlink_compassmot(mavlink_channel_t chan)
|
||||
}
|
||||
|
||||
// stop motors
|
||||
motors.output_min();
|
||||
motors.armed(false);
|
||||
motors->output_min();
|
||||
motors->armed(false);
|
||||
|
||||
// set and save motor compensation
|
||||
if (updated) {
|
||||
|
@ -9,12 +9,12 @@
|
||||
bool Copter::acro_init(bool ignore_checks)
|
||||
{
|
||||
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
||||
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
||||
if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
||||
(get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid) > get_non_takeoff_throttle())) {
|
||||
return false;
|
||||
}
|
||||
// set target altitude to zero for reporting
|
||||
pos_control.set_alt_target(0);
|
||||
pos_control->set_alt_target(0);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -27,16 +27,16 @@ void Copter::acro_run()
|
||||
float pilot_throttle_scaled;
|
||||
|
||||
// if not armed set throttle to zero and exit immediately
|
||||
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
return;
|
||||
}
|
||||
|
||||
// clear landing flag
|
||||
set_land_complete(false);
|
||||
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// convert the input to the desired body frame rate
|
||||
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
|
||||
@ -45,10 +45,10 @@ void Copter::acro_run()
|
||||
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in(), g2.acro_thr_mid);
|
||||
|
||||
// run attitude controller
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
||||
|
||||
// output pilot's throttle without angle boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
}
|
||||
|
||||
|
||||
@ -127,7 +127,7 @@ void Copter::get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, in
|
||||
}
|
||||
|
||||
// convert earth-frame level rates to body-frame level rates
|
||||
attitude_control.euler_rate_to_ang_vel(attitude_control.get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
|
||||
attitude_control->euler_rate_to_ang_vel(attitude_control->get_att_target_euler_cd()*radians(0.01f), rate_ef_level, rate_bf_level);
|
||||
|
||||
// combine earth frame rate corrections with rate requests
|
||||
if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
|
||||
|
@ -10,19 +10,19 @@ bool Copter::althold_init(bool ignore_checks)
|
||||
{
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// do not allow helis to enter Alt Hold if the Rotor Runup is not complete
|
||||
if (!ignore_checks && !motors.rotor_runup_complete()){
|
||||
if (!ignore_checks && !motors->rotor_runup_complete()){
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
|
||||
// initialize vertical speeds and leash lengths
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// stop takeoff if running
|
||||
@ -39,15 +39,15 @@ void Copter::althold_run()
|
||||
float takeoff_climb_rate = 0.0f;
|
||||
|
||||
// initialize vertical speeds and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
|
||||
// get pilot desired lean angles
|
||||
float target_roll, target_pitch;
|
||||
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control.get_althold_lean_angle_max());
|
||||
get_pilot_desired_lean_angles(channel_roll->get_control_in(), channel_pitch->get_control_in(), target_roll, target_pitch, attitude_control->get_althold_lean_angle_max());
|
||||
|
||||
// get pilot's desired yaw rate
|
||||
float target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
@ -58,13 +58,13 @@ void Copter::althold_run()
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// helicopters are held on the ground until rotor speed runup has finished
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
|
||||
#else
|
||||
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
|
||||
#endif
|
||||
|
||||
// Alt Hold State Machine Determination
|
||||
if (!motors.armed() || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !motors->get_interlock()) {
|
||||
althold_state = AltHold_MotorStopped;
|
||||
} else if (takeoff_state.running || takeoff_triggered) {
|
||||
althold_state = AltHold_Takeoff;
|
||||
@ -79,22 +79,22 @@ void Copter::althold_run()
|
||||
|
||||
case AltHold_MotorStopped:
|
||||
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// force descent rate and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
#else
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
#endif
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case AltHold_Takeoff:
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// initiate take-off
|
||||
if (!takeoff_state.running) {
|
||||
@ -109,31 +109,31 @@ void Copter::althold_run()
|
||||
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case AltHold_Landed:
|
||||
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
||||
if (target_climb_rate < 0.0f) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
}
|
||||
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control.update_z_controller();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case AltHold_Flying:
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
#if AC_AVOID_ENABLED == ENABLED
|
||||
// apply avoidance
|
||||
@ -141,17 +141,17 @@ void Copter::althold_run()
|
||||
#endif
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -24,7 +24,7 @@ bool Copter::auto_init(bool ignore_checks)
|
||||
auto_mode = Auto_Loiter;
|
||||
|
||||
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
|
||||
if (motors.armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
|
||||
if (motors->armed() && ap.land_complete && !mission.starts_with_takeoff_cmd()) {
|
||||
gcs_send_text(MAV_SEVERITY_CRITICAL, "Auto: Missing Takeoff Cmd");
|
||||
return false;
|
||||
}
|
||||
@ -36,7 +36,7 @@ bool Copter::auto_init(bool ignore_checks)
|
||||
}
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
wp_nav->wp_and_spline_init();
|
||||
|
||||
// clear guided limits
|
||||
guided_limit_clear();
|
||||
@ -129,7 +129,7 @@ void Copter::auto_takeoff_start(const Location& dest_loc)
|
||||
}
|
||||
|
||||
// set waypoint controller target
|
||||
if (!wp_nav.set_wp_destination(dest)) {
|
||||
if (!wp_nav->set_wp_destination(dest)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
return;
|
||||
@ -150,17 +150,17 @@ void Copter::auto_takeoff_start(const Location& dest_loc)
|
||||
void Copter::auto_takeoff_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
// initialise wpnav targets
|
||||
wp_nav.shift_wp_origin_to_current_pos();
|
||||
wp_nav->shift_wp_origin_to_current_pos();
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// reset attitude control targets
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// clear i term when we're taking off
|
||||
set_throttle_takeoff();
|
||||
@ -176,24 +176,24 @@ void Copter::auto_takeoff_run()
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// helicopters stay in landed state until rotor speed runup has finished
|
||||
if (motors.rotor_runup_complete()) {
|
||||
if (motors->rotor_runup_complete()) {
|
||||
set_land_complete(false);
|
||||
} else {
|
||||
// initialise wpnav targets
|
||||
wp_nav.shift_wp_origin_to_current_pos();
|
||||
wp_nav->shift_wp_origin_to_current_pos();
|
||||
}
|
||||
#else
|
||||
set_land_complete(false);
|
||||
#endif
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
auto_takeoff_attitude_run(target_yaw_rate);
|
||||
@ -205,7 +205,7 @@ void Copter::auto_wp_start(const Vector3f& destination)
|
||||
auto_mode = Auto_WP;
|
||||
|
||||
// initialise wpnav (no need to check return status because terrain data is not used)
|
||||
wp_nav.set_wp_destination(destination, false);
|
||||
wp_nav->set_wp_destination(destination, false);
|
||||
|
||||
// initialise yaw
|
||||
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
|
||||
@ -220,7 +220,7 @@ void Copter::auto_wp_start(const Location_Class& dest_loc)
|
||||
auto_mode = Auto_WP;
|
||||
|
||||
// send target to waypoint controller
|
||||
if (!wp_nav.set_wp_destination(dest_loc)) {
|
||||
if (!wp_nav->set_wp_destination(dest_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
return;
|
||||
@ -238,16 +238,16 @@ void Copter::auto_wp_start(const Location_Class& dest_loc)
|
||||
void Copter::auto_wp_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// clear i term when we're taking off
|
||||
set_throttle_takeoff();
|
||||
@ -265,21 +265,21 @@ void Copter::auto_wp_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
@ -292,7 +292,7 @@ void Copter::auto_spline_start(const Location_Class& destination, bool stopped_a
|
||||
auto_mode = Auto_Spline;
|
||||
|
||||
// initialise wpnav
|
||||
if (!wp_nav.set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
|
||||
if (!wp_nav->set_spline_destination(destination, stopped_at_start, seg_end_type, next_destination)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
return;
|
||||
@ -310,16 +310,16 @@ void Copter::auto_spline_start(const Location_Class& destination, bool stopped_a
|
||||
void Copter::auto_spline_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
// To-Do: reset waypoint origin to current location because copter is probably on the ground so we don't want it lurching left or right on take-off
|
||||
// (of course it would be better if people just used take-off)
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else // multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
#endif
|
||||
// clear i term when we're taking off
|
||||
set_throttle_takeoff();
|
||||
@ -337,21 +337,21 @@ void Copter::auto_spline_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
wp_nav.update_spline();
|
||||
wp_nav->update_spline();
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
@ -360,7 +360,7 @@ void Copter::auto_land_start()
|
||||
{
|
||||
// set target to stopping point
|
||||
Vector3f stopping_point;
|
||||
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
||||
wp_nav->get_loiter_stopping_point_xy(stopping_point);
|
||||
|
||||
// call location specific land start function
|
||||
auto_land_start(stopping_point);
|
||||
@ -372,12 +372,12 @@ void Copter::auto_land_start(const Vector3f& destination)
|
||||
auto_mode = Auto_Land;
|
||||
|
||||
// initialise loiter target destination
|
||||
wp_nav.init_loiter_target(destination);
|
||||
wp_nav->init_loiter_target(destination);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude());
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target(inertial_nav.get_altitude());
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
@ -389,23 +389,23 @@ void Copter::auto_land_start(const Vector3f& destination)
|
||||
void Copter::auto_land_run()
|
||||
{
|
||||
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
return;
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
land_run_horizontal_control();
|
||||
land_run_vertical_control();
|
||||
@ -439,16 +439,16 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f
|
||||
circle_center_neu = inertial_nav.get_position();
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_CIRCLE_INIT);
|
||||
}
|
||||
circle_nav.set_center(circle_center_neu);
|
||||
circle_nav->set_center(circle_center_neu);
|
||||
|
||||
// set circle radius
|
||||
if (!is_zero(radius_m)) {
|
||||
circle_nav.set_radius(radius_m * 100.0f);
|
||||
circle_nav->set_radius(radius_m * 100.0f);
|
||||
}
|
||||
|
||||
// check our distance from edge of circle
|
||||
Vector3f circle_edge_neu;
|
||||
circle_nav.get_closest_point_on_circle(circle_edge_neu);
|
||||
circle_nav->get_closest_point_on_circle(circle_edge_neu);
|
||||
float dist_to_edge = (inertial_nav.get_position() - circle_edge_neu).length();
|
||||
|
||||
// if more than 3m then fly to edge
|
||||
@ -463,7 +463,7 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f
|
||||
circle_edge.set_alt_cm(circle_center.alt, circle_center.get_alt_frame());
|
||||
|
||||
// initialise wpnav to move to edge of circle
|
||||
if (!wp_nav.set_wp_destination(circle_edge)) {
|
||||
if (!wp_nav->set_wp_destination(circle_edge)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
failsafe_terrain_on_event();
|
||||
}
|
||||
@ -471,7 +471,7 @@ void Copter::auto_circle_movetoedge_start(const Location_Class &circle_center, f
|
||||
// if we are outside the circle, point at the edge, otherwise hold yaw
|
||||
const Vector3f &curr_pos = inertial_nav.get_position();
|
||||
float dist_to_center = norm(circle_center_neu.x - curr_pos.x, circle_center_neu.y - curr_pos.y);
|
||||
if (dist_to_center > circle_nav.get_radius() && dist_to_center > 500) {
|
||||
if (dist_to_center > circle_nav->get_radius() && dist_to_center > 500) {
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
} else {
|
||||
// vehicle is within circle so hold yaw to avoid spinning as we move to edge of circle
|
||||
@ -489,7 +489,7 @@ void Copter::auto_circle_start()
|
||||
auto_mode = Auto_Circle;
|
||||
|
||||
// initialise circle controller
|
||||
circle_nav.init(circle_nav.get_center());
|
||||
circle_nav->init(circle_nav->get_center());
|
||||
}
|
||||
|
||||
// auto_circle_run - circle in AUTO flight mode
|
||||
@ -497,13 +497,13 @@ void Copter::auto_circle_start()
|
||||
void Copter::auto_circle_run()
|
||||
{
|
||||
// call circle controller
|
||||
circle_nav.update();
|
||||
circle_nav->update();
|
||||
|
||||
// call z-axis position controller
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
#if NAV_GUIDED == ENABLED
|
||||
@ -542,11 +542,11 @@ bool Copter::auto_loiter_start()
|
||||
|
||||
// calculate stopping point
|
||||
Vector3f stopping_point;
|
||||
pos_control.get_stopping_point_xy(stopping_point);
|
||||
pos_control.get_stopping_point_z(stopping_point);
|
||||
pos_control->get_stopping_point_xy(stopping_point);
|
||||
pos_control->get_stopping_point_z(stopping_point);
|
||||
|
||||
// initialise waypoint controller target to stopping point
|
||||
wp_nav.set_wp_origin_and_destination(origin, stopping_point);
|
||||
wp_nav->set_wp_origin_and_destination(origin, stopping_point);
|
||||
|
||||
// hold yaw at current heading
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
@ -559,15 +559,15 @@ bool Copter::auto_loiter_start()
|
||||
void Copter::auto_loiter_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
@ -579,13 +579,13 @@ void Copter::auto_loiter_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint and z-axis position controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
pos_control.update_z_controller();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
pos_control->update_z_controller();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// get_default_auto_yaw_mode - returns auto_yaw_mode based on WP_YAW_BEHAVIOR parameter
|
||||
@ -654,7 +654,7 @@ void Copter::set_auto_yaw_mode(uint8_t yaw_mode)
|
||||
void Copter::set_auto_yaw_look_at_heading(float angle_deg, float turn_rate_dps, int8_t direction, uint8_t relative_angle)
|
||||
{
|
||||
// get current yaw target
|
||||
int32_t curr_yaw_target = attitude_control.get_att_target_euler_cd().z;
|
||||
int32_t curr_yaw_target = attitude_control->get_att_target_euler_cd().z;
|
||||
|
||||
// get final angle, 1 = Relative, 0 = Absolute
|
||||
if (relative_angle == 0) {
|
||||
@ -746,7 +746,7 @@ float Copter::get_auto_heading(void)
|
||||
default:
|
||||
// point towards next waypoint.
|
||||
// we don't use wp_bearing because we don't want the copter to turn too much during flight
|
||||
return wp_nav.get_yaw();
|
||||
return wp_nav->get_yaw();
|
||||
}
|
||||
}
|
||||
|
||||
@ -755,7 +755,7 @@ void Copter::auto_payload_place_start()
|
||||
{
|
||||
// set target to stopping point
|
||||
Vector3f stopping_point;
|
||||
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
||||
wp_nav->get_loiter_stopping_point_xy(stopping_point);
|
||||
|
||||
// call location specific place start function
|
||||
auto_payload_place_start(stopping_point);
|
||||
@ -769,12 +769,12 @@ void Copter::auto_payload_place_start(const Vector3f& destination)
|
||||
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
|
||||
|
||||
// initialise loiter target destination
|
||||
wp_nav.init_loiter_target(destination);
|
||||
wp_nav->init_loiter_target(destination);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude());
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target(inertial_nav.get_altitude());
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
@ -784,7 +784,7 @@ void Copter::auto_payload_place_start(const Vector3f& destination)
|
||||
bool Copter::auto_payload_place_run_should_run()
|
||||
{
|
||||
// muts be armed
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
return false;
|
||||
}
|
||||
// muts be auto-armed
|
||||
@ -796,7 +796,7 @@ bool Copter::auto_payload_place_run_should_run()
|
||||
return false;
|
||||
}
|
||||
// interlock must be enabled (i.e. unsafe)
|
||||
if (!motors.get_interlock()) {
|
||||
if (!motors->get_interlock()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -810,20 +810,20 @@ void Copter::auto_payload_place_run()
|
||||
if (!auto_payload_place_run_should_run()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
return;
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
switch (nav_payload_place.state) {
|
||||
case PayloadPlaceStateType_FlyToLocation:
|
||||
@ -849,16 +849,16 @@ void Copter::auto_payload_place_run_loiter()
|
||||
land_run_horizontal_control();
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
const float target_yaw_rate = 0;
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// update altitude target and call position controller
|
||||
// const float target_climb_rate = 0;
|
||||
// pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
// pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
void Copter::auto_payload_place_run_descend()
|
||||
|
@ -211,7 +211,7 @@ void Copter::autotune_stop()
|
||||
autotune_load_orig_gains();
|
||||
|
||||
// re-enable angle-to-rate request limits
|
||||
attitude_control.use_ff_and_input_shaping(true);
|
||||
attitude_control->use_ff_and_input_shaping(true);
|
||||
|
||||
// log off event and send message to ground station
|
||||
autotune_update_gcs(AUTOTUNE_MESSAGE_STOPPED);
|
||||
@ -235,18 +235,18 @@ bool Copter::autotune_start(bool ignore_checks)
|
||||
}
|
||||
|
||||
// ensure we are flying
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// initialize vertical speeds and leash lengths
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -261,15 +261,15 @@ void Copter::autotune_run()
|
||||
int16_t target_climb_rate;
|
||||
|
||||
// initialize vertical speeds and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
// this should not actually be possible because of the autotune_init() checks
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
pos_control->relax_alt_hold_controllers(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -297,15 +297,15 @@ void Copter::autotune_run()
|
||||
if (ap.land_complete) {
|
||||
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
||||
if (target_climb_rate < 0.0f) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
}
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
pos_control.update_z_controller();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
pos_control->relax_alt_hold_controllers(0.0f);
|
||||
pos_control->update_z_controller();
|
||||
}else{
|
||||
// check if pilot is overriding the controls
|
||||
if (!is_zero(target_roll) || !is_zero(target_pitch) || !is_zero(target_yaw_rate) || target_climb_rate != 0) {
|
||||
@ -313,7 +313,7 @@ void Copter::autotune_run()
|
||||
autotune_state.pilot_override = true;
|
||||
// set gains to their original values
|
||||
autotune_load_orig_gains();
|
||||
attitude_control.use_ff_and_input_shaping(true);
|
||||
attitude_control->use_ff_and_input_shaping(true);
|
||||
}
|
||||
// reset pilot override time
|
||||
autotune_override_time = millis();
|
||||
@ -329,19 +329,19 @@ void Copter::autotune_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// if pilot override call attitude controller
|
||||
if (autotune_state.pilot_override || autotune_state.mode != AUTOTUNE_MODE_TUNING) {
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// somehow get attitude requests from autotuning
|
||||
autotune_attitude_control();
|
||||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
}
|
||||
|
||||
@ -358,10 +358,10 @@ void Copter::autotune_attitude_control()
|
||||
case AUTOTUNE_STEP_WAITING_FOR_LEVEL:
|
||||
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
|
||||
// re-enable rate limits
|
||||
attitude_control.use_ff_and_input_shaping(true);
|
||||
attitude_control->use_ff_and_input_shaping(true);
|
||||
|
||||
// hold level attitude
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, autotune_desired_yaw, true, get_smoothing_gain());
|
||||
|
||||
// hold the copter level for 0.5 seconds before we begin a twitch
|
||||
// reset counter if we are no longer level
|
||||
@ -390,11 +390,11 @@ void Copter::autotune_attitude_control()
|
||||
|
||||
switch (autotune_state.axis) {
|
||||
case AUTOTUNE_AXIS_ROLL:
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control.max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control.max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_roll())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||
autotune_start_rate = ToDeg(ahrs.get_gyro().x) * 100.0f;
|
||||
autotune_start_angle = ahrs.roll_sensor;
|
||||
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_roll_pid().filt_hz()*2.0f);
|
||||
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_roll_pid().filt_hz()*2.0f);
|
||||
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
|
||||
rotation_rate_filt.reset(autotune_start_rate);
|
||||
} else {
|
||||
@ -402,11 +402,11 @@ void Copter::autotune_attitude_control()
|
||||
}
|
||||
break;
|
||||
case AUTOTUNE_AXIS_PITCH:
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control.max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control.max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_RATE_RLLPIT_CDS, AUTOTUNE_TARGET_RATE_RLLPIT_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_pitch())*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_RLLPIT_CD, AUTOTUNE_TARGET_ANGLE_RLLPIT_CD);
|
||||
autotune_start_rate = ToDeg(ahrs.get_gyro().y) * 100.0f;
|
||||
autotune_start_angle = ahrs.pitch_sensor;
|
||||
rotation_rate_filt.set_cutoff_frequency(attitude_control.get_rate_pitch_pid().filt_hz()*2.0f);
|
||||
rotation_rate_filt.set_cutoff_frequency(attitude_control->get_rate_pitch_pid().filt_hz()*2.0f);
|
||||
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
|
||||
rotation_rate_filt.reset(autotune_start_rate);
|
||||
} else {
|
||||
@ -414,8 +414,8 @@ void Copter::autotune_attitude_control()
|
||||
}
|
||||
break;
|
||||
case AUTOTUNE_AXIS_YAW:
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control.max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control.max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
|
||||
autotune_target_rate = constrain_float(ToDeg(attitude_control->max_rate_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_RATE_YAW_CDS, AUTOTUNE_TARGET_RATE_YAW_CDS);
|
||||
autotune_target_angle = constrain_float(ToDeg(attitude_control->max_angle_step_bf_yaw()*0.75f)*100.0f, AUTOTUNE_TARGET_MIN_ANGLE_YAW_CD, AUTOTUNE_TARGET_ANGLE_YAW_CD);
|
||||
autotune_start_rate = ToDeg(ahrs.get_gyro().z) * 100.0f;
|
||||
autotune_start_angle = ahrs.yaw_sensor;
|
||||
rotation_rate_filt.set_cutoff_frequency(AUTOTUNE_Y_FILT_FREQ);
|
||||
@ -433,41 +433,41 @@ void Copter::autotune_attitude_control()
|
||||
// Note: we should be using intra-test gains (which are very close to the original gains but have lower I)
|
||||
|
||||
// disable rate limits
|
||||
attitude_control.use_ff_and_input_shaping(false);
|
||||
attitude_control->use_ff_and_input_shaping(false);
|
||||
|
||||
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
|
||||
// Testing increasing stabilize P gain so will set lean angle target
|
||||
switch (autotune_state.axis) {
|
||||
case AUTOTUNE_AXIS_ROLL:
|
||||
// request roll to 20deg
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, 0.0f, get_smoothing_gain());
|
||||
break;
|
||||
case AUTOTUNE_AXIS_PITCH:
|
||||
// request pitch to 20deg
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, direction_sign * autotune_target_angle + autotune_start_angle, 0.0f, get_smoothing_gain());
|
||||
break;
|
||||
case AUTOTUNE_AXIS_YAW:
|
||||
// request pitch to 20deg
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, wrap_180_cd(direction_sign * autotune_target_angle + autotune_start_angle), false, get_smoothing_gain());
|
||||
break;
|
||||
}
|
||||
} else {
|
||||
// Testing rate P and D gains so will set body-frame rate targets.
|
||||
// Rate controller will use existing body-frame rates and convert to motor outputs
|
||||
// for all axes except the one we override here.
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw( 0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
switch (autotune_state.axis) {
|
||||
case AUTOTUNE_AXIS_ROLL:
|
||||
// override body-frame roll rate
|
||||
attitude_control.rate_bf_roll_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
attitude_control->rate_bf_roll_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
break;
|
||||
case AUTOTUNE_AXIS_PITCH:
|
||||
// override body-frame pitch rate
|
||||
attitude_control.rate_bf_pitch_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
attitude_control->rate_bf_pitch_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
break;
|
||||
case AUTOTUNE_AXIS_YAW:
|
||||
// override body-frame yaw rate
|
||||
attitude_control.rate_bf_yaw_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
attitude_control->rate_bf_yaw_target(direction_sign * autotune_target_rate + autotune_start_rate);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -526,13 +526,13 @@ void Copter::autotune_attitude_control()
|
||||
|
||||
// log this iterations lean angle and rotation rate
|
||||
Log_Write_AutoTuneDetails(lean_angle, rotation_rate);
|
||||
DataFlash.Log_Write_Rate(ahrs, motors, attitude_control, pos_control);
|
||||
DataFlash.Log_Write_Rate(ahrs, *motors, *attitude_control, *pos_control);
|
||||
break;
|
||||
|
||||
case AUTOTUNE_STEP_UPDATE_GAINS:
|
||||
|
||||
// re-enable rate limits
|
||||
attitude_control.use_ff_and_input_shaping(true);
|
||||
attitude_control->use_ff_and_input_shaping(true);
|
||||
|
||||
// log the latest gains
|
||||
if ((autotune_state.tune_type == AUTOTUNE_TYPE_SP_DOWN) || (autotune_state.tune_type == AUTOTUNE_TYPE_SP_UP)) {
|
||||
@ -731,7 +731,7 @@ void Copter::autotune_attitude_control()
|
||||
autotune_state.positive_direction = !autotune_state.positive_direction;
|
||||
|
||||
if (autotune_state.axis == AUTOTUNE_AXIS_YAW) {
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw( 0.0f, 0.0f, ahrs.yaw_sensor, false, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// set gains to their intra-test values (which are very close to the original gains)
|
||||
@ -765,39 +765,39 @@ void Copter::autotune_backup_gains_and_initialise()
|
||||
|
||||
g.autotune_aggressiveness = constrain_float(g.autotune_aggressiveness, 0.05f, 0.2f);
|
||||
|
||||
orig_bf_feedforward = attitude_control.get_bf_feedforward();
|
||||
orig_bf_feedforward = attitude_control->get_bf_feedforward();
|
||||
|
||||
// backup original pids and initialise tuned pid values
|
||||
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
|
||||
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
|
||||
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
|
||||
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
|
||||
orig_roll_accel = attitude_control.get_accel_roll_max();
|
||||
tune_roll_rp = attitude_control.get_rate_roll_pid().kP();
|
||||
tune_roll_rd = attitude_control.get_rate_roll_pid().kD();
|
||||
tune_roll_sp = attitude_control.get_angle_roll_p().kP();
|
||||
tune_roll_accel = attitude_control.get_accel_roll_max();
|
||||
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
||||
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
|
||||
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
||||
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
|
||||
orig_roll_accel = attitude_control->get_accel_roll_max();
|
||||
tune_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
||||
tune_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
||||
tune_roll_sp = attitude_control->get_angle_roll_p().kP();
|
||||
tune_roll_accel = attitude_control->get_accel_roll_max();
|
||||
|
||||
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
|
||||
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
|
||||
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
|
||||
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
|
||||
orig_pitch_accel = attitude_control.get_accel_pitch_max();
|
||||
tune_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
|
||||
tune_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
|
||||
tune_pitch_sp = attitude_control.get_angle_pitch_p().kP();
|
||||
tune_pitch_accel = attitude_control.get_accel_pitch_max();
|
||||
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
||||
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
|
||||
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
||||
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
||||
orig_pitch_accel = attitude_control->get_accel_pitch_max();
|
||||
tune_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
||||
tune_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
||||
tune_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
||||
tune_pitch_accel = attitude_control->get_accel_pitch_max();
|
||||
|
||||
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
|
||||
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
|
||||
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
|
||||
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
|
||||
orig_yaw_accel = attitude_control.get_accel_yaw_max();
|
||||
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
|
||||
tune_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
|
||||
tune_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
|
||||
tune_yaw_sp = attitude_control.get_angle_yaw_p().kP();
|
||||
tune_yaw_accel = attitude_control.get_accel_yaw_max();
|
||||
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
||||
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
|
||||
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
|
||||
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
||||
orig_yaw_accel = attitude_control->get_accel_yaw_max();
|
||||
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
||||
tune_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
||||
tune_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
||||
tune_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
||||
tune_yaw_accel = attitude_control->get_accel_yaw_max();
|
||||
|
||||
Log_Write_Event(DATA_AUTOTUNE_INITIALISED);
|
||||
}
|
||||
@ -806,33 +806,33 @@ void Copter::autotune_backup_gains_and_initialise()
|
||||
// called by autotune_stop and autotune_failed functions
|
||||
void Copter::autotune_load_orig_gains()
|
||||
{
|
||||
attitude_control.bf_feedforward(orig_bf_feedforward);
|
||||
attitude_control->bf_feedforward(orig_bf_feedforward);
|
||||
if (autotune_roll_enabled()) {
|
||||
if (!is_zero(orig_roll_rp)) {
|
||||
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
|
||||
attitude_control.get_rate_roll_pid().kI(orig_roll_ri);
|
||||
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
|
||||
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
|
||||
attitude_control.set_accel_roll_max(orig_roll_accel);
|
||||
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(orig_roll_ri);
|
||||
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
|
||||
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
|
||||
attitude_control->set_accel_roll_max(orig_roll_accel);
|
||||
}
|
||||
}
|
||||
if (autotune_pitch_enabled()) {
|
||||
if (!is_zero(orig_pitch_rp)) {
|
||||
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
|
||||
attitude_control.get_rate_pitch_pid().kI(orig_pitch_ri);
|
||||
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
|
||||
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
|
||||
attitude_control.set_accel_pitch_max(orig_pitch_accel);
|
||||
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(orig_pitch_ri);
|
||||
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
|
||||
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
|
||||
attitude_control->set_accel_pitch_max(orig_pitch_accel);
|
||||
}
|
||||
}
|
||||
if (autotune_yaw_enabled()) {
|
||||
if (!is_zero(orig_yaw_rp)) {
|
||||
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
|
||||
attitude_control.get_rate_yaw_pid().kI(orig_yaw_ri);
|
||||
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
||||
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
|
||||
attitude_control.set_accel_yaw_max(orig_yaw_accel);
|
||||
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(orig_yaw_ri);
|
||||
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
||||
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
|
||||
attitude_control->set_accel_yaw_max(orig_yaw_accel);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -840,37 +840,37 @@ void Copter::autotune_load_orig_gains()
|
||||
// autotune_load_tuned_gains - load tuned gains
|
||||
void Copter::autotune_load_tuned_gains()
|
||||
{
|
||||
if (!attitude_control.get_bf_feedforward()) {
|
||||
attitude_control.bf_feedforward(true);
|
||||
attitude_control.set_accel_roll_max(0.0f);
|
||||
attitude_control.set_accel_pitch_max(0.0f);
|
||||
if (!attitude_control->get_bf_feedforward()) {
|
||||
attitude_control->bf_feedforward(true);
|
||||
attitude_control->set_accel_roll_max(0.0f);
|
||||
attitude_control->set_accel_pitch_max(0.0f);
|
||||
}
|
||||
if (autotune_roll_enabled()) {
|
||||
if (!is_zero(tune_roll_rp)) {
|
||||
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
|
||||
attitude_control.set_accel_roll_max(tune_roll_accel);
|
||||
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
||||
attitude_control->set_accel_roll_max(tune_roll_accel);
|
||||
}
|
||||
}
|
||||
if (autotune_pitch_enabled()) {
|
||||
if (!is_zero(tune_pitch_rp)) {
|
||||
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
attitude_control.set_accel_pitch_max(tune_pitch_accel);
|
||||
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
attitude_control->set_accel_pitch_max(tune_pitch_accel);
|
||||
}
|
||||
}
|
||||
if (autotune_yaw_enabled()) {
|
||||
if (!is_zero(tune_yaw_rp)) {
|
||||
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
attitude_control.set_accel_yaw_max(tune_yaw_accel);
|
||||
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
attitude_control->set_accel_yaw_max(tune_yaw_accel);
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -881,25 +881,25 @@ void Copter::autotune_load_intra_test_gains()
|
||||
{
|
||||
// we are restarting tuning so reset gains to tuning-start gains (i.e. low I term)
|
||||
// sanity check the gains
|
||||
attitude_control.bf_feedforward(true);
|
||||
attitude_control->bf_feedforward(true);
|
||||
if (autotune_roll_enabled()) {
|
||||
attitude_control.get_rate_roll_pid().kP(orig_roll_rp);
|
||||
attitude_control.get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control.get_rate_roll_pid().kD(orig_roll_rd);
|
||||
attitude_control.get_angle_roll_p().kP(orig_roll_sp);
|
||||
attitude_control->get_rate_roll_pid().kP(orig_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(orig_roll_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control->get_rate_roll_pid().kD(orig_roll_rd);
|
||||
attitude_control->get_angle_roll_p().kP(orig_roll_sp);
|
||||
}
|
||||
if (autotune_pitch_enabled()) {
|
||||
attitude_control.get_rate_pitch_pid().kP(orig_pitch_rp);
|
||||
attitude_control.get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control.get_rate_pitch_pid().kD(orig_pitch_rd);
|
||||
attitude_control.get_angle_pitch_p().kP(orig_pitch_sp);
|
||||
attitude_control->get_rate_pitch_pid().kP(orig_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(orig_pitch_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control->get_rate_pitch_pid().kD(orig_pitch_rd);
|
||||
attitude_control->get_angle_pitch_p().kP(orig_pitch_sp);
|
||||
}
|
||||
if (autotune_yaw_enabled()) {
|
||||
attitude_control.get_rate_yaw_pid().kP(orig_yaw_rp);
|
||||
attitude_control.get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control.get_rate_yaw_pid().kD(orig_yaw_rd);
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
||||
attitude_control.get_angle_yaw_p().kP(orig_yaw_sp);
|
||||
attitude_control->get_rate_yaw_pid().kP(orig_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(orig_yaw_rp*AUTOTUNE_PI_RATIO_FOR_TESTING);
|
||||
attitude_control->get_rate_yaw_pid().kD(orig_yaw_rd);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(orig_yaw_rLPF);
|
||||
attitude_control->get_angle_yaw_p().kP(orig_yaw_sp);
|
||||
}
|
||||
}
|
||||
|
||||
@ -909,23 +909,23 @@ void Copter::autotune_load_twitch_gains()
|
||||
{
|
||||
switch (autotune_state.axis) {
|
||||
case AUTOTUNE_AXIS_ROLL:
|
||||
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*0.01f);
|
||||
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
|
||||
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*0.01f);
|
||||
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
||||
break;
|
||||
case AUTOTUNE_AXIS_PITCH:
|
||||
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
|
||||
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*0.01f);
|
||||
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
break;
|
||||
case AUTOTUNE_AXIS_YAW:
|
||||
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
|
||||
attitude_control.get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*0.01f);
|
||||
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -937,76 +937,76 @@ void Copter::autotune_save_tuning_gains()
|
||||
// if we successfully completed tuning
|
||||
if (autotune_state.mode == AUTOTUNE_MODE_SUCCESS) {
|
||||
|
||||
if (!attitude_control.get_bf_feedforward()) {
|
||||
attitude_control.bf_feedforward_save(true);
|
||||
attitude_control.save_accel_roll_max(0.0f);
|
||||
attitude_control.save_accel_pitch_max(0.0f);
|
||||
if (!attitude_control->get_bf_feedforward()) {
|
||||
attitude_control->bf_feedforward_save(true);
|
||||
attitude_control->save_accel_roll_max(0.0f);
|
||||
attitude_control->save_accel_pitch_max(0.0f);
|
||||
}
|
||||
|
||||
// sanity check the rate P values
|
||||
if (autotune_roll_enabled() && !is_zero(tune_roll_rp)) {
|
||||
// rate roll gains
|
||||
attitude_control.get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control.get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control.get_rate_roll_pid().save_gains();
|
||||
attitude_control->get_rate_roll_pid().kP(tune_roll_rp);
|
||||
attitude_control->get_rate_roll_pid().kI(tune_roll_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_roll_pid().kD(tune_roll_rd);
|
||||
attitude_control->get_rate_roll_pid().save_gains();
|
||||
|
||||
// stabilize roll
|
||||
attitude_control.get_angle_roll_p().kP(tune_roll_sp);
|
||||
attitude_control.get_angle_roll_p().save_gains();
|
||||
attitude_control->get_angle_roll_p().kP(tune_roll_sp);
|
||||
attitude_control->get_angle_roll_p().save_gains();
|
||||
|
||||
// acceleration roll
|
||||
attitude_control.save_accel_roll_max(tune_roll_accel);
|
||||
attitude_control->save_accel_roll_max(tune_roll_accel);
|
||||
|
||||
// resave pids to originals in case the autotune is run again
|
||||
orig_roll_rp = attitude_control.get_rate_roll_pid().kP();
|
||||
orig_roll_ri = attitude_control.get_rate_roll_pid().kI();
|
||||
orig_roll_rd = attitude_control.get_rate_roll_pid().kD();
|
||||
orig_roll_sp = attitude_control.get_angle_roll_p().kP();
|
||||
orig_roll_rp = attitude_control->get_rate_roll_pid().kP();
|
||||
orig_roll_ri = attitude_control->get_rate_roll_pid().kI();
|
||||
orig_roll_rd = attitude_control->get_rate_roll_pid().kD();
|
||||
orig_roll_sp = attitude_control->get_angle_roll_p().kP();
|
||||
}
|
||||
|
||||
if (autotune_pitch_enabled() && !is_zero(tune_pitch_rp)) {
|
||||
// rate pitch gains
|
||||
attitude_control.get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control.get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control.get_rate_pitch_pid().save_gains();
|
||||
attitude_control->get_rate_pitch_pid().kP(tune_pitch_rp);
|
||||
attitude_control->get_rate_pitch_pid().kI(tune_pitch_rp*AUTOTUNE_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_pitch_pid().kD(tune_pitch_rd);
|
||||
attitude_control->get_rate_pitch_pid().save_gains();
|
||||
|
||||
// stabilize pitch
|
||||
attitude_control.get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
attitude_control.get_angle_pitch_p().save_gains();
|
||||
attitude_control->get_angle_pitch_p().kP(tune_pitch_sp);
|
||||
attitude_control->get_angle_pitch_p().save_gains();
|
||||
|
||||
// acceleration pitch
|
||||
attitude_control.save_accel_pitch_max(tune_pitch_accel);
|
||||
attitude_control->save_accel_pitch_max(tune_pitch_accel);
|
||||
|
||||
// resave pids to originals in case the autotune is run again
|
||||
orig_pitch_rp = attitude_control.get_rate_pitch_pid().kP();
|
||||
orig_pitch_ri = attitude_control.get_rate_pitch_pid().kI();
|
||||
orig_pitch_rd = attitude_control.get_rate_pitch_pid().kD();
|
||||
orig_pitch_sp = attitude_control.get_angle_pitch_p().kP();
|
||||
orig_pitch_rp = attitude_control->get_rate_pitch_pid().kP();
|
||||
orig_pitch_ri = attitude_control->get_rate_pitch_pid().kI();
|
||||
orig_pitch_rd = attitude_control->get_rate_pitch_pid().kD();
|
||||
orig_pitch_sp = attitude_control->get_angle_pitch_p().kP();
|
||||
}
|
||||
|
||||
if (autotune_yaw_enabled() && !is_zero(tune_yaw_rp)) {
|
||||
// rate yaw gains
|
||||
attitude_control.get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control.get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
||||
attitude_control.get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control.get_rate_yaw_pid().save_gains();
|
||||
attitude_control->get_rate_yaw_pid().kP(tune_yaw_rp);
|
||||
attitude_control->get_rate_yaw_pid().kI(tune_yaw_rp*AUTOTUNE_YAW_PI_RATIO_FINAL);
|
||||
attitude_control->get_rate_yaw_pid().kD(0.0f);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(tune_yaw_rLPF);
|
||||
attitude_control->get_rate_yaw_pid().save_gains();
|
||||
|
||||
// stabilize yaw
|
||||
attitude_control.get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
attitude_control.get_angle_yaw_p().save_gains();
|
||||
attitude_control->get_angle_yaw_p().kP(tune_yaw_sp);
|
||||
attitude_control->get_angle_yaw_p().save_gains();
|
||||
|
||||
// acceleration yaw
|
||||
attitude_control.save_accel_yaw_max(tune_yaw_accel);
|
||||
attitude_control->save_accel_yaw_max(tune_yaw_accel);
|
||||
|
||||
// resave pids to originals in case the autotune is run again
|
||||
orig_yaw_rp = attitude_control.get_rate_yaw_pid().kP();
|
||||
orig_yaw_ri = attitude_control.get_rate_yaw_pid().kI();
|
||||
orig_yaw_rd = attitude_control.get_rate_yaw_pid().kD();
|
||||
orig_yaw_rLPF = attitude_control.get_rate_yaw_pid().filt_hz();
|
||||
orig_yaw_sp = attitude_control.get_angle_yaw_p().kP();
|
||||
orig_yaw_rp = attitude_control->get_rate_yaw_pid().kP();
|
||||
orig_yaw_ri = attitude_control->get_rate_yaw_pid().kI();
|
||||
orig_yaw_rd = attitude_control->get_rate_yaw_pid().kD();
|
||||
orig_yaw_rLPF = attitude_control->get_rate_yaw_pid().filt_hz();
|
||||
orig_yaw_sp = attitude_control->get_angle_yaw_p().kP();
|
||||
}
|
||||
// update GCS and log save gains event
|
||||
autotune_update_gcs(AUTOTUNE_MESSAGE_SAVED_GAINS);
|
||||
|
@ -10,19 +10,19 @@ bool Copter::brake_init(bool ignore_checks)
|
||||
if (position_ok() || ignore_checks) {
|
||||
|
||||
// set desired acceleration to zero
|
||||
wp_nav.clear_pilot_desired_acceleration();
|
||||
wp_nav->clear_pilot_desired_acceleration();
|
||||
|
||||
// set target to current position
|
||||
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
|
||||
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
|
||||
|
||||
// initialize vertical speed and acceleration
|
||||
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
|
||||
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
|
||||
pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
|
||||
pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
brake_timeout_ms = 0;
|
||||
@ -38,24 +38,24 @@ bool Copter::brake_init(bool ignore_checks)
|
||||
void Copter::brake_run()
|
||||
{
|
||||
// if not auto armed set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
wp_nav.init_brake_target(BRAKE_MODE_DECEL_RATE);
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
wp_nav->init_brake_target(BRAKE_MODE_DECEL_RATE);
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
pos_control->relax_alt_hold_controllers(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
// relax stop target if we might be landed
|
||||
if (ap.land_complete_maybe) {
|
||||
wp_nav.loiter_soften_for_landing();
|
||||
wp_nav->loiter_soften_for_landing();
|
||||
}
|
||||
|
||||
// if landed immediately disarm
|
||||
@ -64,19 +64,19 @@ void Copter::brake_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run brake controller
|
||||
wp_nav.update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_brake(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
|
||||
if (brake_timeout_ms != 0 && millis()-brake_timeout_start >= brake_timeout_ms) {
|
||||
if (!set_mode(LOITER, MODE_REASON_BRAKE_TIMEOUT)) {
|
||||
|
@ -11,14 +11,14 @@ bool Copter::circle_init(bool ignore_checks)
|
||||
circle_pilot_yaw_override = false;
|
||||
|
||||
// initialize speeds and accelerations
|
||||
pos_control.set_speed_xy(wp_nav.get_speed_xy());
|
||||
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
|
||||
pos_control.set_jerk_xy_to_default();
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_xy(wp_nav->get_speed_xy());
|
||||
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
|
||||
pos_control->set_jerk_xy_to_default();
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise circle controller including setting the circle center based on vehicle speed
|
||||
circle_nav.init();
|
||||
circle_nav->init();
|
||||
|
||||
return true;
|
||||
}else{
|
||||
@ -34,24 +34,24 @@ void Copter::circle_run()
|
||||
float target_climb_rate = 0;
|
||||
|
||||
// initialize speeds and accelerations
|
||||
pos_control.set_speed_xy(wp_nav.get_speed_xy());
|
||||
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_xy(wp_nav->get_speed_xy());
|
||||
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
// To-Do: add some initialisation of position controllers
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -76,24 +76,24 @@ void Copter::circle_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run circle controller
|
||||
circle_nav.update();
|
||||
circle_nav->update();
|
||||
|
||||
// call attitude controller
|
||||
if (circle_pilot_yaw_override) {
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(circle_nav.get_roll(), circle_nav.get_pitch(), circle_nav.get_yaw(),true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(circle_nav->get_roll(), circle_nav->get_pitch(), circle_nav->get_yaw(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
@ -47,9 +47,9 @@ void Copter::drift_run()
|
||||
float pilot_throttle_scaled;
|
||||
|
||||
// if landed and throttle at zero, set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !motors.get_interlock() || (ap.land_complete && ap.throttle_zero)) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
if (!motors->armed() || !motors->get_interlock() || (ap.land_complete && ap.throttle_zero)) {
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -98,13 +98,13 @@ void Copter::drift_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// output pilot's throttle with angle boost
|
||||
attitude_control.set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(get_throttle_assist(vel.z, pilot_throttle_scaled), true, g.throttle_filt);
|
||||
}
|
||||
|
||||
// get_throttle_assist - return throttle output (range 0 ~ 1) based on pilot input and z-axis velocity
|
||||
|
@ -55,7 +55,7 @@ bool Copter::flip_init(bool ignore_checks)
|
||||
}
|
||||
|
||||
// only allow flip when flying
|
||||
if (!motors.armed() || ap.land_complete) {
|
||||
if (!motors->armed() || ap.land_complete) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -98,7 +98,7 @@ void Copter::flip_run()
|
||||
float recovery_angle;
|
||||
|
||||
// if pilot inputs roll > 40deg or timeout occurs abandon flip
|
||||
if (!motors.armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
|
||||
if (!motors->armed() || (abs(channel_roll->get_control_in()) >= 4000) || (abs(channel_pitch->get_control_in()) >= 4000) || ((millis() - flip_start_time) > FLIP_TIMEOUT_MS)) {
|
||||
flip_state = Flip_Abandon;
|
||||
}
|
||||
|
||||
@ -120,7 +120,7 @@ void Copter::flip_run()
|
||||
|
||||
case Flip_Start:
|
||||
// under 45 degrees request 400deg/sec roll or pitch
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
|
||||
// increase throttle
|
||||
throttle_out += FLIP_THR_INC;
|
||||
@ -139,7 +139,7 @@ void Copter::flip_run()
|
||||
|
||||
case Flip_Roll:
|
||||
// between 45deg ~ -90deg request 400deg/sec roll
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(FLIP_ROTATION_RATE * flip_roll_dir, 0.0, 0.0);
|
||||
// decrease throttle
|
||||
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
|
||||
|
||||
@ -151,7 +151,7 @@ void Copter::flip_run()
|
||||
|
||||
case Flip_Pitch_A:
|
||||
// between 45deg ~ -90deg request 400deg/sec pitch
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(0.0f, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
// decrease throttle
|
||||
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
|
||||
|
||||
@ -163,7 +163,7 @@ void Copter::flip_run()
|
||||
|
||||
case Flip_Pitch_B:
|
||||
// between 45deg ~ -90deg request 400deg/sec pitch
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(0.0, FLIP_ROTATION_RATE * flip_pitch_dir, 0.0);
|
||||
// decrease throttle
|
||||
throttle_out = MAX(throttle_out - FLIP_THR_DEC, 0.0f);
|
||||
|
||||
@ -175,7 +175,7 @@ void Copter::flip_run()
|
||||
|
||||
case Flip_Recover:
|
||||
// use originally captured earth-frame angle targets to recover
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(flip_orig_attitude.x, flip_orig_attitude.y, flip_orig_attitude.z, false, get_smoothing_gain());
|
||||
|
||||
// increase throttle to gain any lost altitude
|
||||
throttle_out += FLIP_THR_INC;
|
||||
@ -212,8 +212,8 @@ void Copter::flip_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// output pilot's throttle without angle boost
|
||||
attitude_control.set_throttle_out(throttle_out, false, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(throttle_out, false, g.throttle_filt);
|
||||
}
|
||||
|
@ -59,7 +59,7 @@ bool Copter::guided_takeoff_start(float final_alt_above_home)
|
||||
Location_Class target_loc = current_loc;
|
||||
target_loc.set_alt_cm(final_alt_above_home, Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
|
||||
if (!wp_nav.set_wp_destination(target_loc)) {
|
||||
if (!wp_nav->set_wp_destination(target_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
// failure is propagated to GCS with NAK
|
||||
@ -85,17 +85,17 @@ void Copter::guided_pos_control_start()
|
||||
guided_mode = Guided_WP;
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
wp_nav->wp_and_spline_init();
|
||||
|
||||
// initialise wpnav to stopping point at current altitude
|
||||
// To-Do: set to current location if disarmed?
|
||||
// To-Do: set to stopping point altitude?
|
||||
Vector3f stopping_point;
|
||||
stopping_point.z = inertial_nav.get_altitude();
|
||||
wp_nav.get_wp_stopping_point_xy(stopping_point);
|
||||
wp_nav->get_wp_stopping_point_xy(stopping_point);
|
||||
|
||||
// no need to check return status because terrain data is not used
|
||||
wp_nav.set_wp_destination(stopping_point, false);
|
||||
wp_nav->set_wp_destination(stopping_point, false);
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(get_default_auto_yaw_mode(false));
|
||||
@ -108,11 +108,11 @@ void Copter::guided_vel_control_start()
|
||||
guided_mode = Guided_Velocity;
|
||||
|
||||
// initialize vertical speeds and leash lengths
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise velocity controller
|
||||
pos_control.init_vel_controller_xyz();
|
||||
pos_control->init_vel_controller_xyz();
|
||||
}
|
||||
|
||||
// initialise guided mode's posvel controller
|
||||
@ -121,23 +121,23 @@ void Copter::guided_posvel_control_start()
|
||||
// set guided_mode to velocity controller
|
||||
guided_mode = Guided_PosVel;
|
||||
|
||||
pos_control.init_xy_controller();
|
||||
pos_control->init_xy_controller();
|
||||
|
||||
// set speed and acceleration from wpnav's speed and acceleration
|
||||
pos_control.set_speed_xy(wp_nav.get_speed_xy());
|
||||
pos_control.set_accel_xy(wp_nav.get_wp_acceleration());
|
||||
pos_control.set_jerk_xy_to_default();
|
||||
pos_control->set_speed_xy(wp_nav->get_speed_xy());
|
||||
pos_control->set_accel_xy(wp_nav->get_wp_acceleration());
|
||||
pos_control->set_jerk_xy_to_default();
|
||||
|
||||
const Vector3f& curr_pos = inertial_nav.get_position();
|
||||
const Vector3f& curr_vel = inertial_nav.get_velocity();
|
||||
|
||||
// set target position and velocity to current position and velocity
|
||||
pos_control.set_xy_target(curr_pos.x, curr_pos.y);
|
||||
pos_control.set_desired_velocity_xy(curr_vel.x, curr_vel.y);
|
||||
pos_control->set_xy_target(curr_pos.x, curr_pos.y);
|
||||
pos_control->set_desired_velocity_xy(curr_vel.x, curr_vel.y);
|
||||
|
||||
// set vertical speed and acceleration
|
||||
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
|
||||
pos_control.set_accel_z(wp_nav.get_accel_z());
|
||||
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
|
||||
pos_control->set_accel_z(wp_nav->get_accel_z());
|
||||
|
||||
// pilot always controls yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
@ -150,13 +150,13 @@ void Copter::guided_angle_control_start()
|
||||
guided_mode = Guided_Angle;
|
||||
|
||||
// set vertical speed and acceleration
|
||||
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
|
||||
pos_control.set_accel_z(wp_nav.get_accel_z());
|
||||
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
|
||||
pos_control->set_accel_z(wp_nav->get_accel_z());
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise targets
|
||||
@ -193,7 +193,7 @@ bool Copter::guided_set_destination(const Vector3f& destination)
|
||||
#endif
|
||||
|
||||
// no need to check return status because terrain data is not used
|
||||
wp_nav.set_wp_destination(destination, false);
|
||||
wp_nav->set_wp_destination(destination, false);
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, destination, Vector3f());
|
||||
@ -220,7 +220,7 @@ bool Copter::guided_set_destination(const Location_Class& dest_loc)
|
||||
}
|
||||
#endif
|
||||
|
||||
if (!wp_nav.set_wp_destination(dest_loc)) {
|
||||
if (!wp_nav->set_wp_destination(dest_loc)) {
|
||||
// failure to set destination can only be because of missing terrain data
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
// failure is propagated to GCS with NAK
|
||||
@ -259,7 +259,7 @@ void Copter::guided_set_destination_posvel(const Vector3f& destination, const Ve
|
||||
guided_pos_target_cm = destination;
|
||||
guided_vel_target_cms = velocity;
|
||||
|
||||
pos_control.set_pos_target(guided_pos_target_cm);
|
||||
pos_control->set_pos_target(guided_pos_target_cm);
|
||||
|
||||
// log target
|
||||
Log_Write_GuidedTarget(guided_mode, destination, velocity);
|
||||
@ -285,7 +285,7 @@ void Copter::guided_set_angle(const Quaternion &q, float climb_rate_cms, bool us
|
||||
guided_angle_state.update_time_ms = millis();
|
||||
|
||||
// interpret positive climb rate as triggering take-off
|
||||
if (motors.armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
|
||||
if (motors->armed() && !ap.auto_armed && (guided_angle_state.climb_rate_cms > 0.0f)) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
|
||||
@ -334,15 +334,15 @@ void Copter::guided_run()
|
||||
void Copter::guided_takeoff_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
@ -355,13 +355,13 @@ void Copter::guided_takeoff_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
auto_takeoff_attitude_run(target_yaw_rate);
|
||||
@ -372,15 +372,15 @@ void Copter::guided_takeoff_run()
|
||||
void Copter::guided_pos_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
@ -396,21 +396,21 @@ void Copter::guided_pos_control_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
@ -419,17 +419,17 @@ void Copter::guided_pos_control_run()
|
||||
void Copter::guided_vel_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
// initialise velocity controller
|
||||
pos_control.init_vel_controller_xyz();
|
||||
pos_control->init_vel_controller_xyz();
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
@ -445,26 +445,26 @@ void Copter::guided_vel_control_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// set velocity to zero if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control.get_desired_velocity().is_zero()) {
|
||||
if (tnow - vel_update_time_ms > GUIDED_POSVEL_TIMEOUT_MS && !pos_control->get_desired_velocity().is_zero()) {
|
||||
guided_set_desired_velocity_with_accel_and_fence_limits(Vector3f(0.0f,0.0f,0.0f));
|
||||
} else {
|
||||
guided_set_desired_velocity_with_accel_and_fence_limits(guided_vel_target_cms);
|
||||
}
|
||||
|
||||
// call velocity controller which includes z axis controller
|
||||
pos_control.update_vel_controller_xyz(ekfNavVelGainScaler);
|
||||
pos_control->update_vel_controller_xyz(ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
@ -473,18 +473,18 @@ void Copter::guided_vel_control_run()
|
||||
void Copter::guided_posvel_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
// set target position and velocity to current position and velocity
|
||||
pos_control.set_pos_target(inertial_nav.get_position());
|
||||
pos_control.set_desired_velocity(Vector3f(0,0,0));
|
||||
pos_control->set_pos_target(inertial_nav.get_position());
|
||||
pos_control->set_desired_velocity(Vector3f(0,0,0));
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
return;
|
||||
}
|
||||
@ -501,7 +501,7 @@ void Copter::guided_posvel_control_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// set velocity to zero if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
@ -510,10 +510,10 @@ void Copter::guided_posvel_control_run()
|
||||
}
|
||||
|
||||
// calculate dt
|
||||
float dt = pos_control.time_since_last_xy_update();
|
||||
float dt = pos_control->time_since_last_xy_update();
|
||||
|
||||
// update at poscontrol update rate
|
||||
if (dt >= pos_control.get_dt_xy()) {
|
||||
if (dt >= pos_control->get_dt_xy()) {
|
||||
// sanity check dt
|
||||
if (dt >= 0.2f) {
|
||||
dt = 0.0f;
|
||||
@ -523,22 +523,22 @@ void Copter::guided_posvel_control_run()
|
||||
guided_pos_target_cm += guided_vel_target_cms * dt;
|
||||
|
||||
// send position and velocity targets to position controller
|
||||
pos_control.set_pos_target(guided_pos_target_cm);
|
||||
pos_control.set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
|
||||
pos_control->set_pos_target(guided_pos_target_cm);
|
||||
pos_control->set_desired_velocity_xy(guided_vel_target_cms.x, guided_vel_target_cms.y);
|
||||
|
||||
// run position controller
|
||||
pos_control.update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
|
||||
pos_control->update_xy_controller(AC_PosControl::XY_MODE_POS_AND_VEL_FF, ekfNavVelGainScaler, false);
|
||||
}
|
||||
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(pos_control.get_roll(), pos_control.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(pos_control->get_roll(), pos_control->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(pos_control.get_roll(), pos_control.get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(pos_control->get_roll(), pos_control->get_pitch(), get_auto_heading(), true, get_smoothing_gain());
|
||||
}
|
||||
}
|
||||
|
||||
@ -547,18 +547,18 @@ void Copter::guided_posvel_control_run()
|
||||
void Copter::guided_angle_control_run()
|
||||
{
|
||||
// if not auto armed or motors not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || (ap.land_complete && guided_angle_state.climb_rate_cms <= 0.0f)) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0.0f,false,g.throttle_filt);
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0.0f,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0.0f,true,g.throttle_filt);
|
||||
#endif
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
pos_control->relax_alt_hold_controllers(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -566,7 +566,7 @@ void Copter::guided_angle_control_run()
|
||||
float roll_in = guided_angle_state.roll_cd;
|
||||
float pitch_in = guided_angle_state.pitch_cd;
|
||||
float total_in = norm(roll_in, pitch_in);
|
||||
float angle_max = MIN(attitude_control.get_althold_lean_angle_max(), aparm.angle_max);
|
||||
float angle_max = MIN(attitude_control->get_althold_lean_angle_max(), aparm.angle_max);
|
||||
if (total_in > angle_max) {
|
||||
float ratio = angle_max / total_in;
|
||||
roll_in *= ratio;
|
||||
@ -578,7 +578,7 @@ void Copter::guided_angle_control_run()
|
||||
float yaw_rate_in = wrap_180_cd(guided_angle_state.yaw_rate_cds);
|
||||
|
||||
// constrain climb rate
|
||||
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav.get_speed_down()), wp_nav.get_speed_up());
|
||||
float climb_rate_cms = constrain_float(guided_angle_state.climb_rate_cms, -fabsf(wp_nav->get_speed_down()), wp_nav->get_speed_up());
|
||||
|
||||
// check for timeout - set lean angles and climb rate to zero if no updates received for 3 seconds
|
||||
uint32_t tnow = millis();
|
||||
@ -590,25 +590,25 @@ void Copter::guided_angle_control_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
if (guided_angle_state.use_yaw_rate) {
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(roll_in, pitch_in, yaw_rate_in, get_smoothing_gain());
|
||||
} else {
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(climb_rate_cms, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
// helper function to update position controller's desired velocity while respecting acceleration limits
|
||||
void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vector3f& vel_des)
|
||||
{
|
||||
// get current desired velocity
|
||||
Vector3f curr_vel_des = pos_control.get_desired_velocity();
|
||||
Vector3f curr_vel_des = pos_control->get_desired_velocity();
|
||||
|
||||
// exit immediately if already equal
|
||||
if (curr_vel_des == vel_des) {
|
||||
@ -620,7 +620,7 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
|
||||
|
||||
// limit xy change
|
||||
float vel_delta_xy = safe_sqrt(sq(vel_delta.x)+sq(vel_delta.y));
|
||||
float vel_delta_xy_max = G_Dt * pos_control.get_accel_xy();
|
||||
float vel_delta_xy_max = G_Dt * pos_control->get_accel_xy();
|
||||
float ratio_xy = 1.0f;
|
||||
if (!is_zero(vel_delta_xy) && (vel_delta_xy > vel_delta_xy_max)) {
|
||||
ratio_xy = vel_delta_xy_max / vel_delta_xy;
|
||||
@ -629,16 +629,16 @@ void Copter::guided_set_desired_velocity_with_accel_and_fence_limits(const Vecto
|
||||
curr_vel_des.y += (vel_delta.y * ratio_xy);
|
||||
|
||||
// limit z change
|
||||
float vel_delta_z_max = G_Dt * pos_control.get_accel_z();
|
||||
float vel_delta_z_max = G_Dt * pos_control->get_accel_z();
|
||||
curr_vel_des.z += constrain_float(vel_delta.z, -vel_delta_z_max, vel_delta_z_max);
|
||||
|
||||
#if AC_AVOID_ENABLED
|
||||
// limit the velocity to prevent fence violations
|
||||
avoid.adjust_velocity(pos_control.get_pos_xy_kP(), pos_control.get_accel_xy(), curr_vel_des);
|
||||
avoid.adjust_velocity(pos_control->get_pos_xy_kP(), pos_control->get_accel_xy(), curr_vel_des);
|
||||
#endif
|
||||
|
||||
// update position controller with new target
|
||||
pos_control.set_desired_velocity(curr_vel_des);
|
||||
pos_control->set_desired_velocity(curr_vel_des);
|
||||
}
|
||||
|
||||
// Guided Limit code
|
||||
|
@ -13,18 +13,18 @@ bool Copter::land_init(bool ignore_checks)
|
||||
if (land_with_gps) {
|
||||
// set target to stopping point
|
||||
Vector3f stopping_point;
|
||||
wp_nav.get_loiter_stopping_point_xy(stopping_point);
|
||||
wp_nav.init_loiter_target(stopping_point);
|
||||
wp_nav->get_loiter_stopping_point_xy(stopping_point);
|
||||
wp_nav->init_loiter_target(stopping_point);
|
||||
}
|
||||
|
||||
// initialize vertical speeds and leash lengths
|
||||
pos_control.set_speed_z(wp_nav.get_speed_down(), wp_nav.get_speed_up());
|
||||
pos_control.set_accel_z(wp_nav.get_accel_z());
|
||||
pos_control->set_speed_z(wp_nav->get_speed_down(), wp_nav->get_speed_up());
|
||||
pos_control->set_accel_z(wp_nav->get_accel_z());
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
land_start_time = millis();
|
||||
@ -54,17 +54,17 @@ void Copter::land_run()
|
||||
void Copter::land_gps_run()
|
||||
{
|
||||
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
// disarm when the landing detector says we've landed
|
||||
if (ap.land_complete) {
|
||||
@ -74,7 +74,7 @@ void Copter::land_gps_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// pause before beginning land descent
|
||||
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
|
||||
@ -114,15 +114,15 @@ void Copter::land_nogps_run()
|
||||
}
|
||||
|
||||
// if not auto armed or landed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
|
||||
// disarm when the landing detector says we've landed
|
||||
@ -133,10 +133,10 @@ void Copter::land_nogps_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// pause before beginning land descent
|
||||
if(land_pause && millis()-land_start_time >= LAND_WITH_DELAY_MS) {
|
||||
@ -155,7 +155,7 @@ int32_t Copter::land_get_alt_above_ground(void)
|
||||
if (rangefinder_alt_ok()) {
|
||||
alt_above_ground = rangefinder_state.alt_cm_filt.get();
|
||||
} else {
|
||||
bool navigating = pos_control.is_active_xy();
|
||||
bool navigating = pos_control->is_active_xy();
|
||||
if (!navigating || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, alt_above_ground)) {
|
||||
alt_above_ground = current_loc.alt;
|
||||
}
|
||||
@ -165,7 +165,7 @@ int32_t Copter::land_get_alt_above_ground(void)
|
||||
|
||||
void Copter::land_run_vertical_control(bool pause_descent)
|
||||
{
|
||||
bool navigating = pos_control.is_active_xy();
|
||||
bool navigating = pos_control->is_active_xy();
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
bool doing_precision_landing = !ap.land_repo_active && precland.target_acquired() && navigating;
|
||||
@ -184,21 +184,21 @@ void Copter::land_run_vertical_control(bool pause_descent)
|
||||
if (g.land_speed_high > 0) {
|
||||
max_land_descent_velocity = -g.land_speed_high;
|
||||
} else {
|
||||
max_land_descent_velocity = pos_control.get_speed_down();
|
||||
max_land_descent_velocity = pos_control->get_speed_down();
|
||||
}
|
||||
|
||||
// Don't speed up for landing.
|
||||
max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
|
||||
|
||||
// Compute a vertical velocity demand such that the vehicle approaches LAND_START_ALT. Without the below constraint, this would cause the vehicle to hover at LAND_START_ALT.
|
||||
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control.get_accel_z());
|
||||
cmb_rate = AC_AttitudeControl::sqrt_controller(LAND_START_ALT-alt_above_ground, g.p_alt_hold.kP(), pos_control->get_accel_z());
|
||||
|
||||
// Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
|
||||
cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
|
||||
|
||||
if (doing_precision_landing && rangefinder_alt_ok() && rangefinder_state.alt_cm > 35.0f && rangefinder_state.alt_cm < 200.0f) {
|
||||
float max_descent_speed = abs(g.land_speed)/2.0f;
|
||||
float land_slowdown = MAX(0.0f, pos_control.get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
|
||||
float land_slowdown = MAX(0.0f, pos_control->get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
|
||||
cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
|
||||
}
|
||||
}
|
||||
@ -207,8 +207,8 @@ void Copter::land_run_vertical_control(bool pause_descent)
|
||||
desired_climb_rate = cmb_rate;
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(cmb_rate, G_Dt, true);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
|
||||
void Copter::land_run_horizontal_control()
|
||||
@ -218,7 +218,7 @@ void Copter::land_run_horizontal_control()
|
||||
|
||||
// relax loiter target if we might be landed
|
||||
if (ap.land_complete_maybe) {
|
||||
wp_nav.loiter_soften_for_landing();
|
||||
wp_nav->loiter_soften_for_landing();
|
||||
}
|
||||
|
||||
// process pilot inputs
|
||||
@ -262,19 +262,19 @@ void Copter::land_run_horizontal_control()
|
||||
target_vel_rel.x = -inertial_nav.get_velocity().x;
|
||||
target_vel_rel.y = -inertial_nav.get_velocity().y;
|
||||
}
|
||||
pos_control.set_xy_target(target_pos.x, target_pos.y);
|
||||
pos_control.override_vehicle_velocity_xy(-target_vel_rel);
|
||||
pos_control->set_xy_target(target_pos.x, target_pos.y);
|
||||
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
|
||||
}
|
||||
#endif
|
||||
|
||||
// process roll, pitch inputs
|
||||
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
|
||||
wp_nav->set_pilot_desired_acceleration(roll_control, pitch_control);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
int32_t nav_roll = wp_nav.get_roll();
|
||||
int32_t nav_pitch = wp_nav.get_pitch();
|
||||
int32_t nav_roll = wp_nav->get_roll();
|
||||
int32_t nav_pitch = wp_nav->get_pitch();
|
||||
|
||||
if (g2.wp_navalt_min > 0) {
|
||||
// user has requested an altitude below which navigation
|
||||
@ -293,13 +293,13 @@ void Copter::land_run_horizontal_control()
|
||||
nav_pitch *= ratio;
|
||||
|
||||
// tell position controller we are applying an external limit
|
||||
pos_control.set_limit_accel_xy();
|
||||
pos_control->set_limit_accel_xy();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// land_do_not_use_GPS - forces land-mode to not use the GPS but instead rely on pilot input for roll and pitch
|
||||
|
@ -9,7 +9,7 @@ bool Copter::loiter_init(bool ignore_checks)
|
||||
{
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// do not allow helis to enter Loiter if the Rotor Runup is not complete
|
||||
if (!ignore_checks && !motors.rotor_runup_complete()){
|
||||
if (!ignore_checks && !motors->rotor_runup_complete()){
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
@ -17,16 +17,16 @@ bool Copter::loiter_init(bool ignore_checks)
|
||||
if (position_ok() || ignore_checks) {
|
||||
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
// initialize vertical speed and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -45,7 +45,7 @@ bool Copter::do_precision_loiter() const
|
||||
return false; // don't move on the ground
|
||||
}
|
||||
// if the pilot *really* wants to move the vehicle, let them....
|
||||
if (wp_nav.get_pilot_desired_acceleration().length() > 50.0f) {
|
||||
if (wp_nav->get_pilot_desired_acceleration().length() > 50.0f) {
|
||||
return false;
|
||||
}
|
||||
if (!precland.target_acquired()) {
|
||||
@ -56,7 +56,7 @@ bool Copter::do_precision_loiter() const
|
||||
|
||||
void Copter::precision_loiter_xy()
|
||||
{
|
||||
wp_nav.clear_pilot_desired_acceleration();
|
||||
wp_nav->clear_pilot_desired_acceleration();
|
||||
Vector2f target_pos, target_vel_rel;
|
||||
if (!precland.get_target_position_cm(target_pos)) {
|
||||
target_pos.x = inertial_nav.get_position().x;
|
||||
@ -66,8 +66,8 @@ void Copter::precision_loiter_xy()
|
||||
target_vel_rel.x = -inertial_nav.get_velocity().x;
|
||||
target_vel_rel.y = -inertial_nav.get_velocity().y;
|
||||
}
|
||||
pos_control.set_xy_target(target_pos.x, target_pos.y);
|
||||
pos_control.override_vehicle_velocity_xy(-target_vel_rel);
|
||||
pos_control->set_xy_target(target_pos.x, target_pos.y);
|
||||
pos_control->override_vehicle_velocity_xy(-target_vel_rel);
|
||||
}
|
||||
#endif
|
||||
|
||||
@ -81,8 +81,8 @@ void Copter::loiter_run()
|
||||
float takeoff_climb_rate = 0.0f;
|
||||
|
||||
// initialize vertical speed and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// process pilot inputs unless we are in radio failsafe
|
||||
if (!failsafe.radio) {
|
||||
@ -90,7 +90,7 @@ void Copter::loiter_run()
|
||||
update_simple_mode();
|
||||
|
||||
// process pilot's roll and pitch input
|
||||
wp_nav.set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in());
|
||||
wp_nav->set_pilot_desired_acceleration(channel_roll->get_control_in(), channel_pitch->get_control_in());
|
||||
|
||||
// get pilot's desired yaw rate
|
||||
target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
|
||||
@ -100,23 +100,23 @@ void Copter::loiter_run()
|
||||
target_climb_rate = constrain_float(target_climb_rate, -g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
} else {
|
||||
// clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
|
||||
wp_nav.clear_pilot_desired_acceleration();
|
||||
wp_nav->clear_pilot_desired_acceleration();
|
||||
}
|
||||
|
||||
// relax loiter target if we might be landed
|
||||
if (ap.land_complete_maybe) {
|
||||
wp_nav.loiter_soften_for_landing();
|
||||
wp_nav->loiter_soften_for_landing();
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// helicopters are held on the ground until rotor speed runup has finished
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
|
||||
#else
|
||||
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
|
||||
#endif
|
||||
|
||||
// Loiter State Machine Determination
|
||||
if (!motors.armed() || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !motors->get_interlock()) {
|
||||
loiter_state = Loiter_MotorStopped;
|
||||
} else if (takeoff_state.running || takeoff_triggered) {
|
||||
loiter_state = Loiter_Takeoff;
|
||||
@ -131,24 +131,24 @@ void Copter::loiter_run()
|
||||
|
||||
case Loiter_MotorStopped:
|
||||
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// force descent rate and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
#else
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
wp_nav->init_loiter_target();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
#endif
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
pos_control.update_z_controller();
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Loiter_Takeoff:
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// initiate take-off
|
||||
if (!takeoff_state.running) {
|
||||
@ -163,36 +163,36 @@ void Copter::loiter_run()
|
||||
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Loiter_Landed:
|
||||
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
||||
if (target_climb_rate < 0.0f) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
}
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control.update_z_controller();
|
||||
wp_nav->init_loiter_target();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Loiter_Flying:
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
#if PRECISION_LANDING == ENABLED
|
||||
if (do_precision_loiter()) {
|
||||
@ -201,20 +201,20 @@ void Copter::loiter_run()
|
||||
#endif
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -77,7 +77,7 @@ bool Copter::poshold_init(bool ignore_checks)
|
||||
{
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// do not allow helis to enter Pos Hold if the Rotor Runup is not complete
|
||||
if (!ignore_checks && !motors.rotor_runup_complete()){
|
||||
if (!ignore_checks && !motors->rotor_runup_complete()){
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
@ -88,13 +88,13 @@ bool Copter::poshold_init(bool ignore_checks)
|
||||
}
|
||||
|
||||
// initialize vertical speeds and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise lean angles to current attitude
|
||||
@ -110,7 +110,7 @@ bool Copter::poshold_init(bool ignore_checks)
|
||||
poshold.pitch_mode = POSHOLD_LOITER;
|
||||
// set target to current position
|
||||
// only init here as we can switch to PosHold in flight with a velocity <> 0 that will be used as _last_vel in PosControl and never updated again as we inhibit Reset_I
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
}else{
|
||||
// if not landed start in pilot override to avoid hard twitch
|
||||
poshold.roll_mode = POSHOLD_PILOT_OVERRIDE;
|
||||
@ -144,15 +144,15 @@ void Copter::poshold_run()
|
||||
const Vector3f& vel = inertial_nav.get_velocity();
|
||||
|
||||
// initialize vertical speeds and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
pos_control.relax_alt_hold_controllers(0.0f);
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
wp_nav->init_loiter_target();
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
pos_control->relax_alt_hold_controllers(0.0f);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -174,7 +174,7 @@ void Copter::poshold_run()
|
||||
// check for take-off
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// helicopters are held on the ground until rotor speed runup has finished
|
||||
if (ap.land_complete && (takeoff_state.running || (target_climb_rate > 0.0f && motors.rotor_runup_complete()))) {
|
||||
if (ap.land_complete && (takeoff_state.running || (target_climb_rate > 0.0f && motors->rotor_runup_complete()))) {
|
||||
#else
|
||||
if (ap.land_complete && (takeoff_state.running || target_climb_rate > 0.0f)) {
|
||||
#endif
|
||||
@ -191,23 +191,23 @@ void Copter::poshold_run()
|
||||
|
||||
// relax loiter target if we might be landed
|
||||
if (ap.land_complete_maybe) {
|
||||
wp_nav.loiter_soften_for_landing();
|
||||
wp_nav->loiter_soften_for_landing();
|
||||
}
|
||||
|
||||
// if landed initialise loiter targets, set throttle to zero and exit
|
||||
if (ap.land_complete) {
|
||||
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
||||
if (target_climb_rate < 0.0f) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
}
|
||||
wp_nav.init_loiter_target();
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control.update_z_controller();
|
||||
wp_nav->init_loiter_target();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control->update_z_controller();
|
||||
return;
|
||||
}else{
|
||||
// convert pilot input to lean angles
|
||||
@ -411,7 +411,7 @@ void Copter::poshold_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
//
|
||||
// Shared roll & pitch states (POSHOLD_BRAKE_TO_LOITER and POSHOLD_LOITER)
|
||||
@ -423,7 +423,7 @@ void Copter::poshold_run()
|
||||
poshold.pitch_mode = POSHOLD_BRAKE_TO_LOITER;
|
||||
poshold.brake_to_loiter_timer = POSHOLD_BRAKE_TO_LOITER_TIMER;
|
||||
// init loiter controller
|
||||
wp_nav.init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav.init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
|
||||
wp_nav->init_loiter_target(inertial_nav.get_position(), poshold.loiter_reset_I); // (false) to avoid I_term reset. In original code, velocity(0,0,0) was used instead of current velocity: wp_nav->init_loiter_target(inertial_nav.get_position(), Vector3f(0,0,0));
|
||||
// at this stage, we are going to run update_loiter that will reset I_term once. From now, we ensure next time that we will enter loiter and update it, I_term won't be reset anymore
|
||||
poshold.loiter_reset_I = false;
|
||||
// set delay to start of wind compensation estimate updates
|
||||
@ -456,11 +456,11 @@ void Copter::poshold_run()
|
||||
poshold_update_brake_angle_from_velocity(poshold.brake_pitch, -vel_fw);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// calculate final roll and pitch output by mixing loiter and brake controls
|
||||
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav.get_roll());
|
||||
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav.get_pitch());
|
||||
poshold.roll = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_roll + poshold.wind_comp_roll, wp_nav->get_roll());
|
||||
poshold.pitch = poshold_mix_controls(brake_to_loiter_mix, poshold.brake_pitch + poshold.wind_comp_pitch, wp_nav->get_pitch());
|
||||
|
||||
// check for pilot input
|
||||
if (!is_zero(target_roll) || !is_zero(target_pitch)) {
|
||||
@ -487,11 +487,11 @@ void Copter::poshold_run()
|
||||
|
||||
case POSHOLD_LOITER:
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// set roll angle based on loiter controller outputs
|
||||
poshold.roll = wp_nav.get_roll();
|
||||
poshold.pitch = wp_nav.get_pitch();
|
||||
poshold.roll = wp_nav->get_roll();
|
||||
poshold.pitch = wp_nav->get_pitch();
|
||||
|
||||
// update wind compensation estimate
|
||||
poshold_update_wind_comp_estimate();
|
||||
@ -531,17 +531,17 @@ void Copter::poshold_run()
|
||||
poshold.pitch = constrain_int16(poshold.pitch, -aparm.angle_max, aparm.angle_max);
|
||||
|
||||
// update attitude controller targets
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(poshold.roll, poshold.pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
// update altitude target and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control->update_z_controller();
|
||||
}
|
||||
}
|
||||
|
||||
@ -618,7 +618,7 @@ void Copter::poshold_update_wind_comp_estimate()
|
||||
|
||||
// get position controller accel target
|
||||
// To-Do: clean this up by using accessor in loiter controller (or move entire PosHold controller to a library shared with loiter)
|
||||
const Vector3f& accel_target = pos_control.get_accel_target();
|
||||
const Vector3f& accel_target = pos_control->get_accel_target();
|
||||
|
||||
// update wind compensation in earth-frame lean angles
|
||||
if (is_zero(poshold.wind_comp_ef.x)) {
|
||||
|
@ -92,21 +92,21 @@ void Copter::rtl_climb_start()
|
||||
rtl_state_complete = false;
|
||||
|
||||
// initialise waypoint and spline controller
|
||||
wp_nav.wp_and_spline_init();
|
||||
wp_nav->wp_and_spline_init();
|
||||
|
||||
// RTL_SPEED == 0 means use WPNAV_SPEED
|
||||
if (g.rtl_speed_cms != 0) {
|
||||
wp_nav.set_speed_xy(g.rtl_speed_cms);
|
||||
wp_nav->set_speed_xy(g.rtl_speed_cms);
|
||||
}
|
||||
|
||||
// set the destination
|
||||
if (!wp_nav.set_wp_destination(rtl_path.climb_target)) {
|
||||
if (!wp_nav->set_wp_destination(rtl_path.climb_target)) {
|
||||
// this should not happen because rtl_build_path will have checked terrain data was available
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION);
|
||||
set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE);
|
||||
return;
|
||||
}
|
||||
wp_nav.set_fast_waypoint(true);
|
||||
wp_nav->set_fast_waypoint(true);
|
||||
|
||||
// hold current yaw during initial climb
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
@ -118,7 +118,7 @@ void Copter::rtl_return_start()
|
||||
rtl_state = RTL_ReturnHome;
|
||||
rtl_state_complete = false;
|
||||
|
||||
if (!wp_nav.set_wp_destination(rtl_path.return_target)) {
|
||||
if (!wp_nav->set_wp_destination(rtl_path.return_target)) {
|
||||
// failure must be caused by missing terrain data, restart RTL
|
||||
rtl_restart_without_terrain();
|
||||
}
|
||||
@ -132,16 +132,16 @@ void Copter::rtl_return_start()
|
||||
void Copter::rtl_climb_return_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// reset attitude control targets
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
@ -158,25 +158,25 @@ void Copter::rtl_climb_return_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
rtl_state_complete = wp_nav.reached_wp_destination();
|
||||
rtl_state_complete = wp_nav->reached_wp_destination();
|
||||
}
|
||||
|
||||
// rtl_loiterathome_start - initialise return to home
|
||||
@ -199,16 +199,16 @@ void Copter::rtl_loiterathome_start()
|
||||
void Copter::rtl_loiterathome_run()
|
||||
{
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
// reset attitude control targets
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// To-Do: re-initialise wpnav targets
|
||||
return;
|
||||
@ -225,21 +225,21 @@ void Copter::rtl_loiterathome_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run waypoint controller
|
||||
failsafe_terrain_set_status(wp_nav.update_wpnav());
|
||||
failsafe_terrain_set_status(wp_nav->update_wpnav());
|
||||
|
||||
// call z-axis position controller (wpnav should have already updated it's alt target)
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// call attitude controller
|
||||
if (auto_yaw_mode == AUTO_YAW_HOLD) {
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
}else{
|
||||
// roll, pitch from waypoint controller, yaw heading from auto_heading()
|
||||
attitude_control.input_euler_angle_roll_pitch_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), get_auto_heading(),true, get_smoothing_gain());
|
||||
}
|
||||
|
||||
// check if we've completed this stage of RTL
|
||||
@ -263,10 +263,10 @@ void Copter::rtl_descent_start()
|
||||
rtl_state_complete = false;
|
||||
|
||||
// Set wp navigation target to above home
|
||||
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
|
||||
wp_nav->init_loiter_target(wp_nav->get_wp_destination());
|
||||
|
||||
// initialise altitude target to stopping point
|
||||
pos_control.set_target_to_stopping_point_z();
|
||||
pos_control->set_target_to_stopping_point_z();
|
||||
|
||||
// initialise yaw
|
||||
set_auto_yaw_mode(AUTO_YAW_HOLD);
|
||||
@ -280,18 +280,18 @@ void Copter::rtl_descent_run()
|
||||
float target_yaw_rate = 0;
|
||||
|
||||
// if not auto armed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
return;
|
||||
}
|
||||
|
||||
@ -319,20 +319,20 @@ void Copter::rtl_descent_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// process roll, pitch inputs
|
||||
wp_nav.set_pilot_desired_acceleration(roll_control, pitch_control);
|
||||
wp_nav->set_pilot_desired_acceleration(roll_control, pitch_control);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call z-axis position controller
|
||||
pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt);
|
||||
pos_control->update_z_controller();
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// check if we've reached within 20cm of final altitude
|
||||
rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f;
|
||||
@ -345,12 +345,12 @@ void Copter::rtl_land_start()
|
||||
rtl_state_complete = false;
|
||||
|
||||
// Set wp navigation target to above home
|
||||
wp_nav.init_loiter_target(wp_nav.get_wp_destination());
|
||||
wp_nav->init_loiter_target(wp_nav->get_wp_destination());
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
// initialise yaw
|
||||
@ -362,18 +362,18 @@ void Copter::rtl_land_start()
|
||||
void Copter::rtl_land_run()
|
||||
{
|
||||
// if not auto armed or landing completed or motor interlock not enabled set throttle to zero and exit immediately
|
||||
if (!motors.armed() || !ap.auto_armed || ap.land_complete || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !ap.auto_armed || ap.land_complete || !motors->get_interlock()) {
|
||||
#if FRAME_CONFIG == HELI_FRAME // Helicopters always stabilize roll/pitch/yaw
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control.set_throttle_out(0,false,g.throttle_filt);
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0, 0, 0, get_smoothing_gain());
|
||||
attitude_control->set_throttle_out(0,false,g.throttle_filt);
|
||||
#else
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
// multicopters do not stabilize roll/pitch/yaw when disarmed
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
#endif
|
||||
// set target to current position
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
// disarm when the landing detector says we've landed
|
||||
if (ap.land_complete) {
|
||||
@ -386,7 +386,7 @@ void Copter::rtl_land_run()
|
||||
}
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
land_run_horizontal_control();
|
||||
land_run_vertical_control();
|
||||
@ -399,8 +399,8 @@ void Copter::rtl_build_path(bool terrain_following_allowed)
|
||||
{
|
||||
// origin point is our stopping point
|
||||
Vector3f stopping_point;
|
||||
pos_control.get_stopping_point_xy(stopping_point);
|
||||
pos_control.get_stopping_point_z(stopping_point);
|
||||
pos_control->get_stopping_point_xy(stopping_point);
|
||||
pos_control->get_stopping_point_z(stopping_point);
|
||||
rtl_path.origin_point = Location_Class(stopping_point);
|
||||
rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME);
|
||||
|
||||
|
@ -8,13 +8,13 @@
|
||||
bool Copter::sport_init(bool ignore_checks)
|
||||
{
|
||||
// initialize vertical speed and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// initialise position and desired velocity
|
||||
if (!pos_control.is_active_z()) {
|
||||
pos_control.set_alt_target_to_current_alt();
|
||||
pos_control.set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
if (!pos_control->is_active_z()) {
|
||||
pos_control->set_alt_target_to_current_alt();
|
||||
pos_control->set_desired_velocity_z(inertial_nav.get_velocity_z());
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -28,8 +28,8 @@ void Copter::sport_run()
|
||||
float takeoff_climb_rate = 0.0f;
|
||||
|
||||
// initialize vertical speed and acceleration
|
||||
pos_control.set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control.set_accel_z(g.pilot_accel_z);
|
||||
pos_control->set_speed_z(-g.pilot_velocity_z_max, g.pilot_velocity_z_max);
|
||||
pos_control->set_accel_z(g.pilot_accel_z);
|
||||
|
||||
// apply SIMPLE mode transform
|
||||
update_simple_mode();
|
||||
@ -68,13 +68,13 @@ void Copter::sport_run()
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// helicopters are held on the ground until rotor speed runup has finished
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors.rotor_runup_complete());
|
||||
bool takeoff_triggered = (ap.land_complete && (target_climb_rate > 0.0f) && motors->rotor_runup_complete());
|
||||
#else
|
||||
bool takeoff_triggered = ap.land_complete && (target_climb_rate > 0.0f);
|
||||
#endif
|
||||
|
||||
// State Machine Determination
|
||||
if (!motors.armed() || !motors.get_interlock()) {
|
||||
if (!motors->armed() || !motors->get_interlock()) {
|
||||
sport_state = Sport_MotorStopped;
|
||||
} else if (takeoff_state.running || takeoff_triggered) {
|
||||
sport_state = Sport_Takeoff;
|
||||
@ -89,23 +89,23 @@ void Copter::sport_run()
|
||||
|
||||
case Sport_MotorStopped:
|
||||
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// force descent rate and call position controller
|
||||
pos_control.set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
pos_control->set_alt_target_from_climb_rate(-abs(g.land_speed), G_Dt, false);
|
||||
#else
|
||||
attitude_control.relax_attitude_controllers();
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
attitude_control->relax_attitude_controllers();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
#endif
|
||||
pos_control.update_z_controller();
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Sport_Takeoff:
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// initiate take-off
|
||||
if (!takeoff_state.running) {
|
||||
@ -120,43 +120,43 @@ void Copter::sport_run()
|
||||
takeoff_get_climb_rates(target_climb_rate, takeoff_climb_rate);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->add_takeoff_climb_rate(takeoff_climb_rate, G_Dt);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Sport_Landed:
|
||||
// set motors to spin-when-armed if throttle below deadzone, otherwise full range (but motors will only spin at min throttle)
|
||||
if (target_climb_rate < 0.0f) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
}
|
||||
|
||||
attitude_control.reset_rate_controller_I_terms();
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
pos_control.relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control.update_z_controller();
|
||||
attitude_control->reset_rate_controller_I_terms();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
pos_control->relax_alt_hold_controllers(0.0f); // forces throttle output to go to zero
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
|
||||
case Sport_Flying:
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
attitude_control->input_euler_rate_roll_pitch_yaw(target_roll_rate, target_pitch_rate, target_yaw_rate);
|
||||
|
||||
// adjust climb rate using rangefinder
|
||||
if (rangefinder_alt_ok()) {
|
||||
// if rangefinder is ok, use surface tracking
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control.get_alt_target(), G_Dt);
|
||||
target_climb_rate = get_surface_tracking_climb_rate(target_climb_rate, pos_control->get_alt_target(), G_Dt);
|
||||
}
|
||||
|
||||
// call position controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(target_climb_rate, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
@ -8,12 +8,12 @@
|
||||
bool Copter::stabilize_init(bool ignore_checks)
|
||||
{
|
||||
// if landed and the mode we're switching from does not have manual throttle and the throttle stick is too high
|
||||
if (motors.armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
||||
if (motors->armed() && ap.land_complete && !mode_has_manual_throttle(control_mode) &&
|
||||
(get_pilot_desired_throttle(channel_throttle->get_control_in()) > get_non_takeoff_throttle())) {
|
||||
return false;
|
||||
}
|
||||
// set target altitude to zero for reporting
|
||||
pos_control.set_alt_target(0);
|
||||
pos_control->set_alt_target(0);
|
||||
|
||||
return true;
|
||||
}
|
||||
@ -27,16 +27,16 @@ void Copter::stabilize_run()
|
||||
float pilot_throttle_scaled;
|
||||
|
||||
// if not armed set throttle to zero and exit immediately
|
||||
if (!motors.armed() || ap.throttle_zero || !motors.get_interlock()) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
if (!motors->armed() || ap.throttle_zero || !motors->get_interlock()) {
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
return;
|
||||
}
|
||||
|
||||
// clear landing flag
|
||||
set_land_complete(false);
|
||||
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// apply SIMPLE mode transform to pilot inputs
|
||||
update_simple_mode();
|
||||
@ -52,10 +52,10 @@ void Copter::stabilize_run()
|
||||
pilot_throttle_scaled = get_pilot_desired_throttle(channel_throttle->get_control_in());
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// body-frame rate controller is run directly from 100hz loop
|
||||
|
||||
// output pilot's throttle
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, true, g.throttle_filt);
|
||||
}
|
||||
|
@ -10,7 +10,7 @@ bool Copter::throw_init(bool ignore_checks)
|
||||
#endif
|
||||
|
||||
// do not enter the mode when already armed or when flying
|
||||
if (motors.armed()) {
|
||||
if (motors->armed()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -34,11 +34,11 @@ void Copter::throw_run()
|
||||
*/
|
||||
|
||||
// Don't enter THROW mode if interlock will prevent motors running
|
||||
if (!motors.armed() && motors.get_interlock()) {
|
||||
if (!motors->armed() && motors->get_interlock()) {
|
||||
// state machine entry is always from a disarmed state
|
||||
throw_state.stage = Throw_Disarmed;
|
||||
|
||||
} else if (throw_state.stage == Throw_Disarmed && motors.armed()) {
|
||||
} else if (throw_state.stage == Throw_Disarmed && motors->armed()) {
|
||||
gcs_send_text(MAV_SEVERITY_INFO,"waiting for throw");
|
||||
throw_state.stage = Throw_Detecting;
|
||||
|
||||
@ -55,20 +55,20 @@ void Copter::throw_run()
|
||||
|
||||
// initialize vertical speed and acceleration limits
|
||||
// use brake mode values for rapid response
|
||||
pos_control.set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
|
||||
pos_control.set_accel_z(BRAKE_MODE_DECEL_RATE);
|
||||
pos_control->set_speed_z(BRAKE_MODE_SPEED_Z, BRAKE_MODE_SPEED_Z);
|
||||
pos_control->set_accel_z(BRAKE_MODE_DECEL_RATE);
|
||||
|
||||
// initialise the demanded height to 3m above the throw height
|
||||
// we want to rapidly clear surrounding obstacles
|
||||
if (g2.throw_type == ThrowType_Drop) {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude() - 100);
|
||||
pos_control->set_alt_target(inertial_nav.get_altitude() - 100);
|
||||
} else {
|
||||
pos_control.set_alt_target(inertial_nav.get_altitude() + 300);
|
||||
pos_control->set_alt_target(inertial_nav.get_altitude() + 300);
|
||||
}
|
||||
|
||||
// set the initial velocity of the height controller demand to the measured velocity if it is going up
|
||||
// if it is going down, set it to zero to enforce a very hard stop
|
||||
pos_control.set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
|
||||
pos_control->set_desired_velocity_z(fmaxf(inertial_nav.get_velocity_z(),0.0f));
|
||||
|
||||
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
||||
set_auto_armed(true);
|
||||
@ -78,7 +78,7 @@ void Copter::throw_run()
|
||||
throw_state.stage = Throw_PosHold;
|
||||
|
||||
// initialise the loiter target to the curent position and velocity
|
||||
wp_nav.init_loiter_target();
|
||||
wp_nav->init_loiter_target();
|
||||
|
||||
// Set the auto_arm status to true to avoid a possible automatic disarm caused by selection of an auto mode with throttle at minimum
|
||||
set_auto_armed(true);
|
||||
@ -107,26 +107,26 @@ void Copter::throw_run()
|
||||
|
||||
// prevent motors from rotating before the throw is detected unless enabled by the user
|
||||
if (g.throw_motor_start == 1) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
}
|
||||
|
||||
// demand zero throttle (motors will be stopped anyway) and continually reset the attitude controller
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
break;
|
||||
|
||||
case Throw_Detecting:
|
||||
|
||||
// prevent motors from rotating before the throw is detected unless enabled by the user
|
||||
if (g.throw_motor_start == 1) {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SPIN_WHEN_ARMED);
|
||||
} else {
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_SHUT_DOWN);
|
||||
}
|
||||
|
||||
// Hold throttle at zero during the throw and continually reset the attitude controller
|
||||
attitude_control.set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
attitude_control->set_throttle_out_unstabilized(0,true,g.throttle_filt);
|
||||
|
||||
// Play the waiting for throw tone sequence to alert the user
|
||||
AP_Notify::flags.waiting_for_throw = true;
|
||||
@ -136,44 +136,44 @@ void Copter::throw_run()
|
||||
case Throw_Uprighting:
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// demand a level roll/pitch attitude with zero yaw rate
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
|
||||
// output 50% throttle and turn off angle boost to maximise righting moment
|
||||
attitude_control.set_throttle_out(0.5f, false, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(0.5f, false, g.throttle_filt);
|
||||
|
||||
break;
|
||||
|
||||
case Throw_HgtStabilise:
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(0.0f, 0.0f, 0.0f, get_smoothing_gain());
|
||||
|
||||
// call height controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
|
||||
break;
|
||||
|
||||
case Throw_PosHold:
|
||||
|
||||
// set motors to full range
|
||||
motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
motors->set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED);
|
||||
|
||||
// run loiter controller
|
||||
wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), 0.0f, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), 0.0f, get_smoothing_gain());
|
||||
|
||||
// call height controller
|
||||
pos_control.set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control.update_z_controller();
|
||||
pos_control->set_alt_target_from_climb_rate_ff(0.0f, G_Dt, false);
|
||||
pos_control->update_z_controller();
|
||||
|
||||
break;
|
||||
}
|
||||
@ -258,11 +258,11 @@ bool Copter::throw_attitude_good()
|
||||
bool Copter::throw_height_good()
|
||||
{
|
||||
// Check that we are no more than 0.5m below the demanded height
|
||||
return (pos_control.get_alt_error() < 50.0f);
|
||||
return (pos_control->get_alt_error() < 50.0f);
|
||||
}
|
||||
|
||||
bool Copter::throw_position_good()
|
||||
{
|
||||
// check that our horizontal position error is within 50cm
|
||||
return (pos_control.get_horizontal_error() < 50.0f);
|
||||
return (pos_control->get_horizontal_error() < 50.0f);
|
||||
}
|
||||
|
@ -13,7 +13,7 @@ void Copter::crash_check()
|
||||
static uint16_t crash_counter; // number of iterations vehicle may have been crashed
|
||||
|
||||
// return immediately if disarmed, or crash checking disabled
|
||||
if (!motors.armed() || ap.land_complete || g.fs_crash_check == 0) {
|
||||
if (!motors->armed() || ap.land_complete || g.fs_crash_check == 0) {
|
||||
crash_counter = 0;
|
||||
return;
|
||||
}
|
||||
@ -31,7 +31,7 @@ void Copter::crash_check()
|
||||
}
|
||||
|
||||
// check for angle error over 30 degrees
|
||||
const float angle_error = attitude_control.get_att_error_angle_deg();
|
||||
const float angle_error = attitude_control->get_att_error_angle_deg();
|
||||
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
|
||||
crash_counter = 0;
|
||||
return;
|
||||
@ -74,7 +74,7 @@ void Copter::parachute_check()
|
||||
parachute.update();
|
||||
|
||||
// return immediately if motors are not armed or pilot's throttle is above zero
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
control_loss_count = 0;
|
||||
return;
|
||||
}
|
||||
@ -97,7 +97,7 @@ void Copter::parachute_check()
|
||||
}
|
||||
|
||||
// check for angle error over 30 degrees
|
||||
const float angle_error = attitude_control.get_att_error_angle_deg();
|
||||
const float angle_error = attitude_control->get_att_error_angle_deg();
|
||||
if (angle_error <= CRASH_CHECK_ANGLE_DEVIATION_DEG) {
|
||||
if (control_loss_count > 0) {
|
||||
control_loss_count--;
|
||||
|
@ -35,7 +35,7 @@ void Copter::ekf_check()
|
||||
}
|
||||
|
||||
// return immediately if motors are not armed, ekf check is disabled, not using ekf or usb is connected
|
||||
if (!motors.armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
|
||||
if (!motors->armed() || ap.usb_connected || (g.fs_ekf_thresh <= 0.0f)) {
|
||||
ekf_check_state.fail_count = 0;
|
||||
ekf_check_state.bad_variance = false;
|
||||
AP_Notify::flags.ekf_bad = ekf_check_state.bad_variance;
|
||||
@ -122,7 +122,7 @@ void Copter::failsafe_ekf_event()
|
||||
}
|
||||
|
||||
// do nothing if motors disarmed
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -174,7 +174,7 @@ void Copter::check_ekf_reset()
|
||||
float yaw_angle_change_rad = 0.0f;
|
||||
uint32_t new_ekfYawReset_ms = ahrs.getLastYawResetAngle(yaw_angle_change_rad);
|
||||
if (new_ekfYawReset_ms != ekfYawReset_ms) {
|
||||
attitude_control.shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
|
||||
attitude_control->shift_ef_yaw_target(ToDeg(yaw_angle_change_rad) * 100.0f);
|
||||
ekfYawReset_ms = new_ekfYawReset_ms;
|
||||
Log_Write_Event(DATA_EKF_YAW_RESET);
|
||||
}
|
||||
|
@ -79,20 +79,20 @@ void Copter::esc_calibration_passthrough()
|
||||
// clear esc flag for next time
|
||||
g.esc_calibrate.set_and_save(ESCCAL_NONE);
|
||||
|
||||
if (motors.get_pwm_type() >= AP_Motors::PWM_TYPE_ONESHOT) {
|
||||
if (motors->get_pwm_type() >= AP_Motors::PWM_TYPE_ONESHOT) {
|
||||
// run at full speed for oneshot ESCs (actually done on push)
|
||||
motors.set_update_rate(g.rc_speed);
|
||||
motors->set_update_rate(g.rc_speed);
|
||||
} else {
|
||||
// reduce update rate to motors to 50Hz
|
||||
motors.set_update_rate(50);
|
||||
motors->set_update_rate(50);
|
||||
}
|
||||
|
||||
// send message to GCS
|
||||
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Passing pilot throttle to ESCs");
|
||||
|
||||
// arm motors
|
||||
motors.armed(true);
|
||||
motors.enable();
|
||||
motors->armed(true);
|
||||
motors->enable();
|
||||
|
||||
while(1) {
|
||||
// flash LEDS
|
||||
@ -106,7 +106,7 @@ void Copter::esc_calibration_passthrough()
|
||||
delay(3);
|
||||
|
||||
// pass through to motors
|
||||
motors.set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(channel_throttle->get_control_in() / 1000.0f);
|
||||
}
|
||||
#endif // FRAME_CONFIG != HELI_FRAME
|
||||
}
|
||||
@ -117,20 +117,20 @@ void Copter::esc_calibration_auto()
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
bool printed_msg = false;
|
||||
|
||||
if (motors.get_pwm_type() >= AP_Motors::PWM_TYPE_ONESHOT) {
|
||||
if (motors->get_pwm_type() >= AP_Motors::PWM_TYPE_ONESHOT) {
|
||||
// run at full speed for oneshot ESCs (actually done on push)
|
||||
motors.set_update_rate(g.rc_speed);
|
||||
motors->set_update_rate(g.rc_speed);
|
||||
} else {
|
||||
// reduce update rate to motors to 50Hz
|
||||
motors.set_update_rate(50);
|
||||
motors->set_update_rate(50);
|
||||
}
|
||||
|
||||
// send message to GCS
|
||||
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Auto calibration");
|
||||
|
||||
// arm and enable motors
|
||||
motors.armed(true);
|
||||
motors.enable();
|
||||
motors->armed(true);
|
||||
motors->enable();
|
||||
|
||||
// flash LEDS
|
||||
AP_Notify::flags.esc_calibration = true;
|
||||
@ -144,19 +144,19 @@ void Copter::esc_calibration_auto()
|
||||
gcs_send_text(MAV_SEVERITY_INFO,"ESC calibration: Push safety switch");
|
||||
printed_msg = true;
|
||||
}
|
||||
motors.set_throttle_passthrough_for_esc_calibration(1.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(1.0f);
|
||||
delay(3);
|
||||
}
|
||||
|
||||
// delay for 5 seconds while outputting pulses
|
||||
uint32_t tstart = millis();
|
||||
while (millis() - tstart < 5000) {
|
||||
motors.set_throttle_passthrough_for_esc_calibration(1.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(1.0f);
|
||||
delay(3);
|
||||
}
|
||||
|
||||
// reduce throttle to minimum
|
||||
motors.set_throttle_passthrough_for_esc_calibration(0.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(0.0f);
|
||||
|
||||
// clear esc parameter
|
||||
g.esc_calibrate.set_and_save(ESCCAL_NONE);
|
||||
@ -164,7 +164,7 @@ void Copter::esc_calibration_auto()
|
||||
// block until we restart
|
||||
while(1) {
|
||||
delay(3);
|
||||
motors.set_throttle_passthrough_for_esc_calibration(0.0f);
|
||||
motors->set_throttle_passthrough_for_esc_calibration(0.0f);
|
||||
}
|
||||
#endif // FRAME_CONFIG != HELI_FRAME
|
||||
}
|
||||
|
@ -7,7 +7,7 @@
|
||||
void Copter::failsafe_radio_on_event()
|
||||
{
|
||||
// if motors are not armed there is nothing to do
|
||||
if( !motors.armed() ) {
|
||||
if( !motors->armed() ) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -50,7 +50,7 @@ void Copter::failsafe_battery_event(void)
|
||||
}
|
||||
|
||||
// failsafe check
|
||||
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors.armed()) {
|
||||
if (g.failsafe_battery_enabled != FS_BATT_DISABLED && motors->armed()) {
|
||||
if (should_disarm_on_failsafe()) {
|
||||
init_disarm_motors();
|
||||
} else {
|
||||
@ -97,7 +97,7 @@ void Copter::failsafe_gcs_check()
|
||||
}
|
||||
|
||||
// do nothing if gcs failsafe already triggered or motors disarmed
|
||||
if (failsafe.gcs || !motors.armed()) {
|
||||
if (failsafe.gcs || !motors->armed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -50,11 +50,11 @@ void Copter::failsafe_check()
|
||||
if (!in_failsafe && failsafe_enabled && tnow - failsafe_last_timestamp > 2000000) {
|
||||
// motors are running but we have gone 2 second since the
|
||||
// main loop ran. That means we're in trouble and should
|
||||
// disarm the motors.
|
||||
// disarm the motors->
|
||||
in_failsafe = true;
|
||||
// reduce motors to minimum (we do not immediately disarm because we want to log the failure)
|
||||
if (motors.armed()) {
|
||||
motors.output_min();
|
||||
if (motors->armed()) {
|
||||
motors->output_min();
|
||||
}
|
||||
// log an error
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_CPU,ERROR_CODE_FAILSAFE_OCCURRED);
|
||||
@ -63,9 +63,9 @@ void Copter::failsafe_check()
|
||||
if (failsafe_enabled && in_failsafe && tnow - failsafe_last_timestamp > 1000000) {
|
||||
// disarm motors every second
|
||||
failsafe_last_timestamp = tnow;
|
||||
if(motors.armed()) {
|
||||
motors.armed(false);
|
||||
motors.output();
|
||||
if(motors->armed()) {
|
||||
motors->armed(false);
|
||||
motors->output();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@ -18,7 +18,7 @@ void Copter::fence_check()
|
||||
new_breaches = fence.check_fence(current_loc.alt/100.0f);
|
||||
|
||||
// return immediately if motors are not armed
|
||||
if(!motors.armed()) {
|
||||
if(!motors->armed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
|
@ -13,7 +13,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
|
||||
{
|
||||
// boolean to record if flight mode could be set
|
||||
bool success = false;
|
||||
bool ignore_checks = !motors.armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
|
||||
bool ignore_checks = !motors->armed(); // allow switching to any mode if disarmed. We rely on the arming check to perform
|
||||
|
||||
// return immediately if we are already in the desired mode
|
||||
if (mode == control_mode) {
|
||||
@ -271,7 +271,7 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
|
||||
}
|
||||
|
||||
// smooth throttle transition when switching from manual to automatic flight modes
|
||||
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors.armed() && !ap.land_complete) {
|
||||
if (mode_has_manual_throttle(old_control_mode) && !mode_has_manual_throttle(new_control_mode) && motors->armed() && !ap.land_complete) {
|
||||
// this assumes all manual flight modes use get_pilot_desired_throttle to translate pilot input to output throttle
|
||||
set_accel_throttle_I_from_pilot_throttle();
|
||||
}
|
||||
@ -282,8 +282,8 @@ void Copter::exit_mode(control_mode_t old_control_mode, control_mode_t new_contr
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// firmly reset the flybar passthrough to false when exiting acro mode.
|
||||
if (old_control_mode == ACRO) {
|
||||
attitude_control.use_flybar_passthrough(false, false);
|
||||
motors.set_acro_tail(false);
|
||||
attitude_control->use_flybar_passthrough(false, false);
|
||||
motors->set_acro_tail(false);
|
||||
}
|
||||
|
||||
// if we are changing from a mode that did not use manual throttle,
|
||||
|
@ -23,7 +23,7 @@ void Copter::heli_init()
|
||||
// should be called at 50hz
|
||||
void Copter::check_dynamic_flight(void)
|
||||
{
|
||||
if (!motors.armed() || !motors.rotor_runup_complete() ||
|
||||
if (!motors->armed() || !motors->rotor_runup_complete() ||
|
||||
control_mode == LAND || (control_mode==RTL && rtl_state == RTL_Land) || (control_mode == AUTO && auto_mode == Auto_Land)) {
|
||||
heli_dynamic_flight_counter = 0;
|
||||
heli_flags.dynamic_flight = false;
|
||||
@ -39,7 +39,7 @@ void Copter::check_dynamic_flight(void)
|
||||
moving = (velocity >= HELI_DYNAMIC_FLIGHT_SPEED_MIN);
|
||||
}else{
|
||||
// with no GPS lock base it on throttle and forward lean angle
|
||||
moving = (motors.get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
|
||||
moving = (motors->get_throttle() > 0.8f || ahrs.pitch_sensor < -1500);
|
||||
}
|
||||
|
||||
if (!moving && rangefinder_state.enabled && rangefinder.status() == RangeFinder::RangeFinder_Good) {
|
||||
@ -74,9 +74,9 @@ void Copter::check_dynamic_flight(void)
|
||||
void Copter::update_heli_control_dynamics(void)
|
||||
{
|
||||
// Use Leaky_I if we are not moving fast
|
||||
attitude_control.use_leaky_i(!heli_flags.dynamic_flight);
|
||||
attitude_control->use_leaky_i(!heli_flags.dynamic_flight);
|
||||
|
||||
if (ap.land_complete || (is_zero(motors.get_desired_rotor_speed()))){
|
||||
if (ap.land_complete || (is_zero(motors->get_desired_rotor_speed()))){
|
||||
// if we are landed or there is no rotor power demanded, decrement slew scalar
|
||||
hover_roll_trim_scalar_slew--;
|
||||
} else {
|
||||
@ -86,7 +86,7 @@ void Copter::update_heli_control_dynamics(void)
|
||||
hover_roll_trim_scalar_slew = constrain_int16(hover_roll_trim_scalar_slew, 0, scheduler.get_loop_rate_hz());
|
||||
|
||||
// set hover roll trim scalar, will ramp from 0 to 1 over 1 second after we think helicopter has taken off
|
||||
attitude_control.set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/scheduler.get_loop_rate_hz()));
|
||||
attitude_control->set_hover_roll_trim_scalar((float)(hover_roll_trim_scalar_slew/scheduler.get_loop_rate_hz()));
|
||||
}
|
||||
|
||||
// heli_update_landing_swash - sets swash plate flag so higher minimum is used when landed or landing
|
||||
@ -99,33 +99,33 @@ void Copter::heli_update_landing_swash()
|
||||
case DRIFT:
|
||||
case SPORT:
|
||||
// manual modes always uses full swash range
|
||||
motors.set_collective_for_landing(false);
|
||||
motors->set_collective_for_landing(false);
|
||||
break;
|
||||
|
||||
case LAND:
|
||||
// landing always uses limit swash range
|
||||
motors.set_collective_for_landing(true);
|
||||
motors->set_collective_for_landing(true);
|
||||
break;
|
||||
|
||||
case RTL:
|
||||
if (rtl_state == RTL_Land) {
|
||||
motors.set_collective_for_landing(true);
|
||||
motors->set_collective_for_landing(true);
|
||||
}else{
|
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
}
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
if (auto_mode == Auto_Land) {
|
||||
motors.set_collective_for_landing(true);
|
||||
motors->set_collective_for_landing(true);
|
||||
}else{
|
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
// auto and hold use limited swash when landed
|
||||
motors.set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@ -137,7 +137,7 @@ void Copter::heli_update_rotor_speed_targets()
|
||||
static bool rotor_runup_complete_last = false;
|
||||
|
||||
// get rotor control method
|
||||
uint8_t rsc_control_mode = motors.get_rsc_mode();
|
||||
uint8_t rsc_control_mode = motors->get_rsc_mode();
|
||||
|
||||
float rsc_control_deglitched = rotor_speed_deglitch_filter.apply((float)RC_Channels::rc_channel(CH_8)->get_control_in()) * 0.001f;
|
||||
|
||||
@ -146,10 +146,10 @@ void Copter::heli_update_rotor_speed_targets()
|
||||
// pass through pilot desired rotor speed if control input is higher than 10, creating a deadband at the bottom
|
||||
if (rsc_control_deglitched > 0.01f) {
|
||||
ap.motor_interlock_switch = true;
|
||||
motors.set_desired_rotor_speed(rsc_control_deglitched);
|
||||
motors->set_desired_rotor_speed(rsc_control_deglitched);
|
||||
} else {
|
||||
ap.motor_interlock_switch = false;
|
||||
motors.set_desired_rotor_speed(0.0f);
|
||||
motors->set_desired_rotor_speed(0.0f);
|
||||
}
|
||||
break;
|
||||
case ROTOR_CONTROL_MODE_SPEED_SETPOINT:
|
||||
@ -159,21 +159,21 @@ void Copter::heli_update_rotor_speed_targets()
|
||||
// other than being used to create a crude estimate of rotor speed
|
||||
if (rsc_control_deglitched > 0.0f) {
|
||||
ap.motor_interlock_switch = true;
|
||||
motors.set_desired_rotor_speed(motors.get_rsc_setpoint());
|
||||
motors->set_desired_rotor_speed(motors->get_rsc_setpoint());
|
||||
}else{
|
||||
ap.motor_interlock_switch = false;
|
||||
motors.set_desired_rotor_speed(0.0f);
|
||||
motors->set_desired_rotor_speed(0.0f);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// when rotor_runup_complete changes to true, log event
|
||||
if (!rotor_runup_complete_last && motors.rotor_runup_complete()){
|
||||
if (!rotor_runup_complete_last && motors->rotor_runup_complete()){
|
||||
Log_Write_Event(DATA_ROTOR_RUNUP_COMPLETE);
|
||||
} else if (rotor_runup_complete_last && !motors.rotor_runup_complete()){
|
||||
} else if (rotor_runup_complete_last && !motors->rotor_runup_complete()){
|
||||
Log_Write_Event(DATA_ROTOR_SPEED_BELOW_CRITICAL);
|
||||
}
|
||||
rotor_runup_complete_last = motors.rotor_runup_complete();
|
||||
rotor_runup_complete_last = motors->rotor_runup_complete();
|
||||
}
|
||||
|
||||
#endif // FRAME_CONFIG == HELI_FRAME
|
||||
|
@ -9,9 +9,9 @@
|
||||
bool Copter::heli_acro_init(bool ignore_checks)
|
||||
{
|
||||
// if heli is equipped with a flybar, then tell the attitude controller to pass through controls directly to servos
|
||||
attitude_control.use_flybar_passthrough(motors.has_flybar(), motors.supports_yaw_passthrough());
|
||||
attitude_control->use_flybar_passthrough(motors->has_flybar(), motors->supports_yaw_passthrough());
|
||||
|
||||
motors.set_acro_tail(true);
|
||||
motors->set_acro_tail(true);
|
||||
|
||||
// set stab collective false to use full collective pitch range
|
||||
input_manager.set_use_stab_col(false);
|
||||
@ -33,28 +33,28 @@ void Copter::heli_acro_run()
|
||||
// that the servos move in a realistic fashion while disarmed for operational checks.
|
||||
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
|
||||
|
||||
if(!motors.armed()) {
|
||||
if(!motors->armed()) {
|
||||
heli_flags.init_targets_on_arming=true;
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
}
|
||||
|
||||
if(motors.armed() && heli_flags.init_targets_on_arming) {
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
if (motors.rotor_speed_above_critical()) {
|
||||
if(motors->armed() && heli_flags.init_targets_on_arming) {
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
if (motors->rotor_speed_above_critical()) {
|
||||
heli_flags.init_targets_on_arming=false;
|
||||
}
|
||||
}
|
||||
|
||||
// clear landing flag above zero throttle
|
||||
if (motors.armed() && motors.get_interlock() && motors.rotor_runup_complete() && !ap.throttle_zero) {
|
||||
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
|
||||
set_land_complete(false);
|
||||
}
|
||||
|
||||
if (!motors.has_flybar()){
|
||||
if (!motors->has_flybar()){
|
||||
// convert the input to the desired body frame rate
|
||||
get_pilot_desired_angle_rates(channel_roll->get_control_in(), channel_pitch->get_control_in(), channel_yaw->get_control_in(), target_roll, target_pitch, target_yaw);
|
||||
|
||||
if (motors.supports_yaw_passthrough()) {
|
||||
if (motors->supports_yaw_passthrough()) {
|
||||
// if the tail on a flybar heli has an external gyro then
|
||||
// also use no deadzone for the yaw control and
|
||||
// pass-through the input direct to output.
|
||||
@ -62,7 +62,7 @@ void Copter::heli_acro_run()
|
||||
}
|
||||
|
||||
// run attitude controller
|
||||
attitude_control.input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
||||
attitude_control->input_rate_bf_roll_pitch_yaw(target_roll, target_pitch, target_yaw);
|
||||
}else{
|
||||
/*
|
||||
for fly-bar passthrough use control_in values with no
|
||||
@ -72,7 +72,7 @@ void Copter::heli_acro_run()
|
||||
float pitch_in = channel_pitch->get_control_in_zero_dz();
|
||||
float yaw_in;
|
||||
|
||||
if (motors.supports_yaw_passthrough()) {
|
||||
if (motors->supports_yaw_passthrough()) {
|
||||
// if the tail on a flybar heli has an external gyro then
|
||||
// also use no deadzone for the yaw control and
|
||||
// pass-through the input direct to output.
|
||||
@ -85,14 +85,14 @@ void Copter::heli_acro_run()
|
||||
}
|
||||
|
||||
// run attitude controller
|
||||
attitude_control.passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
|
||||
attitude_control->passthrough_bf_roll_pitch_rate_yaw(roll_in, pitch_in, yaw_in);
|
||||
}
|
||||
|
||||
// get pilot's desired throttle
|
||||
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
||||
|
||||
// output pilot's throttle without angle boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
}
|
||||
|
||||
#endif //HELI_FRAME
|
||||
|
@ -10,7 +10,7 @@ bool Copter::heli_stabilize_init(bool ignore_checks)
|
||||
{
|
||||
// set target altitude to zero for reporting
|
||||
// To-Do: make pos controller aware when it's active/inactive so it can always report the altitude error?
|
||||
pos_control.set_alt_target(0);
|
||||
pos_control->set_alt_target(0);
|
||||
|
||||
// set stab collective true to use stabilize scaled collective pitch range
|
||||
input_manager.set_use_stab_col(true);
|
||||
@ -32,20 +32,20 @@ void Copter::heli_stabilize_run()
|
||||
// that the servos move in a realistic fashion while disarmed for operational checks.
|
||||
// Also, unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move
|
||||
|
||||
if(!motors.armed()) {
|
||||
if(!motors->armed()) {
|
||||
heli_flags.init_targets_on_arming=true;
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
}
|
||||
|
||||
if(motors.armed() && heli_flags.init_targets_on_arming) {
|
||||
attitude_control.set_yaw_target_to_current_heading();
|
||||
if (motors.rotor_speed_above_critical()) {
|
||||
if(motors->armed() && heli_flags.init_targets_on_arming) {
|
||||
attitude_control->set_yaw_target_to_current_heading();
|
||||
if (motors->rotor_speed_above_critical()) {
|
||||
heli_flags.init_targets_on_arming=false;
|
||||
}
|
||||
}
|
||||
|
||||
// clear landing flag above zero throttle
|
||||
if (motors.armed() && motors.get_interlock() && motors.rotor_runup_complete() && !ap.throttle_zero) {
|
||||
if (motors->armed() && motors->get_interlock() && motors->rotor_runup_complete() && !ap.throttle_zero) {
|
||||
set_land_complete(false);
|
||||
}
|
||||
|
||||
@ -63,10 +63,10 @@ void Copter::heli_stabilize_run()
|
||||
pilot_throttle_scaled = input_manager.get_pilot_desired_collective(channel_throttle->get_control_in());
|
||||
|
||||
// call attitude controller
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(target_roll, target_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
|
||||
// output pilot's throttle - note that TradHeli does not used angle-boost
|
||||
attitude_control.set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
attitude_control->set_throttle_out(pilot_throttle_scaled, false, g.throttle_filt);
|
||||
}
|
||||
|
||||
#endif //HELI_FRAME
|
||||
|
@ -40,16 +40,16 @@ void Copter::update_land_detector()
|
||||
// range finder : tend to be problematic at very short distances
|
||||
// input throttle : in slow land the input throttle may be only slightly less than hover
|
||||
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
// if disarmed, always landed.
|
||||
set_land_complete(true);
|
||||
} else if (ap.land_complete) {
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// if rotor speed and collective pitch are high then clear landing flag
|
||||
if (motors.get_throttle() > get_non_takeoff_throttle() && !motors.limit.throttle_lower && motors.rotor_runup_complete()) {
|
||||
if (motors->get_throttle() > get_non_takeoff_throttle() && !motors->limit.throttle_lower && motors->rotor_runup_complete()) {
|
||||
#else
|
||||
// if throttle output is high then clear landing flag
|
||||
if (motors.get_throttle() > get_non_takeoff_throttle()) {
|
||||
if (motors->get_throttle() > get_non_takeoff_throttle()) {
|
||||
#endif
|
||||
set_land_complete(false);
|
||||
}
|
||||
@ -57,10 +57,10 @@ void Copter::update_land_detector()
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// check that collective pitch is on lower limit (should be constrained by LAND_COL_MIN)
|
||||
bool motor_at_lower_limit = motors.limit.throttle_lower;
|
||||
bool motor_at_lower_limit = motors->limit.throttle_lower;
|
||||
#else
|
||||
// check that the average throttle output is near minimum (less than 12.5% hover throttle)
|
||||
bool motor_at_lower_limit = motors.limit.throttle_lower && attitude_control.is_throttle_mix_min();
|
||||
bool motor_at_lower_limit = motors->limit.throttle_lower && attitude_control->is_throttle_mix_min();
|
||||
#endif
|
||||
|
||||
// check that the airframe is not accelerating (not falling or breaking after fast forward flight)
|
||||
@ -110,7 +110,7 @@ void Copter::set_land_complete(bool b)
|
||||
bool disarm_on_land_configured = (g.throttle_behavior & THR_BEHAVE_DISARM_ON_LAND_DETECT) != 0;
|
||||
bool mode_disarms_on_land = mode_allows_arming(control_mode,false) && !mode_has_manual_throttle(control_mode);
|
||||
|
||||
if (ap.land_complete && motors.armed() && disarm_on_land_configured && mode_disarms_on_land) {
|
||||
if (ap.land_complete && motors->armed() && disarm_on_land_configured && mode_disarms_on_land) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
}
|
||||
@ -135,27 +135,27 @@ void Copter::update_throttle_thr_mix()
|
||||
{
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// if disarmed or landed prioritise throttle
|
||||
if(!motors.armed() || ap.land_complete) {
|
||||
attitude_control.set_throttle_mix_min();
|
||||
if(!motors->armed() || ap.land_complete) {
|
||||
attitude_control->set_throttle_mix_min();
|
||||
return;
|
||||
}
|
||||
|
||||
if (mode_has_manual_throttle(control_mode)) {
|
||||
// manual throttle
|
||||
if(channel_throttle->get_control_in() <= 0) {
|
||||
attitude_control.set_throttle_mix_min();
|
||||
attitude_control->set_throttle_mix_min();
|
||||
} else {
|
||||
attitude_control.set_throttle_mix_mid();
|
||||
attitude_control->set_throttle_mix_mid();
|
||||
}
|
||||
} else {
|
||||
// autopilot controlled throttle
|
||||
|
||||
// check for aggressive flight requests - requested roll or pitch angle below 15 degrees
|
||||
const Vector3f angle_target = attitude_control.get_att_target_euler_cd();
|
||||
const Vector3f angle_target = attitude_control->get_att_target_euler_cd();
|
||||
bool large_angle_request = (norm(angle_target.x, angle_target.y) > LAND_CHECK_LARGE_ANGLE_CD);
|
||||
|
||||
// check for large external disturbance - angle error over 30 degrees
|
||||
const float angle_error = attitude_control.get_att_error_angle_deg();
|
||||
const float angle_error = attitude_control->get_att_error_angle_deg();
|
||||
bool large_angle_error = (angle_error > LAND_CHECK_ANGLE_ERROR_DEG);
|
||||
|
||||
// check for large acceleration - falling or high turbulence
|
||||
@ -164,12 +164,12 @@ void Copter::update_throttle_thr_mix()
|
||||
bool accel_moving = (accel_ef.length() > LAND_CHECK_ACCEL_MOVING);
|
||||
|
||||
// check for requested decent
|
||||
bool descent_not_demanded = pos_control.get_desired_velocity().z >= 0.0f;
|
||||
bool descent_not_demanded = pos_control->get_desired_velocity().z >= 0.0f;
|
||||
|
||||
if ( large_angle_request || large_angle_error || accel_moving || descent_not_demanded) {
|
||||
attitude_control.set_throttle_mix_max();
|
||||
attitude_control->set_throttle_mix_max();
|
||||
} else {
|
||||
attitude_control.set_throttle_mix_min();
|
||||
attitude_control->set_throttle_mix_min();
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -38,8 +38,8 @@ void Copter::motor_test_output()
|
||||
// sanity check motor_test_throttle value
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
if (motor_test_throttle_value <= 100) {
|
||||
int16_t pwm_min = motors.get_pwm_output_min();
|
||||
int16_t pwm_max = motors.get_pwm_output_max();
|
||||
int16_t pwm_min = motors->get_pwm_output_min();
|
||||
int16_t pwm_max = motors->get_pwm_output_max();
|
||||
pwm = pwm_min + (pwm_max - pwm_min) * (float)motor_test_throttle_value/100.0f;
|
||||
}
|
||||
#endif
|
||||
@ -61,7 +61,7 @@ void Copter::motor_test_output()
|
||||
// sanity check throttle values
|
||||
if (pwm >= MOTOR_TEST_PWM_MIN && pwm <= MOTOR_TEST_PWM_MAX ) {
|
||||
// turn on motor to specified pwm vlaue
|
||||
motors.output_test(motor_test_seq, pwm);
|
||||
motors->output_test(motor_test_seq, pwm);
|
||||
} else {
|
||||
motor_test_stop();
|
||||
}
|
||||
@ -112,10 +112,10 @@ uint8_t Copter::mavlink_motor_test_start(mavlink_channel_t chan, uint8_t motor_s
|
||||
ap.motor_test = true;
|
||||
|
||||
// enable and arm motors
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
init_rc_out();
|
||||
enable_motor_output();
|
||||
motors.armed(true);
|
||||
motors->armed(true);
|
||||
}
|
||||
|
||||
// disable throttle, battery and gps failsafe
|
||||
@ -153,7 +153,7 @@ void Copter::motor_test_stop()
|
||||
ap.motor_test = false;
|
||||
|
||||
// disarm motors
|
||||
motors.armed(false);
|
||||
motors->armed(false);
|
||||
|
||||
// reset timeout
|
||||
motor_test_start_ms = 0;
|
||||
|
@ -30,7 +30,7 @@ void Copter::arm_motors_check()
|
||||
}
|
||||
|
||||
// arm the motors and configure for flight
|
||||
if (arming_counter == ARM_DELAY && !motors.armed()) {
|
||||
if (arming_counter == ARM_DELAY && !motors->armed()) {
|
||||
// reset arming counter if arming fail
|
||||
if (!init_arm_motors(false)) {
|
||||
arming_counter = 0;
|
||||
@ -38,7 +38,7 @@ void Copter::arm_motors_check()
|
||||
}
|
||||
|
||||
// arm the motors and configure for flight
|
||||
if (arming_counter == AUTO_TRIM_DELAY && motors.armed() && control_mode == STABILIZE) {
|
||||
if (arming_counter == AUTO_TRIM_DELAY && motors->armed() && control_mode == STABILIZE) {
|
||||
auto_trim_counter = 250;
|
||||
// ensure auto-disarm doesn't trigger immediately
|
||||
auto_disarm_begin = millis();
|
||||
@ -57,7 +57,7 @@ void Copter::arm_motors_check()
|
||||
}
|
||||
|
||||
// disarm the motors
|
||||
if (arming_counter == DISARM_DELAY && motors.armed()) {
|
||||
if (arming_counter == DISARM_DELAY && motors->armed()) {
|
||||
init_disarm_motors();
|
||||
}
|
||||
|
||||
@ -75,21 +75,21 @@ void Copter::auto_disarm_check()
|
||||
|
||||
// exit immediately if we are already disarmed, or if auto
|
||||
// disarming is disabled
|
||||
if (!motors.armed() || disarm_delay_ms == 0 || control_mode == THROW) {
|
||||
if (!motors->armed() || disarm_delay_ms == 0 || control_mode == THROW) {
|
||||
auto_disarm_begin = tnow_ms;
|
||||
return;
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// if the rotor is still spinning, don't initiate auto disarm
|
||||
if (motors.rotor_speed_above_critical()) {
|
||||
if (motors->rotor_speed_above_critical()) {
|
||||
auto_disarm_begin = tnow_ms;
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
// always allow auto disarm if using interlock switch or motors are Emergency Stopped
|
||||
if ((ap.using_interlock && !motors.get_interlock()) || ap.motor_emergency_stop) {
|
||||
if ((ap.using_interlock && !motors->get_interlock()) || ap.motor_emergency_stop) {
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
// use a shorter delay if using throttle interlock switch or Emergency Stop, because it is less
|
||||
// obvious the copter is armed as the motors will not be spinning
|
||||
@ -131,7 +131,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
|
||||
in_arm_motors = true;
|
||||
|
||||
// return true if already armed
|
||||
if (motors.armed()) {
|
||||
if (motors->armed()) {
|
||||
in_arm_motors = false;
|
||||
return true;
|
||||
}
|
||||
@ -192,7 +192,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
|
||||
enable_motor_output();
|
||||
|
||||
// finally actually arm the motors
|
||||
motors.armed(true);
|
||||
motors->armed(true);
|
||||
|
||||
// log arming to dataflash
|
||||
Log_Write_Event(DATA_ARMED);
|
||||
@ -223,7 +223,7 @@ bool Copter::init_arm_motors(bool arming_from_gcs)
|
||||
void Copter::init_disarm_motors()
|
||||
{
|
||||
// return immediately if we are already disarmed
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -254,7 +254,7 @@ void Copter::init_disarm_motors()
|
||||
Log_Write_Event(DATA_DISARMED);
|
||||
|
||||
// send disarm command to motors
|
||||
motors.armed(false);
|
||||
motors->armed(false);
|
||||
|
||||
// reset the mission
|
||||
mission.reset();
|
||||
@ -286,7 +286,7 @@ void Copter::motors_output()
|
||||
#endif
|
||||
|
||||
// Update arming delay state
|
||||
if (ap.in_arming_delay && (!motors.armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
|
||||
if (ap.in_arming_delay && (!motors->armed() || millis()-arm_time_ms > ARMING_DELAY_SEC*1.0e3f || control_mode == THROW)) {
|
||||
ap.in_arming_delay = false;
|
||||
}
|
||||
|
||||
@ -294,17 +294,17 @@ void Copter::motors_output()
|
||||
if (ap.motor_test) {
|
||||
motor_test_output();
|
||||
} else {
|
||||
bool interlock = motors.armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
|
||||
if (!motors.get_interlock() && interlock) {
|
||||
motors.set_interlock(true);
|
||||
bool interlock = motors->armed() && !ap.in_arming_delay && (!ap.using_interlock || ap.motor_interlock_switch) && !ap.motor_emergency_stop;
|
||||
if (!motors->get_interlock() && interlock) {
|
||||
motors->set_interlock(true);
|
||||
Log_Write_Event(DATA_MOTORS_INTERLOCK_ENABLED);
|
||||
} else if (motors.get_interlock() && !interlock) {
|
||||
motors.set_interlock(false);
|
||||
} else if (motors->get_interlock() && !interlock) {
|
||||
motors->set_interlock(false);
|
||||
Log_Write_Event(DATA_MOTORS_INTERLOCK_DISABLED);
|
||||
}
|
||||
|
||||
// send output signals to motors
|
||||
motors.output();
|
||||
motors->output();
|
||||
}
|
||||
}
|
||||
|
||||
@ -319,7 +319,7 @@ void Copter::lost_vehicle_check()
|
||||
}
|
||||
|
||||
// ensure throttle is down, motors not armed, pitch and roll rc at max. Note: rc1=roll rc2=pitch
|
||||
if (ap.throttle_zero && !motors.armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
|
||||
if (ap.throttle_zero && !motors->armed() && (channel_roll->get_control_in() > 4000) && (channel_pitch->get_control_in() > 4000)) {
|
||||
if (soundalarm_counter >= LOST_VEHICLE_DELAY) {
|
||||
if (AP_Notify::flags.vehicle_lost == false) {
|
||||
AP_Notify::flags.vehicle_lost = true;
|
||||
|
@ -27,17 +27,17 @@ void Copter::calc_wp_distance()
|
||||
switch (control_mode) {
|
||||
case LOITER:
|
||||
case CIRCLE:
|
||||
wp_distance = wp_nav.get_loiter_distance_to_target();
|
||||
wp_distance = wp_nav->get_loiter_distance_to_target();
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
case RTL:
|
||||
wp_distance = wp_nav.get_wp_distance_to_destination();
|
||||
wp_distance = wp_nav->get_wp_distance_to_destination();
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
if (guided_mode == Guided_WP) {
|
||||
wp_distance = wp_nav.get_wp_distance_to_destination();
|
||||
wp_distance = wp_nav->get_wp_distance_to_destination();
|
||||
break;
|
||||
}
|
||||
// no break
|
||||
@ -54,17 +54,17 @@ void Copter::calc_wp_bearing()
|
||||
switch (control_mode) {
|
||||
case LOITER:
|
||||
case CIRCLE:
|
||||
wp_bearing = wp_nav.get_loiter_bearing_to_target();
|
||||
wp_bearing = wp_nav->get_loiter_bearing_to_target();
|
||||
break;
|
||||
|
||||
case AUTO:
|
||||
case RTL:
|
||||
wp_bearing = wp_nav.get_wp_bearing_to_destination();
|
||||
wp_bearing = wp_nav->get_wp_bearing_to_destination();
|
||||
break;
|
||||
|
||||
case GUIDED:
|
||||
if (guided_mode == Guided_WP) {
|
||||
wp_bearing = wp_nav.get_wp_bearing_to_destination();
|
||||
wp_bearing = wp_nav->get_wp_bearing_to_destination();
|
||||
break;
|
||||
}
|
||||
// no break
|
||||
|
@ -48,14 +48,11 @@ void Copter::init_rc_in()
|
||||
// init_rc_out -- initialise motors and check if pilot wants to perform ESC calibration
|
||||
void Copter::init_rc_out()
|
||||
{
|
||||
// default frame class to match firmware if possible
|
||||
set_default_frame_class();
|
||||
|
||||
motors.set_update_rate(g.rc_speed);
|
||||
motors.set_loop_rate(scheduler.get_loop_rate_hz());
|
||||
motors.init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
|
||||
motors->set_update_rate(g.rc_speed);
|
||||
motors->set_loop_rate(scheduler.get_loop_rate_hz());
|
||||
motors->init((AP_Motors::motor_frame_class)g2.frame_class.get(), (AP_Motors::motor_frame_type)g.frame_type.get());
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
motors.set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
|
||||
motors->set_throttle_range(channel_throttle->get_radio_min(), channel_throttle->get_radio_max());
|
||||
#endif
|
||||
|
||||
for(uint8_t i = 0; i < 5; i++) {
|
||||
@ -83,7 +80,7 @@ void Copter::init_rc_out()
|
||||
/*
|
||||
setup a default safety ignore mask, so that servo gimbals can be active while safety is on
|
||||
*/
|
||||
uint16_t safety_ignore_mask = (~copter.motors.get_motor_mask()) & 0x3FFF;
|
||||
uint16_t safety_ignore_mask = (~copter.motors->get_motor_mask()) & 0x3FFF;
|
||||
BoardConfig.set_default_safety_ignore_mask(safety_ignore_mask);
|
||||
#endif
|
||||
}
|
||||
@ -93,8 +90,8 @@ void Copter::init_rc_out()
|
||||
void Copter::enable_motor_output()
|
||||
{
|
||||
// enable motors
|
||||
motors.enable();
|
||||
motors.output_min();
|
||||
motors->enable();
|
||||
motors->output_min();
|
||||
}
|
||||
|
||||
void Copter::read_radio()
|
||||
@ -126,7 +123,7 @@ void Copter::read_radio()
|
||||
uint32_t elapsed = tnow_ms - last_radio_update_ms;
|
||||
// turn on throttle failsafe if no update from the RC Radio for 500ms or 2000ms if we are using RC_OVERRIDE
|
||||
if (((!failsafe.rc_override_active && (elapsed >= FS_RADIO_TIMEOUT_MS)) || (failsafe.rc_override_active && (elapsed >= FS_RADIO_RC_OVERRIDE_TIMEOUT_MS))) &&
|
||||
(g.failsafe_throttle && (ap.rc_receiver_present||motors.armed()) && !failsafe.radio)) {
|
||||
(g.failsafe_throttle && (ap.rc_receiver_present||motors->armed()) && !failsafe.radio)) {
|
||||
Log_Write_Error(ERROR_SUBSYSTEM_RADIO, ERROR_CODE_RADIO_LATE_FRAME);
|
||||
set_failsafe_radio(true);
|
||||
}
|
||||
@ -146,7 +143,7 @@ void Copter::set_throttle_and_failsafe(uint16_t throttle_pwm)
|
||||
if (throttle_pwm < (uint16_t)g.failsafe_throttle_value) {
|
||||
|
||||
// if we are already in failsafe or motors not armed pass through throttle and exit
|
||||
if (failsafe.radio || !(ap.rc_receiver_present || motors.armed())) {
|
||||
if (failsafe.radio || !(ap.rc_receiver_present || motors->armed())) {
|
||||
channel_throttle->set_pwm(throttle_pwm);
|
||||
return;
|
||||
}
|
||||
@ -188,7 +185,7 @@ void Copter::set_throttle_zero_flag(int16_t throttle_control)
|
||||
// if not using throttle interlock and non-zero throttle and not E-stopped,
|
||||
// or using motor interlock and it's enabled, then motors are running,
|
||||
// and we are flying. Immediately set as non-zero
|
||||
if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors.get_interlock())) {
|
||||
if ((!ap.using_interlock && (throttle_control > 0) && !ap.motor_emergency_stop) || (ap.using_interlock && motors->get_interlock())) {
|
||||
last_nonzero_throttle_ms = tnow_ms;
|
||||
ap.throttle_zero = false;
|
||||
} else if (tnow_ms - last_nonzero_throttle_ms > THROTTLE_ZERO_DEBOUNCE_TIME_MS) {
|
||||
@ -199,5 +196,5 @@ void Copter::set_throttle_zero_flag(int16_t throttle_control)
|
||||
// pass pilot's inputs to motors library (used to allow wiggling servos while disarmed on heli, single, coax copters)
|
||||
void Copter::radio_passthrough_to_motors()
|
||||
{
|
||||
motors.set_radio_passthrough(channel_roll->get_control_in()/1000.0f, channel_pitch->get_control_in()/1000.0f, channel_throttle->get_control_in()/1000.0f, channel_yaw->get_control_in()/1000.0f);
|
||||
motors->set_radio_passthrough(channel_roll->get_control_in()/1000.0f, channel_pitch->get_control_in()/1000.0f, channel_throttle->get_control_in()/1000.0f, channel_yaw->get_control_in()/1000.0f);
|
||||
}
|
||||
|
@ -21,7 +21,7 @@ void Copter::read_barometer(void)
|
||||
baro_alt = barometer.get_altitude() * 100.0f;
|
||||
baro_climbrate = barometer.get_climb_rate() * 100.0f;
|
||||
|
||||
motors.set_air_density_ratio(barometer.get_air_density_ratio());
|
||||
motors->set_air_density_ratio(barometer.get_air_density_ratio());
|
||||
}
|
||||
|
||||
void Copter::init_rangefinder(void)
|
||||
@ -69,7 +69,7 @@ void Copter::read_rangefinder(void)
|
||||
}
|
||||
|
||||
// send rangefinder altitude and health to waypoint navigation library
|
||||
wp_nav.set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
||||
wp_nav->set_rangefinder_alt(rangefinder_state.enabled, rangefinder_state.alt_healthy, rangefinder_state.alt_cm_filt.get());
|
||||
|
||||
#else
|
||||
rangefinder_state.enabled = false;
|
||||
@ -165,10 +165,10 @@ void Copter::read_battery(void)
|
||||
|
||||
// update motors with voltage and current
|
||||
if (battery.get_type() != AP_BattMonitor::BattMonitor_TYPE_NONE) {
|
||||
motors.set_voltage(battery.voltage());
|
||||
motors->set_voltage(battery.voltage());
|
||||
}
|
||||
if (battery.has_current()) {
|
||||
motors.set_current(battery.current_amps());
|
||||
motors->set_current(battery.current_amps());
|
||||
}
|
||||
|
||||
// check for low voltage or current if the low voltage check hasn't already been triggered
|
||||
@ -203,7 +203,7 @@ void Copter::compass_cal_update()
|
||||
compass.cancel_calibration_all();
|
||||
}
|
||||
} else {
|
||||
bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors.armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900;
|
||||
bool stick_gesture_detected = compass_cal_stick_gesture_begin != 0 && !motors->armed() && channel_yaw->get_control_in() > 4000 && channel_throttle->get_control_in() > 900;
|
||||
uint32_t tnow = millis();
|
||||
|
||||
if (!stick_gesture_detected) {
|
||||
|
@ -221,7 +221,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
|
||||
max_channels = AP_MOTORS_MAX_NUM_MOTORS;
|
||||
|
||||
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
|
||||
motors.armed(true);
|
||||
motors->armed(true);
|
||||
|
||||
|
||||
cliSerial->println("Outputs armed");
|
||||
@ -238,7 +238,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
|
||||
for (unsigned i = 0; i < max_channels; i++) {
|
||||
|
||||
if (set_mask & 1<<i) {
|
||||
motors.output_test(i, pwm_high);
|
||||
motors->output_test(i, pwm_high);
|
||||
}
|
||||
}
|
||||
c = cliSerial->read();
|
||||
@ -265,7 +265,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
|
||||
/* set disarmed PWM */
|
||||
for (unsigned i = 0; i < max_channels; i++) {
|
||||
if (set_mask & 1<<i) {
|
||||
motors.output_test(i, pwm_low);
|
||||
motors->output_test(i, pwm_low);
|
||||
}
|
||||
}
|
||||
c = cliSerial->read();
|
||||
@ -284,7 +284,7 @@ int8_t Copter::esc_calib(uint8_t argc,const Menu::arg *argv)
|
||||
}
|
||||
|
||||
/* disarm */
|
||||
motors.armed(false);
|
||||
motors->armed(false);
|
||||
|
||||
cliSerial->println("Outputs disarmed");
|
||||
|
||||
|
@ -247,7 +247,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
||||
if (ch_flag == AUX_SWITCH_HIGH) {
|
||||
|
||||
// do not allow saving new waypoints while we're in auto or disarmed
|
||||
if (control_mode == AUTO || !motors.armed()) {
|
||||
if (control_mode == AUTO || !motors->armed()) {
|
||||
return;
|
||||
}
|
||||
|
||||
@ -364,7 +364,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
||||
#if SPRAYER == ENABLED
|
||||
sprayer.run(ch_flag == AUX_SWITCH_HIGH);
|
||||
// if we are disarmed the pilot must want to test the pump
|
||||
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors.armed());
|
||||
sprayer.test_pump((ch_flag == AUX_SWITCH_HIGH) && !motors->armed());
|
||||
#endif
|
||||
break;
|
||||
|
||||
@ -452,12 +452,12 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
|
||||
|
||||
case AUXSW_ATTCON_FEEDFWD:
|
||||
// enable or disable feed forward
|
||||
attitude_control.bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
|
||||
attitude_control->bf_feedforward(ch_flag == AUX_SWITCH_HIGH);
|
||||
break;
|
||||
|
||||
case AUXSW_ATTCON_ACCEL_LIM:
|
||||
// enable or disable accel limiting by restoring defaults
|
||||
attitude_control.accel_limiting(ch_flag == AUX_SWITCH_HIGH);
|
||||
attitude_control->accel_limiting(ch_flag == AUX_SWITCH_HIGH);
|
||||
break;
|
||||
|
||||
case AUXSW_RETRACT_MOUNT:
|
||||
|
@ -56,9 +56,9 @@ void Copter::run_cli(AP_HAL::UARTDriver *port)
|
||||
failsafe_disable();
|
||||
|
||||
// cut the engines
|
||||
if(motors.armed()) {
|
||||
motors.armed(false);
|
||||
motors.output();
|
||||
if(motors->armed()) {
|
||||
motors->armed(false);
|
||||
motors->output();
|
||||
}
|
||||
|
||||
while (1) {
|
||||
@ -177,10 +177,17 @@ void Copter::init_ardupilot()
|
||||
#endif
|
||||
|
||||
init_rc_in(); // sets up rc channels from radio
|
||||
|
||||
// default frame class to match firmware if possible
|
||||
set_default_frame_class();
|
||||
|
||||
// allocate the motors class
|
||||
allocate_motors();
|
||||
|
||||
init_rc_out(); // sets up motors and output to escs
|
||||
|
||||
// initialise which outputs Servo and Relay events can use
|
||||
ServoRelayEvents.set_channel_mask(~motors.get_motor_mask());
|
||||
ServoRelayEvents.set_channel_mask(~motors->get_motor_mask());
|
||||
|
||||
relay.init();
|
||||
|
||||
@ -208,14 +215,14 @@ void Copter::init_ardupilot()
|
||||
Location_Class::set_ahrs(&ahrs);
|
||||
#if AP_TERRAIN_AVAILABLE && AC_TERRAIN
|
||||
Location_Class::set_terrain(&terrain);
|
||||
wp_nav.set_terrain(&terrain);
|
||||
wp_nav->set_terrain(&terrain);
|
||||
#endif
|
||||
#if AC_AVOID_ENABLED == ENABLED
|
||||
wp_nav.set_avoidance(&avoid);
|
||||
wp_nav->set_avoidance(&avoid);
|
||||
#endif
|
||||
|
||||
attitude_control.parameter_sanity_check();
|
||||
pos_control.set_dt(MAIN_LOOP_SECONDS);
|
||||
attitude_control->parameter_sanity_check();
|
||||
pos_control->set_dt(MAIN_LOOP_SECONDS);
|
||||
|
||||
// init the optical flow sensor
|
||||
init_optflow();
|
||||
@ -362,7 +369,7 @@ bool Copter::ekf_position_ok()
|
||||
nav_filter_status filt_status = inertial_nav.get_filter_status();
|
||||
|
||||
// if disarmed we accept a predicted horizontal position
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
return ((filt_status.flags.horiz_pos_abs || filt_status.flags.pred_horiz_pos_abs));
|
||||
} else {
|
||||
// once armed we require a good absolute position and EKF must not be in const_pos_mode
|
||||
@ -385,7 +392,7 @@ bool Copter::optflow_position_ok()
|
||||
nav_filter_status filt_status = inertial_nav.get_filter_status();
|
||||
|
||||
// if disarmed we accept a predicted horizontal relative position
|
||||
if (!motors.armed()) {
|
||||
if (!motors->armed()) {
|
||||
return (filt_status.flags.pred_horiz_pos_rel);
|
||||
} else {
|
||||
return (filt_status.flags.horiz_pos_rel && !filt_status.flags.const_pos_mode);
|
||||
@ -399,7 +406,7 @@ void Copter::update_auto_armed()
|
||||
// disarm checks
|
||||
if(ap.auto_armed){
|
||||
// if motors are disarmed, auto_armed should also be false
|
||||
if(!motors.armed()) {
|
||||
if(!motors->armed()) {
|
||||
set_auto_armed(false);
|
||||
return;
|
||||
}
|
||||
@ -410,7 +417,7 @@ void Copter::update_auto_armed()
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// if helicopters are on the ground, and the motor is switched off, auto-armed should be false
|
||||
// so that rotor runup is checked again before attempting to take-off
|
||||
if(ap.land_complete && !motors.rotor_runup_complete()) {
|
||||
if(ap.land_complete && !motors->rotor_runup_complete()) {
|
||||
set_auto_armed(false);
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
@ -419,13 +426,13 @@ void Copter::update_auto_armed()
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// for tradheli if motors are armed and throttle is above zero and the motor is started, auto_armed should be true
|
||||
if(motors.armed() && !ap.throttle_zero && motors.rotor_runup_complete()) {
|
||||
if(motors->armed() && !ap.throttle_zero && motors->rotor_runup_complete()) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#else
|
||||
// if motors are armed and throttle is above zero auto_armed should be true
|
||||
// if motors are armed and we are in throw mode, then auto_ermed should be true
|
||||
if(motors.armed() && (!ap.throttle_zero || control_mode == THROW)) {
|
||||
if(motors->armed() && (!ap.throttle_zero || control_mode == THROW)) {
|
||||
set_auto_armed(true);
|
||||
}
|
||||
#endif // HELI_FRAME
|
||||
@ -452,7 +459,7 @@ bool Copter::should_log(uint32_t mask)
|
||||
if (!(mask & g.log_bitmask) || in_mavlink_delay) {
|
||||
return false;
|
||||
}
|
||||
bool ret = motors.armed() || DataFlash.log_while_disarmed();
|
||||
bool ret = motors->armed() || DataFlash.log_while_disarmed();
|
||||
if (ret && !DataFlash.logging_started() && !in_log_download) {
|
||||
start_logging();
|
||||
}
|
||||
@ -465,30 +472,8 @@ bool Copter::should_log(uint32_t mask)
|
||||
// default frame_class to match firmware if possible
|
||||
void Copter::set_default_frame_class()
|
||||
{
|
||||
switch (FRAME_CONFIG) {
|
||||
case QUAD_FRAME:
|
||||
case HEXA_FRAME:
|
||||
case OCTA_FRAME:
|
||||
case OCTA_QUAD_FRAME:
|
||||
case Y6_FRAME:
|
||||
// reset frame_class to undefined if firmware does not match
|
||||
// Note: this assumes that Y6 is the highest numbered frame to be supported by the AP_Motors_Matrix class
|
||||
if (g2.frame_class > AP_Motors::MOTOR_FRAME_Y6) {
|
||||
g2.frame_class = AP_Motors::MOTOR_FRAME_UNDEFINED;
|
||||
}
|
||||
break;
|
||||
case TRI_FRAME:
|
||||
g2.frame_class = AP_Motors::MOTOR_FRAME_TRI;
|
||||
break;
|
||||
case HELI_FRAME:
|
||||
g2.frame_class = AP_Motors::MOTOR_FRAME_HELI;
|
||||
break;
|
||||
case SINGLE_FRAME:
|
||||
g2.frame_class = AP_Motors::MOTOR_FRAME_SINGLE;
|
||||
break;
|
||||
case COAX_FRAME:
|
||||
g2.frame_class = AP_Motors::MOTOR_FRAME_COAX;
|
||||
break;
|
||||
if (FRAME_CONFIG == HELI_FRAME) {
|
||||
g2.frame_class.set(AP_Motors::MOTOR_FRAME_HELI);
|
||||
}
|
||||
}
|
||||
|
||||
@ -545,3 +530,64 @@ const char* Copter::get_frame_string()
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
allocate the motors class
|
||||
*/
|
||||
void Copter::allocate_motors(void)
|
||||
{
|
||||
switch ((AP_Motors::motor_frame_class)g2.frame_class.get()) {
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
case AP_Motors::MOTOR_FRAME_QUAD:
|
||||
case AP_Motors::MOTOR_FRAME_HEXA:
|
||||
case AP_Motors::MOTOR_FRAME_Y6:
|
||||
case AP_Motors::MOTOR_FRAME_OCTA:
|
||||
case AP_Motors::MOTOR_FRAME_OCTAQUAD:
|
||||
motors = new AP_MotorsMatrix(MAIN_LOOP_RATE);
|
||||
break;
|
||||
case AP_Motors::MOTOR_FRAME_TRI:
|
||||
motors = new AP_MotorsTri(MAIN_LOOP_RATE);
|
||||
break;
|
||||
case AP_Motors::MOTOR_FRAME_SINGLE:
|
||||
motors = new AP_MotorsSingle(MAIN_LOOP_RATE);
|
||||
break;
|
||||
case AP_Motors::MOTOR_FRAME_COAX:
|
||||
motors = new AP_MotorsCoax(MAIN_LOOP_RATE);
|
||||
break;
|
||||
#else // FRAME_CONFIG == HELI_FRAME
|
||||
case AP_Motors::MOTOR_FRAME_HELI:
|
||||
motors = new AP_MotorsHeli_Single(MAIN_LOOP_RATE);
|
||||
break;
|
||||
#endif
|
||||
default:
|
||||
break;
|
||||
}
|
||||
if (motors == nullptr) {
|
||||
AP_HAL::panic("Unable to allocate FRAME_CLASS=%u", (unsigned)g2.frame_class.get());
|
||||
}
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
attitude_control = new AC_AttitudeControl_Multi(ahrs, aparm, *motors, MAIN_LOOP_SECONDS);
|
||||
#else
|
||||
attitude_control = new AC_AttitudeControl_Heli(ahrs, aparm, *motors, MAIN_LOOP_SECONDS);
|
||||
#endif
|
||||
if (attitude_control == nullptr) {
|
||||
AP_HAL::panic("Unable to allocate AttitudeControl");
|
||||
}
|
||||
|
||||
pos_control = new AC_PosControl(ahrs, inertial_nav, *motors, *attitude_control,
|
||||
g.p_alt_hold, g.p_vel_z, g.pid_accel_z,
|
||||
g.p_pos_xy, g.pi_vel_xy);
|
||||
if (pos_control == nullptr) {
|
||||
AP_HAL::panic("Unable to allocate PosControl");
|
||||
}
|
||||
|
||||
wp_nav = new AC_WPNav(inertial_nav, ahrs, *pos_control, *attitude_control);
|
||||
if (wp_nav == nullptr) {
|
||||
AP_HAL::panic("Unable to allocate WPNav");
|
||||
}
|
||||
|
||||
circle_nav = new AC_Circle(inertial_nav, ahrs, *pos_control);
|
||||
if (wp_nav == nullptr) {
|
||||
AP_HAL::panic("Unable to allocate CircleNav");
|
||||
}
|
||||
}
|
||||
|
@ -25,11 +25,11 @@ bool Copter::current_mode_has_user_takeoff(bool must_navigate)
|
||||
// initiate user takeoff - called when MAVLink TAKEOFF command is received
|
||||
bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||
{
|
||||
if (motors.armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
|
||||
if (motors->armed() && ap.land_complete && current_mode_has_user_takeoff(must_navigate) && takeoff_alt_cm > current_loc.alt) {
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
// Helicopters should return false if MAVlink takeoff command is received while the rotor is not spinning
|
||||
if (!motors.rotor_runup_complete()) {
|
||||
if (!motors->rotor_runup_complete()) {
|
||||
return false;
|
||||
}
|
||||
#endif
|
||||
@ -59,7 +59,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate)
|
||||
void Copter::takeoff_timer_start(float alt_cm)
|
||||
{
|
||||
// calculate climb rate
|
||||
float speed = MIN(wp_nav.get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
|
||||
float speed = MIN(wp_nav->get_speed_up(), MAX(g.pilot_velocity_z_max*2.0f/3.0f, g.pilot_velocity_z_max-50.0f));
|
||||
|
||||
// sanity check speed and target
|
||||
if (takeoff_state.running || speed <= 0.0f || alt_cm <= 0.0f) {
|
||||
@ -147,7 +147,7 @@ void Copter::auto_takeoff_set_start_alt(void)
|
||||
// start with our current altitude
|
||||
auto_takeoff_no_nav_alt_cm = inertial_nav.get_altitude();
|
||||
|
||||
if (!motors.armed() || !ap.auto_armed || !motors.get_interlock() || ap.land_complete) {
|
||||
if (!motors->armed() || !ap.auto_armed || !motors->get_interlock() || ap.land_complete) {
|
||||
// we are not flying, add the wp_navalt_min
|
||||
auto_takeoff_no_nav_alt_cm += g2.wp_navalt_min * 100;
|
||||
}
|
||||
@ -171,12 +171,12 @@ void Copter::auto_takeoff_attitude_run(float target_yaw_rate)
|
||||
hover_roll_trim_scalar_slew = 0;
|
||||
#endif
|
||||
// tell the position controller that we have limited roll/pitch demand to prevent integrator buildup
|
||||
pos_control.set_limit_accel_xy();
|
||||
pos_control->set_limit_accel_xy();
|
||||
} else {
|
||||
nav_roll = wp_nav.get_roll();
|
||||
nav_pitch = wp_nav.get_pitch();
|
||||
nav_roll = wp_nav->get_roll();
|
||||
nav_pitch = wp_nav->get_pitch();
|
||||
}
|
||||
|
||||
// roll & pitch from waypoint controller, yaw rate from pilot
|
||||
attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(nav_roll, nav_pitch, target_yaw_rate, get_smoothing_gain());
|
||||
}
|
||||
|
@ -29,36 +29,36 @@ void Copter::tuning() {
|
||||
|
||||
// Roll, Pitch tuning
|
||||
case TUNING_STABILIZE_ROLL_PITCH_KP:
|
||||
attitude_control.get_angle_roll_p().kP(tuning_value);
|
||||
attitude_control.get_angle_pitch_p().kP(tuning_value);
|
||||
attitude_control->get_angle_roll_p().kP(tuning_value);
|
||||
attitude_control->get_angle_pitch_p().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_PITCH_KP:
|
||||
attitude_control.get_rate_roll_pid().kP(tuning_value);
|
||||
attitude_control.get_rate_pitch_pid().kP(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kP(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_PITCH_KI:
|
||||
attitude_control.get_rate_roll_pid().kI(tuning_value);
|
||||
attitude_control.get_rate_pitch_pid().kI(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kI(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kI(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_PITCH_KD:
|
||||
attitude_control.get_rate_roll_pid().kD(tuning_value);
|
||||
attitude_control.get_rate_pitch_pid().kD(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kD(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kD(tuning_value);
|
||||
break;
|
||||
|
||||
// Yaw tuning
|
||||
case TUNING_STABILIZE_YAW_KP:
|
||||
attitude_control.get_angle_yaw_p().kP(tuning_value);
|
||||
attitude_control->get_angle_yaw_p().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_YAW_RATE_KP:
|
||||
attitude_control.get_rate_yaw_pid().kP(tuning_value);
|
||||
attitude_control->get_rate_yaw_pid().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_YAW_RATE_KD:
|
||||
attitude_control.get_rate_yaw_pid().kD(tuning_value);
|
||||
attitude_control->get_rate_yaw_pid().kD(tuning_value);
|
||||
break;
|
||||
|
||||
// Altitude and throttle tuning
|
||||
@ -97,7 +97,7 @@ void Copter::tuning() {
|
||||
|
||||
case TUNING_WP_SPEED:
|
||||
// set waypoint navigation horizontal speed to 0 ~ 1000 cm/s
|
||||
wp_nav.set_speed_xy(control_in);
|
||||
wp_nav->set_speed_xy(control_in);
|
||||
break;
|
||||
|
||||
// Acro roll pitch gain
|
||||
@ -112,19 +112,19 @@ void Copter::tuning() {
|
||||
|
||||
#if FRAME_CONFIG == HELI_FRAME
|
||||
case TUNING_HELI_EXTERNAL_GYRO:
|
||||
motors.ext_gyro_gain((float)control_in / 1000.0f);
|
||||
motors->ext_gyro_gain((float)control_in / 1000.0f);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_FF:
|
||||
attitude_control.get_heli_rate_pitch_pid().ff(tuning_value);
|
||||
attitude_control->get_heli_rate_pitch_pid().ff(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_FF:
|
||||
attitude_control.get_heli_rate_roll_pid().ff(tuning_value);
|
||||
attitude_control->get_heli_rate_roll_pid().ff(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_YAW_FF:
|
||||
attitude_control.get_heli_rate_yaw_pid().ff(tuning_value);
|
||||
attitude_control->get_heli_rate_yaw_pid().ff(tuning_value);
|
||||
break;
|
||||
#endif
|
||||
|
||||
@ -135,7 +135,7 @@ void Copter::tuning() {
|
||||
|
||||
case TUNING_CIRCLE_RATE:
|
||||
// set circle rate up to approximately 45 deg/sec in either direction
|
||||
circle_nav.set_rate((float)control_in/25.0f-20.0f);
|
||||
circle_nav->set_rate((float)control_in/25.0f-20.0f);
|
||||
break;
|
||||
|
||||
case TUNING_RANGEFINDER_GAIN:
|
||||
@ -180,37 +180,37 @@ void Copter::tuning() {
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_KP:
|
||||
attitude_control.get_rate_pitch_pid().kP(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_KI:
|
||||
attitude_control.get_rate_pitch_pid().kI(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kI(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_PITCH_KD:
|
||||
attitude_control.get_rate_pitch_pid().kD(tuning_value);
|
||||
attitude_control->get_rate_pitch_pid().kD(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_KP:
|
||||
attitude_control.get_rate_roll_pid().kP(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kP(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_KI:
|
||||
attitude_control.get_rate_roll_pid().kI(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kI(tuning_value);
|
||||
break;
|
||||
|
||||
case TUNING_RATE_ROLL_KD:
|
||||
attitude_control.get_rate_roll_pid().kD(tuning_value);
|
||||
attitude_control->get_rate_roll_pid().kD(tuning_value);
|
||||
break;
|
||||
|
||||
#if FRAME_CONFIG != HELI_FRAME
|
||||
case TUNING_RATE_MOT_YAW_HEADROOM:
|
||||
motors.set_yaw_headroom(tuning_value*1000);
|
||||
motors->set_yaw_headroom(tuning_value*1000);
|
||||
break;
|
||||
#endif
|
||||
|
||||
case TUNING_RATE_YAW_FILT:
|
||||
attitude_control.get_rate_yaw_pid().filt_hz(tuning_value);
|
||||
attitude_control->get_rate_yaw_pid().filt_hz(tuning_value);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
Loading…
Reference in New Issue
Block a user