Line data    Source code

       1             : #include "Sub.h"
       2             : #include "actuators.h"
       3             : 
       4             : 
       5             :  
       6             : const AP_Param::GroupInfo Actuators::var_info[] = {
       7             : 
       8             :     // @Param: 1_INC
       9             :     // @DisplayName: Increment step for actuator 1
      10             :     // @Description:  Initial increment step for changing the actuator's PWM
      11             :     // @Units: us
      12             :     // @User: Standard
      13             :     AP_GROUPINFO("1_INC", 1, Actuators, actuator_increment_step[0], ACTUATOR_DEFAULT_INCREMENT),
      14             : 
      15             :     // @Param: 2_INC
      16             :     // @DisplayName: Increment step for actuator 2
      17             :     // @Description:  Initial increment step for changing the actuator's PWM
      18             :     // @Units: us
      19             :     // @User: Standard
      20             :     AP_GROUPINFO("2_INC", 2, Actuators, actuator_increment_step[1], ACTUATOR_DEFAULT_INCREMENT),
      21             : 
      22             :     // @Param: 3_INC
      23             :     // @DisplayName: Increment step for actuator 3
      24             :     // @Description:  Initial increment step for changing the actuator's PWM
      25             :     // @Units: us
      26             :     // @User: Standard
      27             :     AP_GROUPINFO("3_INC", 3, Actuators, actuator_increment_step[2], ACTUATOR_DEFAULT_INCREMENT),
      28             : 
      29             :     // @Param: 4_INC
      30             :     // @DisplayName: Increment step for actuator 4
      31             :     // @Description:  Initial increment step for changing the actuator's PWM
      32             :     // @Units: us
      33             :     // @User: Standard
      34             :     AP_GROUPINFO("4_INC", 4, Actuators, actuator_increment_step[3], ACTUATOR_DEFAULT_INCREMENT),
      35             : 
      36             :     // @Param: 5_INC
      37             :     // @DisplayName: Increment step for actuator 5
      38             :     // @Description:  Initial increment step for changing the actuator's PWM
      39             :     // @Units: us
      40             :     // @User: Standard
      41             :     AP_GROUPINFO("5_INC", 5, Actuators, actuator_increment_step[4], ACTUATOR_DEFAULT_INCREMENT),
      42             : 
      43             :     // @Param: 6_INC
      44             :     // @DisplayName: Increment step for actuator 6
      45             :     // @Description:  Initial increment step for changing the actuator's PWM
      46             :     // @Units: us
      47             :     // @User: Standard
      48             :     AP_GROUPINFO("6_INC", 6, Actuators, actuator_increment_step[5], ACTUATOR_DEFAULT_INCREMENT),
      49             : 
      50             :     AP_GROUPEND
      51             : };
      52             : 
      53         581 : Actuators::Actuators() {
      54          83 :     AP_Param::setup_object_defaults(this, var_info);
      55          83 : }
      56             : 
      57             : 
      58          82 : void Actuators::initialize_actuators() {
      59          82 :     const SRV_Channel::Function first_aux = SRV_Channel::Function::k_actuator1;
      60         574 :     for (int i = 0; i < ACTUATOR_CHANNELS; i++) {
      61             :         uint8_t channel_number;
      62         492 :         if (!SRV_Channels::find_channel((SRV_Channel::Function)(first_aux + i), channel_number)) {
      63         492 :             continue;
      64             :         }
      65           0 :         SRV_Channel* chan = SRV_Channels::srv_channel(channel_number);
      66           0 :         uint16_t servo_min = chan->get_output_min();
      67           0 :         uint16_t servo_max = chan->get_output_max();
      68           0 :         uint16_t servo_range = servo_max - servo_min;
      69             : 
      70           0 :         uint16_t servo_trim = chan->get_trim();
      71             :         // set current value to trim
      72           0 :         aux_actuator_value[i] = (servo_trim - servo_min) / static_cast<float>(servo_range);
      73             :     }
      74          82 :     update_actuators();
      75          82 : }
      76             : 
      77     2180765 : void Actuators::update_actuators() {
      78     2180765 :     const SRV_Channel::Function first_aux = SRV_Channel::Function::k_actuator1;
      79    15265355 :     for (int i = 0; i < ACTUATOR_CHANNELS; i++) {
      80             :         uint8_t channel_number;
      81    13084590 :         if (!SRV_Channels::find_channel((SRV_Channel::Function)(first_aux + i), channel_number)) {
      82    13084590 :             continue;
      83             :         }
      84           0 :         SRV_Channel* chan = SRV_Channels::srv_channel(channel_number);
      85           0 :         uint16_t servo_min = chan->get_output_min();
      86           0 :         uint16_t servo_max = chan->get_output_max();
      87           0 :         uint16_t servo_range = servo_max - servo_min;
      88           0 :         chan->set_output_pwm(servo_min + servo_range * aux_actuator_value[i]);
      89             :     }
      90     2180765 : }
      91             : 
      92           0 : void Actuators::increase_actuator(uint8_t actuator_num) {
      93           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
      94           0 :         return;
      95             :     }
      96           0 :     aux_actuator_value[actuator_num] = constrain_float(aux_actuator_value[actuator_num] + actuator_increment_step[actuator_num], 0.0f, 1.0f);
      97             : }
      98             : 
      99           0 : void Actuators::decrease_actuator(uint8_t actuator_num) {
     100           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     101           0 :         return;
     102             :     }
     103           0 :     aux_actuator_value[actuator_num] = constrain_float(aux_actuator_value[actuator_num] - actuator_increment_step[actuator_num], 0.0f, 1.0f);
     104             : }
     105             : 
     106           0 : void Actuators::min_actuator(uint8_t actuator_num) {
     107           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     108           0 :         return;
     109             :     }
     110           0 :     aux_actuator_value[actuator_num] = 0;
     111             : }
     112             : 
     113           0 : void Actuators::max_actuator(uint8_t actuator_num) {
     114           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     115           0 :         return;
     116             :     }
     117           0 :     aux_actuator_value[actuator_num] = 1;
     118             : }
     119             : 
     120           0 : void Actuators::min_toggle_actuator(uint8_t actuator_num) {
     121           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     122           0 :         return;
     123             :     }
     124           0 :     if (aux_actuator_value[actuator_num] < 0.4) {
     125           0 :         aux_actuator_value[actuator_num] = 0.5;
     126             :     } else {
     127           0 :         aux_actuator_value[actuator_num] = 0;
     128             :     }
     129             : }
     130             : 
     131           0 : void Actuators::max_toggle_actuator(uint8_t actuator_num) {
     132           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     133           0 :         return;
     134             :     }
     135           0 :     if (aux_actuator_value[actuator_num] >= 0.6) {
     136           0 :         aux_actuator_value[actuator_num] = 0.5;
     137             :     } else {
     138           0 :         aux_actuator_value[actuator_num] = 1;
     139             :     }
     140             : }
     141             : 
     142           0 : void Actuators::center_actuator(uint8_t actuator_num) {
     143           0 :     if (actuator_num >= ACTUATOR_CHANNELS) {
     144           0 :         return;
     145             :     }
     146           0 :     aux_actuator_value[actuator_num] = 0.5;
     147             : }