diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 04e0fcc813..cca8187a3b 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -537,7 +537,7 @@ void Copter::update_GPS(void) void Copter::smart_rtl_save_position() { - flightmode_smartrtl.save_position(); + mode_smartrtl.save_position(); } void Copter::init_simple_bearing() diff --git a/ArduCopter/Copter.cpp b/ArduCopter/Copter.cpp index a62b57e505..86cc564f86 100644 --- a/ArduCopter/Copter.cpp +++ b/ArduCopter/Copter.cpp @@ -57,7 +57,7 @@ Copter::Copter(void) auto_trim_counter(0), in_mavlink_delay(false), param_loader(var_info), - flightmode(&flightmode_stabilize) + flightmode(&mode_stabilize) { memset(¤t_loc, 0, sizeof(current_loc)); diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 51ab970576..68e0742b27 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -946,54 +946,54 @@ private: Copter::Mode *flightmode; #if FRAME_CONFIG == HELI_FRAME - Copter::ModeAcro_Heli flightmode_acro{*this}; + Copter::ModeAcro_Heli mode_acro{*this}; #else - Copter::ModeAcro flightmode_acro{*this}; + Copter::ModeAcro mode_acro{*this}; #endif - Copter::ModeAltHold flightmode_althold{*this}; + Copter::ModeAltHold mode_althold{*this}; - Copter::ModeAuto flightmode_auto{*this, mission, circle_nav}; + Copter::ModeAuto mode_auto{*this, mission, circle_nav}; #if AUTOTUNE_ENABLED == ENABLED - Copter::ModeAutoTune flightmode_autotune{*this}; + Copter::ModeAutoTune mode_autotune{*this}; #endif - Copter::ModeBrake flightmode_brake{*this}; + Copter::ModeBrake mode_brake{*this}; - Copter::ModeCircle flightmode_circle{*this, circle_nav}; + Copter::ModeCircle mode_circle{*this, circle_nav}; - Copter::ModeDrift flightmode_drift{*this}; + Copter::ModeDrift mode_drift{*this}; - Copter::ModeFlip flightmode_flip{*this}; + Copter::ModeFlip mode_flip{*this}; - Copter::ModeGuided flightmode_guided{*this}; + Copter::ModeGuided mode_guided{*this}; - Copter::ModeLand flightmode_land{*this}; + Copter::ModeLand mode_land{*this}; - Copter::ModeLoiter flightmode_loiter{*this}; + Copter::ModeLoiter mode_loiter{*this}; - Copter::ModePosHold flightmode_poshold{*this}; + Copter::ModePosHold mode_poshold{*this}; - Copter::ModeRTL flightmode_rtl{*this}; + Copter::ModeRTL mode_rtl{*this}; #if FRAME_CONFIG == HELI_FRAME - Copter::ModeStabilize_Heli flightmode_stabilize{*this}; + Copter::ModeStabilize_Heli mode_stabilize{*this}; #else - Copter::ModeStabilize flightmode_stabilize{*this}; + Copter::ModeStabilize mode_stabilize{*this}; #endif - Copter::ModeSport flightmode_sport{*this}; + Copter::ModeSport mode_sport{*this}; - Copter::ModeAvoidADSB flightmode_avoid_adsb{*this}; + Copter::ModeAvoidADSB mode_avoid_adsb{*this}; - Copter::ModeThrow flightmode_throw{*this}; + Copter::ModeThrow mode_throw{*this}; - Copter::ModeGuidedNoGPS flightmode_guided_nogps{*this}; + Copter::ModeGuidedNoGPS mode_guided_nogps{*this}; - Copter::ModeSmartRTL flightmode_smartrtl{*this}; + Copter::ModeSmartRTL mode_smartrtl{*this}; - Copter::Mode *flightmode_for_mode(const uint8_t mode); + Copter::Mode *mode_from_mode_num(const uint8_t mode); void exit_mode(Mode *&old_flightmode, Mode *&new_flightmode); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 94abf7a9b7..ec3d0b92d0 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -1291,7 +1291,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) if (!shot_mode) { if (copter.set_mode(BRAKE, MODE_REASON_GCS_COMMAND)) { - copter.flightmode_brake.timeout_to_loiter_ms(2500); + copter.mode_brake.timeout_to_loiter_ms(2500); } else { copter.set_mode(ALT_HOLD, MODE_REASON_GCS_COMMAND); } @@ -1335,7 +1335,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_attitude_target_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && (copter.control_mode != GUIDED_NOGPS) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && (copter.control_mode != GUIDED_NOGPS) && !(copter.control_mode == AUTO && copter.mode_auto.mode() == Auto_NavGuided)) { break; } @@ -1364,7 +1364,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) use_yaw_rate = true; } - copter.flightmode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), + copter.mode_guided.set_angle(Quaternion(packet.q[0],packet.q[1],packet.q[2],packet.q[3]), climb_rate_cms, use_yaw_rate, packet.body_yaw_rate); break; @@ -1377,7 +1377,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_local_ned_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.mode_auto.mode() == Auto_NavGuided)) { break; } @@ -1446,16 +1446,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) // send request if (!pos_ignore && !vel_ignore && acc_ignore) { - if (copter.flightmode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.mode_guided.set_destination_posvel(pos_vector, vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.flightmode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_velocity(vel_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); result = MAV_RESULT_ACCEPTED; } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (copter.flightmode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.mode_guided.set_destination(pos_vector, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; @@ -1474,7 +1474,7 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) mavlink_msg_set_position_target_global_int_decode(msg, &packet); // exit if vehicle is not in Guided mode or Auto-Guided mode - if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.flightmode_auto.mode() == Auto_NavGuided)) { + if ((copter.control_mode != GUIDED) && !(copter.control_mode == AUTO && copter.mode_auto.mode() == Auto_NavGuided)) { break; } @@ -1548,16 +1548,16 @@ void GCS_MAVLINK_Copter::handleMessage(mavlink_message_t* msg) } if (!pos_ignore && !vel_ignore && acc_ignore) { - if (copter.flightmode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.mode_guided.set_destination_posvel(pos_neu_cm, Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; } } else if (pos_ignore && !vel_ignore && acc_ignore) { - copter.flightmode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); + copter.mode_guided.set_velocity(Vector3f(packet.vx * 100.0f, packet.vy * 100.0f, -packet.vz * 100.0f), !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative); result = MAV_RESULT_ACCEPTED; } else if (!pos_ignore && vel_ignore && acc_ignore) { - if (copter.flightmode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { + if (copter.mode_guided.set_destination(pos_neu_cm, !yaw_ignore, yaw_cd, !yaw_rate_ignore, yaw_rate_cds, yaw_relative)) { result = MAV_RESULT_ACCEPTED; } else { result = MAV_RESULT_FAILED; diff --git a/ArduCopter/avoidance_adsb.cpp b/ArduCopter/avoidance_adsb.cpp index 7ba99bf628..5bf951dfef 100644 --- a/ArduCopter/avoidance_adsb.cpp +++ b/ArduCopter/avoidance_adsb.cpp @@ -182,7 +182,7 @@ bool AP_Avoidance_Copter::handle_avoidance_vertical(const AP_Avoidance::Obstacle } // send target velocity - copter.flightmode_avoid_adsb.set_velocity(velocity_neu); + copter.mode_avoid_adsb.set_velocity(velocity_neu); return true; } @@ -208,7 +208,7 @@ bool AP_Avoidance_Copter::handle_avoidance_horizontal(const AP_Avoidance::Obstac velocity_neu.x *= copter.wp_nav->get_speed_xy(); velocity_neu.y *= copter.wp_nav->get_speed_xy(); // send target velocity - copter.flightmode_avoid_adsb.set_velocity(velocity_neu); + copter.mode_avoid_adsb.set_velocity(velocity_neu); return true; } @@ -240,7 +240,7 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs } } // send target velocity - copter.flightmode_avoid_adsb.set_velocity(velocity_neu); + copter.mode_avoid_adsb.set_velocity(velocity_neu); return true; } diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 1e4c041c7c..38b8bfccd1 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -292,7 +292,7 @@ void Copter::exit_mission() // if we are not on the ground switch to loiter or land if(!ap.land_complete) { // try to enter loiter but if that fails land - if(!flightmode_auto.loiter_start()) { + if(!mode_auto.loiter_start()) { set_mode(LAND, MODE_REASON_MISSION_END); } }else{ @@ -309,7 +309,7 @@ void Copter::exit_mission() void Copter::do_RTL(void) { // start rtl in auto flight mode - flightmode_auto.rtl_start(); + mode_auto.rtl_start(); } /********************************************************************************/ @@ -320,7 +320,7 @@ void Copter::do_RTL(void) void Copter::do_takeoff(const AP_Mission::Mission_Command& cmd) { // Set wp navigation target to safe altitude above current position - flightmode_auto.takeoff_start(cmd.content.location); + mode_auto.takeoff_start(cmd.content.location); } // do_nav_wp - initiate move to next waypoint @@ -350,7 +350,7 @@ void Copter::do_nav_wp(const AP_Mission::Mission_Command& cmd) loiter_time_max = cmd.p1; // Set wp navigation target - flightmode_auto.wp_start(target_loc); + mode_auto.wp_start(target_loc); // if no delay set the waypoint as "fast" if (loiter_time_max == 0 ) { @@ -391,13 +391,13 @@ void Copter::do_land(const AP_Mission::Mission_Command& cmd) Location_Class target_loc = terrain_adjusted_location(cmd); - flightmode_auto.wp_start(target_loc); + mode_auto.wp_start(target_loc); }else{ // set landing state land_state = LandStateType_Descending; // initialise landing controller - flightmode_auto.land_start(); + mode_auto.land_start(); } } @@ -411,12 +411,12 @@ void Copter::do_payload_place(const AP_Mission::Mission_Command& cmd) Location_Class target_loc = terrain_adjusted_location(cmd); - flightmode_auto.wp_start(target_loc); + mode_auto.wp_start(target_loc); } else { nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; // initialise placing controller - flightmode_auto.payload_place_start(); + mode_auto.payload_place_start(); } nav_payload_place.descend_max = cmd.p1; } @@ -452,7 +452,7 @@ void Copter::do_loiter_unlimited(const AP_Mission::Mission_Command& cmd) } // start way point navigator and provide it the desired location - flightmode_auto.wp_start(target_loc); + mode_auto.wp_start(target_loc); } // do_circle - initiate moving in a circle @@ -484,7 +484,7 @@ void Copter::do_circle(const AP_Mission::Mission_Command& cmd) uint8_t circle_radius_m = HIGHBYTE(cmd.p1); // circle radius held in high byte of p1 // move to edge of circle (verify_circle) will ensure we begin circling once we reach the edge - flightmode_auto.circle_movetoedge_start(circle_center, circle_radius_m); + mode_auto.circle_movetoedge_start(circle_center, circle_radius_m); } // do_loiter_time - initiate loitering at a point for a given time period @@ -569,7 +569,7 @@ void Copter::do_spline_wp(const AP_Mission::Mission_Command& cmd) } // set spline navigation target - flightmode_auto.spline_start(target_loc, stopped_at_start, seg_end_type, next_loc); + mode_auto.spline_start(target_loc, stopped_at_start, seg_end_type, next_loc); } #if NAV_GUIDED == ENABLED @@ -578,10 +578,10 @@ void Copter::do_nav_guided_enable(const AP_Mission::Mission_Command& cmd) { if (cmd.p1 > 0) { // initialise guided limits - flightmode_guided.limit_init_time_and_pos(); + mode_guided.limit_init_time_and_pos(); // set spline navigation target - flightmode_auto.nav_guided_start(); + mode_auto.nav_guided_start(); } } #endif // NAV_GUIDED @@ -649,7 +649,7 @@ void Copter::do_gripper(const AP_Mission::Mission_Command& cmd) // do_guided_limits - pass guided limits to guided controller void Copter::do_guided_limits(const AP_Mission::Mission_Command& cmd) { - flightmode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms + mode_guided.limit_set(cmd.p1 * 1000, // convert seconds to ms cmd.content.guided_limits.alt_min * 100.0f, // convert meters to cm cmd.content.guided_limits.alt_max * 100.0f, // convert meters to cm cmd.content.guided_limits.horiz_max * 100.0f); // convert meters to cm @@ -703,7 +703,7 @@ bool Copter::verify_land() Vector3f dest = wp_nav->get_wp_destination(); // initialise landing controller - flightmode_auto.land_start(dest); + mode_auto.land_start(dest); // advance to next state land_state = LandStateType_Descending; @@ -866,7 +866,7 @@ bool Copter::verify_payload_place() case PayloadPlaceStateType_Ascending_Start: { Location_Class target_loc = inertial_nav.get_position(); target_loc.alt = nav_payload_place.descend_start_altitude; - flightmode_auto.wp_start(target_loc); + mode_auto.wp_start(target_loc); nav_payload_place.state = PayloadPlaceStateType_Ascending; } FALLTHROUGH; @@ -939,7 +939,7 @@ bool Copter::verify_loiter_time() bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) { // check if we've reached the edge - if (flightmode_auto.mode() == Auto_CircleMoveToEdge) { + if (mode_auto.mode() == Auto_CircleMoveToEdge) { if (wp_nav->reached_wp_destination()) { Vector3f curr_pos = inertial_nav.get_position(); Vector3f circle_center = pv_location_to_vector(cmd.content.location); @@ -956,7 +956,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) } // start circling - flightmode_auto.circle_start(); + mode_auto.circle_start(); } return false; } @@ -970,7 +970,7 @@ bool Copter::verify_circle(const AP_Mission::Mission_Command& cmd) // returns true with RTL has completed successfully bool Copter::verify_RTL() { - return (flightmode_rtl.state_complete() && (flightmode_rtl.state() == RTL_FinalDescent || flightmode_rtl.state() == RTL_Land)); + return (mode_rtl.state_complete() && (mode_rtl.state() == RTL_FinalDescent || mode_rtl.state() == RTL_Land)); } // verify_spline_wp - check if we have reached the next way point using spline @@ -1005,7 +1005,7 @@ bool Copter::verify_nav_guided_enable(const AP_Mission::Mission_Command& cmd) } // check time and position limits - return flightmode_guided.limit_check(); + return mode_guided.limit_check(); } #endif // NAV_GUIDED @@ -1091,7 +1091,7 @@ bool Copter::verify_yaw() bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { // only process guided waypoint if we are in guided mode - if (control_mode != GUIDED && !(control_mode == AUTO && flightmode_auto.mode() == Auto_NavGuided)) { + if (control_mode != GUIDED && !(control_mode == AUTO && mode_auto.mode() == Auto_NavGuided)) { return false; } @@ -1102,7 +1102,7 @@ bool Copter::do_guided(const AP_Mission::Mission_Command& cmd) { // set wp_nav's destination Location_Class dest(cmd.content.location); - return flightmode_guided.set_destination(dest); + return mode_guided.set_destination(dest); } case MAV_CMD_CONDITION_YAW: diff --git a/ArduCopter/ekf_check.cpp b/ArduCopter/ekf_check.cpp index 43c86fc5b0..fe0ed97954 100644 --- a/ArduCopter/ekf_check.cpp +++ b/ArduCopter/ekf_check.cpp @@ -175,7 +175,7 @@ void Copter::failsafe_ekf_event() // if flight mode is already LAND ensure it's not the GPS controlled LAND if (control_mode == LAND) { - flightmode_land.do_not_use_GPS(); + mode_land.do_not_use_GPS(); } } diff --git a/ArduCopter/events.cpp b/ArduCopter/events.cpp index 6367447d28..3939ad4fa3 100644 --- a/ArduCopter/events.cpp +++ b/ArduCopter/events.cpp @@ -178,7 +178,7 @@ void Copter::failsafe_terrain_on_event() if (should_disarm_on_failsafe()) { init_disarm_motors(); } else if (control_mode == RTL) { - flightmode_rtl.restart_without_terrain(); + mode_rtl.restart_without_terrain(); } else { set_mode_RTL_or_land_with_pause(MODE_REASON_TERRAIN_FAILSAFE); } diff --git a/ArduCopter/flight_mode.cpp b/ArduCopter/flight_mode.cpp index a4a4f06d83..ec3c987ac5 100644 --- a/ArduCopter/flight_mode.cpp +++ b/ArduCopter/flight_mode.cpp @@ -6,87 +6,87 @@ */ // return the static controller object corresponding to supplied mode -Copter::Mode *Copter::flightmode_for_mode(const uint8_t mode) +Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) { Copter::Mode *ret = nullptr; switch (mode) { case ACRO: - ret = &flightmode_acro; + ret = &mode_acro; break; case STABILIZE: - ret = &flightmode_stabilize; + ret = &mode_stabilize; break; case ALT_HOLD: - ret = &flightmode_althold; + ret = &mode_althold; break; case AUTO: - ret = &flightmode_auto; + ret = &mode_auto; break; case CIRCLE: - ret = &flightmode_circle; + ret = &mode_circle; break; case LOITER: - ret = &flightmode_loiter; + ret = &mode_loiter; break; case GUIDED: - ret = &flightmode_guided; + ret = &mode_guided; break; case LAND: - ret = &flightmode_land; + ret = &mode_land; break; case RTL: - ret = &flightmode_rtl; + ret = &mode_rtl; break; case DRIFT: - ret = &flightmode_drift; + ret = &mode_drift; break; case SPORT: - ret = &flightmode_sport; + ret = &mode_sport; break; case FLIP: - ret = &flightmode_flip; + ret = &mode_flip; break; #if AUTOTUNE_ENABLED == ENABLED case AUTOTUNE: - ret = &flightmode_autotune; + ret = &mode_autotune; break; #endif case POSHOLD: - ret = &flightmode_poshold; + ret = &mode_poshold; break; case BRAKE: - ret = &flightmode_brake; + ret = &mode_brake; break; case THROW: - ret = &flightmode_throw; + ret = &mode_throw; break; case AVOID_ADSB: - ret = &flightmode_avoid_adsb; + ret = &mode_avoid_adsb; break; case GUIDED_NOGPS: - ret = &flightmode_guided_nogps; + ret = &mode_guided_nogps; break; case SMART_RTL: - ret = &flightmode_smartrtl; + ret = &mode_smartrtl; break; default: @@ -110,7 +110,7 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) return true; } - Copter::Mode *new_flightmode = flightmode_for_mode(mode); + Copter::Mode *new_flightmode = mode_from_mode_num(mode); if (new_flightmode == nullptr) { gcs().send_text(MAV_SEVERITY_WARNING,"No such mode"); Log_Write_Error(ERROR_SUBSYSTEM_FLIGHT_MODE,mode); @@ -180,13 +180,13 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, Copter::Mode *&new_flightmode) { #if AUTOTUNE_ENABLED == ENABLED - if (old_flightmode == &flightmode_autotune) { - flightmode_autotune.stop(); + if (old_flightmode == &mode_autotune) { + mode_autotune.stop(); } #endif // stop mission when we leave auto mode - if (old_flightmode == &flightmode_auto) { + if (old_flightmode == &mode_auto) { if (mission.state() == AP_Mission::MISSION_RUNNING) { mission.stop(); } @@ -205,13 +205,13 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, takeoff_stop(); // call smart_rtl cleanup - if (old_flightmode == &flightmode_smartrtl) { - flightmode_smartrtl.exit(); + if (old_flightmode == &mode_smartrtl) { + mode_smartrtl.exit(); } #if FRAME_CONFIG == HELI_FRAME // firmly reset the flybar passthrough to false when exiting acro mode. - if (old_flightmode == &flightmode_acro) { + if (old_flightmode == &mode_acro) { attitude_control->use_flybar_passthrough(false, false); motors->set_acro_tail(false); } @@ -220,9 +220,9 @@ void Copter::exit_mode(Copter::Mode *&old_flightmode, // stab col ramp value should be pre-loaded to the correct value to avoid a twitch // heli_stab_col_ramp should really only be active switching between Stabilize and Acro modes if (!old_flightmode->has_manual_throttle()){ - if (new_flightmode == &flightmode_stabilize){ + if (new_flightmode == &mode_stabilize){ input_manager.set_stab_col_ramp(1.0); - } else if (new_flightmode == &flightmode_acro){ + } else if (new_flightmode == &mode_acro){ input_manager.set_stab_col_ramp(0.0); } } diff --git a/ArduCopter/heli.cpp b/ArduCopter/heli.cpp index cb7887a637..2310f4dc8a 100644 --- a/ArduCopter/heli.cpp +++ b/ArduCopter/heli.cpp @@ -24,7 +24,7 @@ void Copter::heli_init() void Copter::check_dynamic_flight(void) { if (!motors->armed() || !motors->rotor_runup_complete() || - control_mode == LAND || (control_mode==RTL && flightmode_rtl.state() == RTL_Land) || (control_mode == AUTO && flightmode_auto.mode() == Auto_Land)) { + control_mode == LAND || (control_mode==RTL && mode_rtl.state() == RTL_Land) || (control_mode == AUTO && mode_auto.mode() == Auto_Land)) { heli_dynamic_flight_counter = 0; heli_flags.dynamic_flight = false; return; @@ -109,7 +109,7 @@ void Copter::heli_update_landing_swash() case RTL: case SMART_RTL: - if (flightmode_rtl.state() == RTL_Land) { + if (mode_rtl.state() == RTL_Land) { motors->set_collective_for_landing(true); }else{ motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); @@ -117,7 +117,7 @@ void Copter::heli_update_landing_swash() break; case AUTO: - if (flightmode_auto.mode() == Auto_Land) { + if (mode_auto.mode() == Auto_Land) { motors->set_collective_for_landing(true); }else{ motors->set_collective_for_landing(!heli_flags.dynamic_flight || ap.land_complete || !ap.auto_armed); diff --git a/ArduCopter/landing_gear.cpp b/ArduCopter/landing_gear.cpp index 81c82b54b0..4b0f6ece2b 100644 --- a/ArduCopter/landing_gear.cpp +++ b/ArduCopter/landing_gear.cpp @@ -15,8 +15,8 @@ void Copter::landinggear_update() // if we are doing an automatic landing procedure, force the landing gear to deploy. // To-Do: should we pause the auto-land procedure to give time for gear to come down? if (control_mode == LAND || - (control_mode == RTL && flightmode_rtl.landing_gear_should_be_deployed()) || - (control_mode == AUTO && flightmode_auto.landing_gear_should_be_deployed())) { + (control_mode == RTL && mode_rtl.landing_gear_should_be_deployed()) || + (control_mode == AUTO && mode_auto.landing_gear_should_be_deployed())) { landinggear.set_position(AP_LandingGear::LandingGear_Deploy_And_Keep_Deployed); } diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index a7b0b7ec7c..57e460ef79 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -39,7 +39,7 @@ bool Copter::ModeAuto::init(bool ignore_checks) wp_nav->wp_and_spline_init(); // clear guided limits - _copter.flightmode_guided.limit_clear(); + _copter.mode_guided.limit_clear(); // start/resume the mission (based on MIS_RESTART parameter) mission.start_or_resume(); @@ -417,7 +417,7 @@ bool Copter::ModeAuto::landing_gear_should_be_deployed() case Auto_Land: return true; case Auto_RTL: - switch(_copter.flightmode_rtl.state()) { + switch(_copter.mode_rtl.state()) { case RTL_LoiterAtHome: case RTL_Land: case RTL_FinalDescent: @@ -438,7 +438,7 @@ void Copter::ModeAuto::rtl_start() _mode = Auto_RTL; // call regular rtl flight mode initialisation and ask it to ignore checks - _copter.flightmode_rtl.init(true); + _copter.mode_rtl.init(true); } // auto_rtl_run - rtl in AUTO flight mode @@ -446,7 +446,7 @@ void Copter::ModeAuto::rtl_start() void Copter::ModeAuto::rtl_run() { // call regular rtl flight mode run function - _copter.flightmode_rtl.run(false); + _copter.mode_rtl.run(false); } // auto_circle_movetoedge_start - initialise waypoint controller to move to edge of a circle with it's center at the specified location @@ -534,10 +534,10 @@ void Copter::ModeAuto::nav_guided_start() _mode = Auto_NavGuided; // call regular guided flight mode initialisation - _copter.flightmode_guided.init(true); + _copter.mode_guided.init(true); // initialise guided start time and position as reference for limit checking - _copter.flightmode_guided.limit_init_time_and_pos(); + _copter.mode_guided.limit_init_time_and_pos(); } // auto_nav_guided_run - allows control by external navigation controller @@ -545,7 +545,7 @@ void Copter::ModeAuto::nav_guided_start() void Copter::ModeAuto::nav_guided_run() { // call regular guided flight mode run function - _copter.flightmode_guided.run(); + _copter.mode_guided.run(); } #endif // NAV_GUIDED diff --git a/ArduCopter/mode_land.cpp b/ArduCopter/mode_land.cpp index c0ef4fa4fc..f204e3615e 100644 --- a/ArduCopter/mode_land.cpp +++ b/ArduCopter/mode_land.cpp @@ -177,7 +177,7 @@ void Copter::land_run_vertical_control(bool pause_descent) // compute desired velocity const float precland_acceptable_error = 15.0f; const float precland_min_descent_speed = 10.0f; - int32_t alt_above_ground = flightmode_land.get_alt_above_ground(); + int32_t alt_above_ground = mode_land.get_alt_above_ground(); float cmb_rate = 0; if (!pause_descent) { @@ -281,7 +281,7 @@ void Copter::land_run_horizontal_control() // there is any position estimate drift after touchdown. We // limit attitude to 7 degrees below this limit and linearly // interpolate for 1m above that - int alt_above_ground = flightmode_land.get_alt_above_ground(); + int alt_above_ground = mode_land.get_alt_above_ground(); float attitude_limit_cd = linear_interpolate(700, aparm.angle_max, alt_above_ground, g2.wp_navalt_min*100U, (g2.wp_navalt_min+1)*100U); float total_angle_cd = norm(nav_roll, nav_pitch); diff --git a/ArduCopter/motors.cpp b/ArduCopter/motors.cpp index 017824ea15..58d1cbc7fe 100644 --- a/ArduCopter/motors.cpp +++ b/ArduCopter/motors.cpp @@ -246,7 +246,7 @@ void Copter::init_disarm_motors() #if AUTOTUNE_ENABLED == ENABLED // save auto tuned parameters - flightmode_autotune.save_tuning_gains(); + mode_autotune.save_tuning_gains(); #endif // we are not in the air diff --git a/ArduCopter/switches.cpp b/ArduCopter/switches.cpp index c495dc76ca..d49c603728 100644 --- a/ArduCopter/switches.cpp +++ b/ArduCopter/switches.cpp @@ -562,10 +562,10 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) #if PRECISION_LANDING == ENABLED switch (ch_flag) { case AUX_SWITCH_HIGH: - flightmode_loiter.set_precision_loiter_enabled(true); + mode_loiter.set_precision_loiter_enabled(true); break; case AUX_SWITCH_LOW: - flightmode_loiter.set_precision_loiter_enabled(false); + mode_loiter.set_precision_loiter_enabled(false); break; } #endif diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index bb67c00e49..44e4135114 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -36,7 +36,7 @@ bool Copter::do_user_takeoff(float takeoff_alt_cm, bool must_navigate) switch(control_mode) { case GUIDED: - if (flightmode_guided.takeoff_start(takeoff_alt_cm)) { + if (mode_guided.takeoff_start(takeoff_alt_cm)) { set_auto_armed(true); return true; }