zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Sub: adjust for desired spool state renames

master
Peter Barker 6 years ago committed by Randy Mackay
parent
ad6c1acbd7
commit
cb88bc7f53
9 changed files with 28 additions and 28 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      ArduSub/control_acro.cpp
  2. 4
      ArduSub/control_althold.cpp
  3. 14
      ArduSub/control_auto.cpp
  4. 4
      ArduSub/control_circle.cpp
  5. 16
      ArduSub/control_guided.cpp
  6. 4
      ArduSub/control_manual.cpp
  7. 4
      ArduSub/control_poshold.cpp
  8. 4
      ArduSub/control_stabilize.cpp
  9. 2
      ArduSub/control_surface.cpp

4
ArduSub/control_acro.cpp
Unescape View File

@ -26,13 +26,13 @@ void Sub::acro_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; 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 // 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); 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);

4
ArduSub/control_althold.cpp
Unescape View File

@ -37,7 +37,7 @@ void Sub::althold_run()
pos_control.set_max_accel_z(g.pilot_accel_z); pos_control.set_max_accel_z(g.pilot_accel_z);
if (!motors.armed()) { 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) // 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -46,7 +46,7 @@ void Sub::althold_run()
return; 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 // get pilot desired lean angles
float target_roll, target_pitch; float target_roll, target_pitch;

14
ArduSub/control_auto.cpp
Unescape View File

@ -117,7 +117,7 @@ void Sub::auto_wp_run()
// (of course it would be better if people just used take-off) // (of course it would be better if people just used take-off)
// call attitude controller // call attitude controller
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; return;
@ -134,7 +134,7 @@ void Sub::auto_wp_run()
} }
// set motors to full range // 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 // run waypoint controller
// TODO logic for terrain tracking target going below fence limit // 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 // 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) // (of course it would be better if people just used take-off)
// Sub vehicles do not stabilize roll/pitch/yaw when disarmed // 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; return;
@ -220,7 +220,7 @@ void Sub::auto_spline_run()
} }
// set motors to full range // 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 // run waypoint controller
wp_nav.update_spline(); wp_nav.update_spline();
@ -389,7 +389,7 @@ void Sub::auto_loiter_run()
{ {
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -404,7 +404,7 @@ void Sub::auto_loiter_run()
} }
// set motors to full range // 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 // run waypoint and z-axis position controller
failsafe_terrain_set_status(wp_nav.update_wpnav()); 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 not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; return;

4
ArduSub/control_circle.cpp
Unescape View File

@ -41,7 +41,7 @@ void Sub::circle_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
// To-Do: add some initialisation of position controllers // 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); 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()); target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
// set motors to full range // 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 // run circle controller
circle_nav.update(); circle_nav.update();

16
ArduSub/control_guided.cpp
Unescape View File

@ -291,7 +291,7 @@ void Sub::guided_pos_control_run()
{ {
// if motors not enabled set throttle to zero and exit immediately // if motors not enabled set throttle to zero and exit immediately
if (!motors.armed()) { 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -309,7 +309,7 @@ void Sub::guided_pos_control_run()
} }
// set motors to full range // 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 // run waypoint controller
failsafe_terrain_set_status(wp_nav.update_wpnav()); failsafe_terrain_set_status(wp_nav.update_wpnav());
@ -342,7 +342,7 @@ void Sub::guided_vel_control_run()
if (!motors.armed()) { if (!motors.armed()) {
// initialise velocity controller // initialise velocity controller
pos_control.init_vel_controller_xyz(); 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -360,7 +360,7 @@ void Sub::guided_vel_control_run()
} }
// set motors to full range // 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 // set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis(); 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 // set target position and velocity to current position and velocity
pos_control.set_pos_target(inertial_nav.get_position()); pos_control.set_pos_target(inertial_nav.get_position());
pos_control.set_desired_velocity(Vector3f(0,0,0)); 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -416,7 +416,7 @@ void Sub::guided_posvel_control_run()
} }
// set motors to full range // 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 // set velocity to zero if no updates received for 3 seconds
uint32_t tnow = AP_HAL::millis(); 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 not enabled set throttle to zero and exit immediately
if (!motors.armed()) { 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 // Sub vehicles do not stabilize roll/pitch/yaw when disarmed
attitude_control.set_throttle_out(0.0f,true,g.throttle_filt); attitude_control.set_throttle_out(0.0f,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
@ -501,7 +501,7 @@ void Sub::guided_angle_control_run()
} }
// set motors to full range // 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 // call attitude controller
attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true); attitude_control.input_euler_angle_roll_pitch_yaw(roll_in, pitch_in, yaw_in, true);

4
ArduSub/control_manual.cpp
Unescape View File

@ -19,13 +19,13 @@ void Sub::manual_run()
{ {
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; 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_roll(channel_roll->norm_input());
motors.set_pitch(channel_pitch->norm_input()); motors.set_pitch(channel_pitch->norm_input());

4
ArduSub/control_poshold.cpp
Unescape View File

@ -40,7 +40,7 @@ void Sub::poshold_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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.clear_pilot_desired_acceleration();
loiter_nav.init_target(); loiter_nav.init_target();
attitude_control.set_throttle_out(0,true,g.throttle_filt); attitude_control.set_throttle_out(0,true,g.throttle_filt);
@ -50,7 +50,7 @@ void Sub::poshold_run()
} }
// set motors to full range // 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 // run loiter controller
loiter_nav.update(); loiter_nav.update();

4
ArduSub/control_stabilize.cpp
Unescape View File

@ -20,14 +20,14 @@ void Sub::stabilize_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
last_pilot_heading = ahrs.yaw_sensor; last_pilot_heading = ahrs.yaw_sensor;
return; 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 // convert pilot input to lean angles
// To-Do: convert get_pilot_desired_lean_angles to return angles as floats // To-Do: convert get_pilot_desired_lean_angles to return angles as floats

2
ArduSub/control_surface.cpp
Unescape View File

@ -27,7 +27,7 @@ void Sub::surface_run()
// if not armed set throttle to zero and exit immediately // if not armed set throttle to zero and exit immediately
if (!motors.armed()) { if (!motors.armed()) {
motors.output_min(); 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.set_throttle_out(0,true,g.throttle_filt);
attitude_control.relax_attitude_controllers(); attitude_control.relax_attitude_controllers();
return; return;

Write Preview
Loading…
Cancel
Save