From 0a9d1f1fb3656f22fb2f19c0bb0b5c47806ce649 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 25 Dec 2021 01:21:36 +0900 Subject: [PATCH 1/3] GCS_MAVLink: Fix #19596 and MAV_CMD_DO_REPEAT_RELAY --- libraries/GCS_MAVLink/GCS_ServoRelay.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/GCS_MAVLink/GCS_ServoRelay.cpp b/libraries/GCS_MAVLink/GCS_ServoRelay.cpp index e2e8a24a1d706..ef8ce51ed85b4 100644 --- a/libraries/GCS_MAVLink/GCS_ServoRelay.cpp +++ b/libraries/GCS_MAVLink/GCS_ServoRelay.cpp @@ -19,7 +19,7 @@ MAV_RESULT GCS_MAVLINK::handle_servorelay_message(const mavlink_command_long_t & break; case MAV_CMD_DO_REPEAT_SERVO: - if (handler->do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4 * 1000)) { + if (handler->do_repeat_servo(packet.param1, packet.param2, packet.param3, packet.param4)) { result = MAV_RESULT_ACCEPTED; } break; @@ -31,7 +31,7 @@ MAV_RESULT GCS_MAVLINK::handle_servorelay_message(const mavlink_command_long_t & break; case MAV_CMD_DO_REPEAT_RELAY: - if (handler->do_repeat_relay(packet.param1, packet.param2, packet.param3 * 1000)) { + if (handler->do_repeat_relay(packet.param1, packet.param2, packet.param3)) { result = MAV_RESULT_ACCEPTED; } break; From 51b56420b247414bbb2b6510d3ae0cb9e05b7ace Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 25 Dec 2021 01:22:25 +0900 Subject: [PATCH 2/3] AP_ServoRelayEvents: Fix #19596 and MAV_CMD_DO_REPEAT_RELAY --- libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp | 8 ++++---- libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp index d5484d01b5eb6..ca6ae3db2a284 100644 --- a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp +++ b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.cpp @@ -81,7 +81,7 @@ bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state) } bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value, - int16_t _repeat, uint16_t _delay_ms) + int16_t _repeat, uint16_t _delay_s) { SRV_Channel *c = SRV_Channels::srv_channel(_channel-1); if (c == nullptr) { @@ -104,14 +104,14 @@ bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_valu type = EVENT_TYPE_SERVO; start_time_ms = 0; - delay_ms = _delay_ms / 2; + delay_ms = uint32_t(_delay_s) * 1000 / 2; repeat = _repeat * 2; servo_value = _servo_value; update_events(); return true; } -bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms) +bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint16_t _delay_s) { AP_Relay *relay = AP::relay(); if (relay == nullptr) { @@ -123,7 +123,7 @@ bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, ui type = EVENT_TYPE_RELAY; channel = relay_num; start_time_ms = 0; - delay_ms = _delay_ms/2; // half cycle time + delay_ms = uint32_t(_delay_s) * 1000 / 2; // half cycle time repeat = _repeat*2; // number of full cycles update_events(); return true; diff --git a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h index 5faa9fe8f42cf..b05cc798c5834 100644 --- a/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h +++ b/libraries/AP_ServoRelayEvents/AP_ServoRelayEvents.h @@ -28,8 +28,8 @@ class AP_ServoRelayEvents { bool do_set_servo(uint8_t channel, uint16_t pwm); bool do_set_relay(uint8_t relay_num, uint8_t state); - bool do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_ms); - bool do_repeat_relay(uint8_t relay_num, int16_t count, uint32_t period_ms); + bool do_repeat_servo(uint8_t channel, uint16_t servo_value, int16_t repeat, uint16_t delay_time_s); + bool do_repeat_relay(uint8_t relay_num, int16_t count, uint16_t period_s); void update_events(void); private: @@ -48,7 +48,7 @@ class AP_ServoRelayEvents { uint32_t start_time_ms; // how long to delay the next firing of event in millis - uint16_t delay_ms; + uint32_t delay_ms; // how many times to cycle : -1 (or -2) = forever, 2 = do one cycle, 4 = do two cycles int16_t repeat; From 68626f6e47a4f3a88758e938f63f6420375c03e8 Mon Sep 17 00:00:00 2001 From: murata Date: Sat, 25 Dec 2021 01:23:36 +0900 Subject: [PATCH 3/3] AP_Mission: Fix #19596 and MAV_CMD_DO_REPEAT_RELAY --- libraries/AP_Mission/AP_Mission_Commands.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Mission/AP_Mission_Commands.cpp b/libraries/AP_Mission/AP_Mission_Commands.cpp index 2bc7bc0cb0140..8a6b05d02fb28 100644 --- a/libraries/AP_Mission/AP_Mission_Commands.cpp +++ b/libraries/AP_Mission/AP_Mission_Commands.cpp @@ -73,12 +73,12 @@ bool AP_Mission::start_command_do_servorelayevents(const AP_Mission::Mission_Com return sre->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); + cmd.content.repeat_servo.cycle_time); case MAV_CMD_DO_REPEAT_RELAY: return sre->do_repeat_relay(cmd.content.repeat_relay.num, cmd.content.repeat_relay.repeat_count, - cmd.content.repeat_relay.cycle_time * 1000.0f); + cmd.content.repeat_relay.cycle_time); default: #if CONFIG_HAL_BOARD == HAL_BOARD_SITL AP_HAL::panic("Unhandled servo/relay case");