Browse Source

Sub: move servorelayevents mission handling into AP_Mission

mission-4.1.18
Peter Barker 6 years ago committed by Peter Barker
parent
commit
5a8a3963d4
  1. 22
      ArduSub/commands_logic.cpp

22
ArduSub/commands_logic.cpp

@ -95,24 +95,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd) @@ -95,24 +95,6 @@ bool Sub::start_command(const AP_Mission::Mission_Command& cmd)
do_set_home(cmd);
break;
case MAV_CMD_DO_SET_SERVO:
ServoRelayEvents.do_set_servo(cmd.content.servo.channel, cmd.content.servo.pwm);
break;
case MAV_CMD_DO_SET_RELAY:
ServoRelayEvents.do_set_relay(cmd.content.relay.num, cmd.content.relay.state);
break;
case MAV_CMD_DO_REPEAT_SERVO:
ServoRelayEvents.do_repeat_servo(cmd.content.repeat_servo.channel, cmd.content.repeat_servo.pwm,
cmd.content.repeat_servo.repeat_count, cmd.content.repeat_servo.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_REPEAT_RELAY:
ServoRelayEvents.do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count,
cmd.content.repeat_relay.cycle_time * 1000.0f);
break;
case MAV_CMD_DO_SET_ROI: // 201
// point the vehicle and camera at a region of interest (ROI)
do_roi(cmd);
@ -228,10 +210,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd) @@ -228,10 +210,6 @@ bool Sub::verify_command(const AP_Mission::Mission_Command& cmd)
// do commands (always return true)
case MAV_CMD_DO_CHANGE_SPEED:
case MAV_CMD_DO_SET_HOME:
case MAV_CMD_DO_SET_SERVO:
case MAV_CMD_DO_SET_RELAY:
case MAV_CMD_DO_REPEAT_SERVO:
case MAV_CMD_DO_REPEAT_RELAY:
case MAV_CMD_DO_SET_ROI:
case MAV_CMD_DO_MOUNT_CONTROL:
case MAV_CMD_DO_CONTROL_VIDEO:

Loading…
Cancel
Save