aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Copter: combined tri, single, coax and multicopter into a single build

Browse Source 
this allows copter to be just 2 builds, one for heli, and one for
everything else
This commit is contained in:
Andrew Tridgell 2017-01-09 18:31:26 +11:00
parent 5cf1c0869d
commit 0f6d0c5ba9
50 changed files with 1085 additions and 1060 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
ArduCopter/ArduCopter.cpp
View File

@ -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
}

24
ArduCopter/Attitude.cpp
View File

@ -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

7
ArduCopter/Copter.cpp
View File

@ -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),

27
ArduCopter/Copter.h
View File

@ -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);

36
ArduCopter/GCS_Mavlink.cpp
View File

@ -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;

32
ArduCopter/Log.cpp
View File

@ -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));
}

8
ArduCopter/afs_copter.cpp
View File

@ -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
}

10
ArduCopter/arming_checks.cpp
View File

@ -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;
}

16
ArduCopter/avoidance_adsb.cpp
View File

@ -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;

18
ArduCopter/baro_ground_effect.cpp
View File

@ -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));

2
ArduCopter/commands.cpp
View File

@ -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)

32
ArduCopter/commands_logic.cpp
View File

@ -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);
}
}

10
ArduCopter/compassmot.cpp
View File

@ -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) {

18
ArduCopter/control_acro.cpp
View File

@ -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) {

70
ArduCopter/control_althold.cpp
View File

@ -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;
}
}

178
ArduCopter/control_auto.cpp
View File

@ -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()

356
ArduCopter/control_autotune.cpp
View File

@ -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);

40
ArduCopter/control_brake.cpp
View File

@ -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)) {

46
ArduCopter/control_circle.cpp
View File

@ -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();
}

12
ArduCopter/control_drift.cpp
View File

@ -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

18
ArduCopter/control_flip.cpp
View File

@ -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);
}

176
ArduCopter/control_guided.cpp
View File

@ -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

74
ArduCopter/control_land.cpp
View File

@ -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

94
ArduCopter/control_loiter.cpp
View File

@ -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;
}
}

76
ArduCopter/control_poshold.cpp
View File

@ -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)) {

106
ArduCopter/control_rtl.cpp
View File

@ -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);

68
ArduCopter/control_sport.cpp
View File

@ -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;
}
}

16
ArduCopter/control_stabilize.cpp
View File

@ -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);
}

58
ArduCopter/control_throw.cpp
View File

@ -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);
}

8
ArduCopter/crash_check.cpp
View File

@ -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--;

6
ArduCopter/ekf_check.cpp
View File

@ -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);
}

30
ArduCopter/esc_calibration.cpp
View File

@ -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
}

6
ArduCopter/events.cpp
View File

@ -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;
}

12
ArduCopter/failsafe.cpp
View File

@ -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();
}
}
}

2
ArduCopter/fence.cpp
View File

@ -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;
}

8
ArduCopter/flight_mode.cpp
View File

@ -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,

40
ArduCopter/heli.cpp
View File

@ -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

28
ArduCopter/heli_control_acro.cpp
View File

@ -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

18
ArduCopter/heli_control_stabilize.cpp
View File

@ -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

30
ArduCopter/land_detector.cpp
View File

@ -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

12
ArduCopter/motor_test.cpp
View File

@ -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;

36
ArduCopter/motors.cpp
View File

@ -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;

12
ArduCopter/navigation.cpp
View File

@ -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

25
ArduCopter/radio.cpp
View File

@ -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);
}

10
ArduCopter/sensors.cpp
View File

@ -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) {

8
ArduCopter/setup.cpp
View File

@ -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");

8
ArduCopter/switches.cpp
View File

@ -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:

124
ArduCopter/system.cpp
View File

@ -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");
}
}

16
ArduCopter/takeoff.cpp
View File

@ -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());
}

50
ArduCopter/tuning.cpp
View File

@ -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;
}
}