diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index e74bda495a..2d472b0181 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -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 } diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 8ecea48481..03c6ce7495 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -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 diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index 329eee004f..d922946f78 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -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), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 6f51717d8f..926b55d7f1 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -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); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 80327191d0..204ede9af1 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -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; diff --git a/ArduCopter/Log.cpp b/ArduCopter/Log.cpp index af4c68ecee..d38f926b61 100644 --- a/ArduCopter/Log.cpp +++ b/ArduCopter/Log.cpp @@ -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)); } diff --git a/ArduCopter/afs_copter.cpp b/ArduCopter/afs_copter.cpp index 278bc36769..6f76d74263 100644 --- a/ArduCopter/afs_copter.cpp +++ b/ArduCopter/afs_copter.cpp @@ -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 } diff --git a/ArduCopter/arming_checks.cpp b/ArduCopter/arming_checks.cpp index 47b30f9bf3..04931503d8 100644 --- a/ArduCopter/arming_checks.cpp +++ b/ArduCopter/arming_checks.cpp @@ -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; } diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 89cb56bf76..579be71365 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -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; diff --git a/ArduCopter/baro_ground_effect.cpp b/ArduCopter/baro_ground_effect.cpp index 9f26adb500..313c120195 100644 --- a/ArduCopter/baro_ground_effect.cpp +++ b/ArduCopter/baro_ground_effect.cpp @@ -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)); diff --git a/ArduCopter/commands.cpp b/ArduCopter/commands.cpp index 91dfb67684..e80d7de935 100644 --- a/ArduCopter/commands.cpp +++ b/ArduCopter/commands.cpp @@ -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) diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index efcafd7f5f..29c9f2947f 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -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(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); } } diff --git a/ArduCopter/compassmot.cpp b/ArduCopter/compassmot.cpp index 87f55710b1..bf9aeefdf2 100644 --- a/ArduCopter/compassmot.cpp +++ b/ArduCopter/compassmot.cpp @@ -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) { diff --git a/ArduCopter/control_acro.cpp b/ArduCopter/control_acro.cpp index 65199372c2..01ee6e7790 100644 --- a/ArduCopter/control_acro.cpp +++ b/ArduCopter/control_acro.cpp @@ -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) { diff --git a/ArduCopter/control_althold.cpp b/ArduCopter/control_althold.cpp index 45f0deea2d..1a314816e3 100644 --- a/ArduCopter/control_althold.cpp +++ b/ArduCopter/control_althold.cpp @@ -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; } } diff --git a/ArduCopter/control_auto.cpp b/ArduCopter/control_auto.cpp index b07bcd6abe..6619ffb7de 100644 --- a/ArduCopter/control_auto.cpp +++ b/ArduCopter/control_auto.cpp @@ -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() diff --git a/ArduCopter/control_autotune.cpp b/ArduCopter/control_autotune.cpp index f91c046612..d6f587e408 100644 --- a/ArduCopter/control_autotune.cpp +++ b/ArduCopter/control_autotune.cpp @@ -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); diff --git a/ArduCopter/control_brake.cpp b/ArduCopter/control_brake.cpp index 6106a86cb5..e0cf9124c5 100644 --- a/ArduCopter/control_brake.cpp +++ b/ArduCopter/control_brake.cpp @@ -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)) { diff --git a/ArduCopter/control_circle.cpp b/ArduCopter/control_circle.cpp index 2785c158df..e1e06fcbf0 100644 --- a/ArduCopter/control_circle.cpp +++ b/ArduCopter/control_circle.cpp @@ -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(); } diff --git a/ArduCopter/control_drift.cpp b/ArduCopter/control_drift.cpp index 14dea0eda9..37a9240c2a 100644 --- a/ArduCopter/control_drift.cpp +++ b/ArduCopter/control_drift.cpp @@ -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 diff --git a/ArduCopter/control_flip.cpp b/ArduCopter/control_flip.cpp index 0c8cd6dd09..217dd2da1c 100644 --- a/ArduCopter/control_flip.cpp +++ b/ArduCopter/control_flip.cpp @@ -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); } diff --git a/ArduCopter/control_guided.cpp b/ArduCopter/control_guided.cpp index eb6deb3d9f..59ae772069 100644 --- a/ArduCopter/control_guided.cpp +++ b/ArduCopter/control_guided.cpp @@ -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 diff --git a/ArduCopter/control_land.cpp b/ArduCopter/control_land.cpp index 4d5961b153..cd48e08e76 100644 --- a/ArduCopter/control_land.cpp +++ b/ArduCopter/control_land.cpp @@ -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 diff --git a/ArduCopter/control_loiter.cpp b/ArduCopter/control_loiter.cpp index ba115b9408..53c0542b85 100644 --- a/ArduCopter/control_loiter.cpp +++ b/ArduCopter/control_loiter.cpp @@ -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; } } diff --git a/ArduCopter/control_poshold.cpp b/ArduCopter/control_poshold.cpp index 76a0675a78..bbf986ab2f 100644 --- a/ArduCopter/control_poshold.cpp +++ b/ArduCopter/control_poshold.cpp @@ -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)) { diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 6fa7e9e1eb..650b922792 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -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); diff --git a/ArduCopter/control_sport.cpp b/ArduCopter/control_sport.cpp index 7bd5c8eef2..3c56b8c3c2 100644 --- a/ArduCopter/control_sport.cpp +++ b/ArduCopter/control_sport.cpp @@ -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; } } diff --git a/ArduCopter/control_stabilize.cpp b/ArduCopter/control_stabilize.cpp index 7e9ea86bf4..a2e5f3ffad 100644 --- a/ArduCopter/control_stabilize.cpp +++ b/ArduCopter/control_stabilize.cpp @@ -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); } diff --git a/ArduCopter/control_throw.cpp b/ArduCopter/control_throw.cpp index ed48ffa021..f9cf4da02d 100644 --- a/ArduCopter/control_throw.cpp +++ b/ArduCopter/control_throw.cpp @@ -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); } diff --git a/ArduCopter/crash_check.cpp b/ArduCopter/crash_check.cpp index 0ff358d44a..6f706f2ced 100644 --- a/ArduCopter/crash_check.cpp +++ b/ArduCopter/crash_check.cpp @@ -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--; diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 5a20710303..ece5534883 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -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); } diff --git a/ArduCopter/esc_calibration.cpp b/ArduCopter/esc_calibration.cpp index dc083c7f02..521abdf638 100644 --- a/ArduCopter/esc_calibration.cpp +++ b/ArduCopter/esc_calibration.cpp @@ -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 } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 2a713e61c4..36b78cd748 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -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; } diff --git a/ArduCopter/failsafe.cpp b/ArduCopter/failsafe.cpp index 09b5623d79..15c09e3add 100644 --- a/ArduCopter/failsafe.cpp +++ b/ArduCopter/failsafe.cpp @@ -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(); } } } diff --git a/ArduCopter/fence.cpp b/ArduCopter/fence.cpp index 067912bf6a..3d7b59c5c8 100644 --- a/ArduCopter/fence.cpp +++ b/ArduCopter/fence.cpp @@ -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; } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index 6bdc9fb91d..69acbf9566 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -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, diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index 06f42afe97..3e0e70c0aa 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -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 diff --git a/ArduCopter/heli_control_acro.cpp b/ArduCopter/heli_control_acro.cpp index efc97eca15..c1037a77b9 100644 --- a/ArduCopter/heli_control_acro.cpp +++ b/ArduCopter/heli_control_acro.cpp @@ -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 diff --git a/ArduCopter/heli_control_stabilize.cpp b/ArduCopter/heli_control_stabilize.cpp index f1f1aadc77..f225c1c8e9 100644 --- a/ArduCopter/heli_control_stabilize.cpp +++ b/ArduCopter/heli_control_stabilize.cpp @@ -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 diff --git a/ArduCopter/land_detector.cpp b/ArduCopter/land_detector.cpp index 7c116b31ef..d13edc4a07 100644 --- a/ArduCopter/land_detector.cpp +++ b/ArduCopter/land_detector.cpp @@ -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 diff --git a/ArduCopter/motor_test.cpp b/ArduCopter/motor_test.cpp index 097d18d4f7..8e140a3e6e 100644 --- a/ArduCopter/motor_test.cpp +++ b/ArduCopter/motor_test.cpp @@ -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; diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index b1db22998d..7dbd330ede 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -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; diff --git a/ArduCopter/navigation.cpp b/ArduCopter/navigation.cpp index 50f0d818d1..ef4e02683b 100644 --- a/ArduCopter/navigation.cpp +++ b/ArduCopter/navigation.cpp @@ -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 diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 972b830e37..7ddc2680a9 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -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); } diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 3eeb05d168..fae4ebe18b 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -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) { diff --git a/ArduCopter/setup.cpp b/ArduCopter/setup.cpp index a49eec7194..0f9b1266a1 100644 --- a/ArduCopter/setup.cpp +++ b/ArduCopter/setup.cpp @@ -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<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<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"); diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index aab68c9dc8..10d3b5d6d1 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -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: diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 4ed6e04baa..ca500c0fb2 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -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"); + } +} diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index ac266fc90f..0834a4292c 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -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()); } diff --git a/ArduCopter/tuning.cpp b/ArduCopter/tuning.cpp index 686b4e912d..716f73b38a 100644 --- a/ArduCopter/tuning.cpp +++ b/ArduCopter/tuning.cpp @@ -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; } }