Browse Source

ArduSub: use ArmingMethod enumeration

mission-4.1.18
Peter Barker 7 years ago committed by Peter Barker
parent
commit
3cfdcb1f2c
  1. 2
      ArduSub/GCS_Mavlink.cpp
  2. 2
      ArduSub/Sub.h
  3. 4
      ArduSub/joystick.cpp
  4. 2
      ArduSub/motors.cpp

2
ArduSub/GCS_Mavlink.cpp

@ -928,7 +928,7 @@ void GCS_MAVLINK_Sub::handleMessage(mavlink_message_t* msg)
case MAV_CMD_COMPONENT_ARM_DISARM: case MAV_CMD_COMPONENT_ARM_DISARM:
if (is_equal(packet.param1,1.0f)) { if (is_equal(packet.param1,1.0f)) {
// attempt to arm and return success or failure // attempt to arm and return success or failure
if (sub.init_arm_motors(true)) { if (sub.init_arm_motors(AP_Arming::ArmingMethod::MAVLINK)) {
result = MAV_RESULT_ACCEPTED; result = MAV_RESULT_ACCEPTED;
} }
} else if (is_zero(packet.param1)) { } else if (is_zero(packet.param1)) {

2
ArduSub/Sub.h

@ -601,7 +601,7 @@ private:
void update_surface_and_bottom_detector(); void update_surface_and_bottom_detector();
void set_surfaced(bool at_surface); void set_surfaced(bool at_surface);
void set_bottomed(bool at_bottom); void set_bottomed(bool at_bottom);
bool init_arm_motors(bool arming_from_gcs); bool init_arm_motors(AP_Arming::ArmingMethod method);
void init_disarm_motors(); void init_disarm_motors();
void motors_output(); void motors_output();
Vector3f pv_location_to_vector(const Location& loc); Vector3f pv_location_to_vector(const Location& loc);

4
ArduSub/joystick.cpp

@ -130,11 +130,11 @@ void Sub::handle_jsbutton_press(uint8_t button, bool shift, bool held)
if (motors.armed()) { if (motors.armed()) {
init_disarm_motors(); init_disarm_motors();
} else { } else {
init_arm_motors(true); init_arm_motors(AP_Arming::ArmingMethod::MAVLINK);
} }
break; break;
case JSButton::button_function_t::k_arm: case JSButton::button_function_t::k_arm:
init_arm_motors(true); init_arm_motors(AP_Arming::ArmingMethod::MAVLINK);
break; break;
case JSButton::button_function_t::k_disarm: case JSButton::button_function_t::k_disarm:
init_disarm_motors(); init_disarm_motors();

2
ArduSub/motors.cpp

@ -8,7 +8,7 @@ void Sub::enable_motor_output()
// init_arm_motors - performs arming process including initialisation of barometer and gyros // init_arm_motors - performs arming process including initialisation of barometer and gyros
// returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure // returns false if arming failed because of pre-arm checks, arming checks or a gyro calibration failure
bool Sub::init_arm_motors(bool arming_from_gcs) bool Sub::init_arm_motors(AP_Arming::ArmingMethod method)
{ {
static bool in_arm_motors = false; static bool in_arm_motors = false;

Loading…
Cancel
Save