Line data    Source code

       1             : /*
       2             :    This program is free software: you can redistribute it and/or modify
       3             :    it under the terms of the GNU General Public License as published by
       4             :    the Free Software Foundation, either version 3 of the License, or
       5             :    (at your option) any later version.
       6             : 
       7             :    This program is distributed in the hope that it will be useful,
       8             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
       9             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      10             :    GNU General Public License for more details.
      11             : 
      12             :    You should have received a copy of the GNU General Public License
      13             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      14             :  */
      15             : /*
      16             :  *   AP_ServoRelayEvents - handle servo and relay MAVLink events
      17             :  */
      18             : 
      19             : #include "AP_ServoRelayEvents_config.h"
      20             : 
      21             : #if AP_SERVORELAYEVENTS_ENABLED
      22             : 
      23             : #include <AP_HAL/AP_HAL.h>
      24             : #include <AP_Common/AP_Common.h>
      25             : #include "AP_ServoRelayEvents.h"
      26             : #include <RC_Channel/RC_Channel.h>
      27             : #include <SRV_Channel/SRV_Channel.h>
      28             : #include <GCS_MAVLink/GCS.h>
      29             : 
      30             : extern const AP_HAL::HAL& hal;
      31             : 
      32         775 : bool AP_ServoRelayEvents::do_set_servo(uint8_t _channel, uint16_t pwm)
      33             : {
      34         775 :     SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
      35         775 :     if (c == nullptr) {
      36           0 :         return false;
      37             :     }
      38         775 :     switch(c->get_function())
      39             :     {
      40         775 :     case SRV_Channel::k_none:
      41             :     case SRV_Channel::k_manual:
      42             :     case SRV_Channel::k_sprayer_pump:
      43             :     case SRV_Channel::k_sprayer_spinner:
      44             :     case SRV_Channel::k_gripper:
      45             :     case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
      46         775 :         break;
      47           0 :     case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: {
      48             :         // mapped channels are set up with a -/+ 4500 angle range by
      49             :         // SRV_Channel::aux_servo_function_setup
      50           0 :         int16_t angle_scaled = constrain_uint16(pwm, 1000, 2000);
      51           0 :         angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500
      52           0 :         pwm = c->pwm_from_scaled_value(angle_scaled);
      53           0 :         break;
      54             :     }
      55           0 :     default:
      56           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
      57           0 :         return false;
      58             :     }
      59         775 :     if (type == EVENT_TYPE_SERVO && 
      60           0 :         channel == _channel) {
      61             :         // cancel previous repeat
      62           0 :         repeat = 0;
      63             :     }
      64         775 :     c->set_output_pwm(pwm);
      65         775 :     c->ignore_small_rcin_changes();
      66         775 :     return true;
      67             : }
      68             : 
      69             : #if AP_RELAY_ENABLED
      70          49 : bool AP_ServoRelayEvents::do_set_relay(uint8_t relay_num, uint8_t state)
      71             : {
      72          49 :     AP_Relay *relay = AP::relay();
      73          49 :     if (relay == nullptr) {
      74           0 :         return false;
      75             :     }
      76             : 
      77          49 :     if (!relay->enabled(relay_num)) {
      78           0 :         return false;
      79             :     }
      80          49 :     if (type == EVENT_TYPE_RELAY && 
      81          49 :         channel == relay_num) {
      82             :         // cancel previous repeat
      83          33 :         repeat = 0;
      84             :     }
      85          49 :     if (state == 1) {
      86          23 :         relay->on(relay_num);
      87          26 :     } else if (state == 0) {
      88          26 :         relay->off(relay_num);
      89             :     } else {
      90           0 :         relay->toggle(relay_num);
      91             :     }
      92          49 :     return true;
      93             : }
      94             : #endif
      95             : 
      96           4 : bool AP_ServoRelayEvents::do_repeat_servo(uint8_t _channel, uint16_t _servo_value, 
      97             :                                           int16_t _repeat, uint16_t _delay_ms)
      98             : {
      99           4 :     SRV_Channel *c = SRV_Channels::srv_channel(_channel-1);
     100           4 :     if (c == nullptr) {
     101           0 :         return false;
     102             :     }
     103           4 :     switch(c->get_function())
     104             :     {
     105           4 :     case SRV_Channel::k_none:
     106             :     case SRV_Channel::k_manual:
     107             :     case SRV_Channel::k_sprayer_pump:
     108             :     case SRV_Channel::k_sprayer_spinner:
     109             :     case SRV_Channel::k_gripper:
     110             :     case SRV_Channel::k_rcin1 ... SRV_Channel::k_rcin16: // rc pass-thru
     111           4 :         break;
     112           0 :     case SRV_Channel::k_rcin1_mapped ... SRV_Channel::k_rcin16_mapped: {
     113             :         // mapped channels are set up with a -/+ 4500 angle range by
     114             :         // SRV_Channel::aux_servo_function_setup
     115           0 :         int16_t angle_scaled = constrain_uint16(_servo_value, 1000, 2000);
     116           0 :         angle_scaled = (angle_scaled - 1500) * 9; // 1000 ... 2000 -> -500 ... 500 -> -4500 ... 4500
     117           0 :         _servo_value = c->pwm_from_scaled_value(angle_scaled);
     118           0 :         break;
     119             :     }
     120           0 :     default:
     121           0 :         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "ServoRelayEvent: Channel %d is already in use", _channel);
     122           0 :         return false;
     123             :     }
     124           4 :     channel = _channel;
     125           4 :     type = EVENT_TYPE_SERVO;
     126             : 
     127           4 :     start_time_ms  = 0;
     128           4 :     delay_ms    = _delay_ms / 2;
     129           4 :     repeat      = _repeat * 2;
     130           4 :     servo_value = _servo_value;
     131           4 :     update_events();
     132           4 :     return true;
     133             : }
     134             : 
     135             : #if AP_RELAY_ENABLED
     136           4 : bool AP_ServoRelayEvents::do_repeat_relay(uint8_t relay_num, int16_t _repeat, uint32_t _delay_ms)
     137             : {
     138           4 :     AP_Relay *relay = AP::relay();
     139           4 :     if (relay == nullptr) {
     140           0 :         return false;
     141             :     }
     142           4 :     if (!relay->enabled(relay_num)) {
     143           0 :         return false;
     144             :     }
     145           4 :     type = EVENT_TYPE_RELAY;
     146           4 :     channel = relay_num;
     147           4 :     start_time_ms  = 0;
     148           4 :     delay_ms        = _delay_ms/2; // half cycle time
     149           4 :     repeat          = _repeat*2;  // number of full cycles
     150           4 :     update_events();
     151           4 :     return true;
     152             : }
     153             : #endif
     154             : 
     155             : 
     156             : /*
     157             :   update state for MAV_CMD_DO_REPEAT_SERVO and MAV_CMD_DO_REPEAT_RELAY
     158             : */
     159     7182448 : void AP_ServoRelayEvents::update_events(void)
     160             : {
     161     7182448 :     if (repeat == 0 || (AP_HAL::millis() - start_time_ms) < delay_ms) {
     162     7182368 :         return;
     163             :     }
     164             : 
     165          80 :     start_time_ms = AP_HAL::millis();
     166             : 
     167          80 :     switch (type) {
     168          40 :     case EVENT_TYPE_SERVO: {
     169          40 :         SRV_Channel *c = SRV_Channels::srv_channel(channel-1);
     170          40 :         if (c != nullptr) {
     171          40 :             if (repeat & 1) {
     172          20 :                 c->set_output_pwm(c->get_trim());
     173             :             } else {
     174          20 :                 c->set_output_pwm(servo_value);
     175          20 :                 c->ignore_small_rcin_changes();
     176             :             }
     177             :         }
     178          40 :         break;
     179             :     }
     180             : 
     181             : #if AP_RELAY_ENABLED
     182          40 :     case EVENT_TYPE_RELAY: {
     183          40 :         AP_Relay *relay = AP::relay();
     184          40 :         if (relay != nullptr) {
     185          40 :             relay->toggle(channel);
     186             :         }
     187          40 :         break;
     188             :     }
     189             : #endif
     190             :     }
     191          80 :     if (repeat > 0) {
     192          80 :         repeat--;
     193             :     } else {
     194             :         // toggle bottom bit so servos flip in value
     195           0 :         repeat ^= 1;
     196             :     }
     197             : }
     198             : 
     199             : // singleton instance
     200             : AP_ServoRelayEvents *AP_ServoRelayEvents::_singleton;
     201             : 
     202             : namespace AP {
     203             : 
     204         832 : AP_ServoRelayEvents *servorelayevents()
     205             : {
     206         832 :     return AP_ServoRelayEvents::get_singleton();
     207             : }
     208             : 
     209             : }
     210             : 
     211             : #endif  // AP_SERVORELAYEVENTS_ENABLED