diff --git a/ArduSub/control_acro.cpp b/ArduSub/control_acro.cpp index 4884d5cc81..48047f4a43 100644 --- a/ArduSub/control_acro.cpp +++ b/ArduSub/control_acro.cpp @@ -26,13 +26,13 @@ void Sub::acro_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return; } - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::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); diff --git a/ArduSub/control_althold.cpp b/ArduSub/control_althold.cpp index a5e14a477b..2a95aab69d 100644 --- a/ArduSub/control_althold.cpp +++ b/ArduSub/control_althold.cpp @@ -37,7 +37,7 @@ void Sub::althold_run() pos_control.set_max_accel_z(g.pilot_accel_z); if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when not auto-armed (i.e. on the ground, pilot has never raised throttle) attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -46,7 +46,7 @@ void Sub::althold_run() return; } - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // get pilot desired lean angles float target_roll, target_pitch; diff --git a/ArduSub/control_auto.cpp b/ArduSub/control_auto.cpp index d12b05c657..e538f6f271 100644 --- a/ArduSub/control_auto.cpp +++ b/ArduSub/control_auto.cpp @@ -117,7 +117,7 @@ void Sub::auto_wp_run() // (of course it would be better if people just used take-off) // call attitude controller // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return; @@ -134,7 +134,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller // TODO logic for terrain tracking target going below fence limit @@ -203,7 +203,7 @@ void Sub::auto_spline_run() // To-Do: reset waypoint origin to current location because vehicle 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) // Sub vehicles do not stabilize roll/pitch/yaw when disarmed - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return; @@ -220,7 +220,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller wp_nav.update_spline(); @@ -389,7 +389,7 @@ void Sub::auto_loiter_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -404,7 +404,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint and z-axis position controller failsafe_terrain_set_status(wp_nav.update_wpnav()); @@ -675,7 +675,7 @@ void Sub::auto_terrain_recover_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return; diff --git a/ArduSub/control_circle.cpp b/ArduSub/control_circle.cpp index 07cf4d7597..7f2248bbde 100644 --- a/ArduSub/control_circle.cpp +++ b/ArduSub/control_circle.cpp @@ -41,7 +41,7 @@ void Sub::circle_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { // To-Do: add some initialisation of position controllers - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -60,7 +60,7 @@ void Sub::circle_run() target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in()); // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run circle controller circle_nav.update(); diff --git a/ArduSub/control_guided.cpp b/ArduSub/control_guided.cpp index 17ee7e5876..a6cda2fa94 100644 --- a/ArduSub/control_guided.cpp +++ b/ArduSub/control_guided.cpp @@ -291,7 +291,7 @@ void Sub::guided_pos_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -309,7 +309,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // run waypoint controller failsafe_terrain_set_status(wp_nav.update_wpnav()); @@ -342,7 +342,7 @@ void Sub::guided_vel_control_run() if (!motors.armed()) { // initialise velocity controller pos_control.init_vel_controller_xyz(); - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -360,7 +360,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); @@ -397,7 +397,7 @@ void Sub::guided_posvel_control_run() // 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)); - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -416,7 +416,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // set velocity to zero if no updates received for 3 seconds uint32_t tnow = AP_HAL::millis(); @@ -467,7 +467,7 @@ void Sub::guided_angle_control_run() { // if motors not enabled set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); // Sub vehicles do not stabilize roll/pitch/yaw when disarmed attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); @@ -501,7 +501,7 @@ void Sub::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::DesiredSpoolState::THROTTLE_UNLIMITED); // call attitude controller attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); diff --git a/ArduSub/control_manual.cpp b/ArduSub/control_manual.cpp index baeb646cd8..b68521abc2 100644 --- a/ArduSub/control_manual.cpp +++ b/ArduSub/control_manual.cpp @@ -19,13 +19,13 @@ void Sub::manual_run() { // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return; } - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); motors.set_roll(channel_roll->norm_input()); motors.set_pitch(channel_pitch->norm_input()); diff --git a/ArduSub/control_poshold.cpp b/ArduSub/control_poshold.cpp index 45d7dae2f8..4ad303d3e7 100644 --- a/ArduSub/control_poshold.cpp +++ b/ArduSub/control_poshold.cpp @@ -40,7 +40,7 @@ void Sub::poshold_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); loiter_nav.clear_pilot_desired_acceleration(); loiter_nav.init_target(); attitude_control.set_throttle_out(0,true,g.throttle_filt); @@ -50,7 +50,7 @@ void Sub::poshold_run() } // set motors to full range - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // run loiter controller loiter_nav.update(); diff --git a/ArduSub/control_stabilize.cpp b/ArduSub/control_stabilize.cpp index 7963cf674b..ad0118a200 100644 --- a/ArduSub/control_stabilize.cpp +++ b/ArduSub/control_stabilize.cpp @@ -20,14 +20,14 @@ void Sub::stabilize_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); last_pilot_heading = ahrs.yaw_sensor; return; } - motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); // convert pilot input to lean angles // To-Do: convert get_pilot_desired_lean_angles to return angles as floats diff --git a/ArduSub/control_surface.cpp b/ArduSub/control_surface.cpp index 924a0fbf06..e6ffe645a6 100644 --- a/ArduSub/control_surface.cpp +++ b/ArduSub/control_surface.cpp @@ -27,7 +27,7 @@ void Sub::surface_run() // if not armed set throttle to zero and exit immediately if (!motors.armed()) { motors.output_min(); - motors.set_desired_spool_state(AP_Motors::DESIRED_GROUND_IDLE); + motors.set_desired_spool_state(AP_Motors::DesiredSpoolState::GROUND_IDLE); attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.relax_attitude_controllers(); return;