CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/Tools/AP_Periph/hardpoint.cpp
Views: 1862
1
#include "AP_Periph.h"

2


3
#ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT

4


5
#include <AP_HAL/AP_HAL.h>

6


7
extern const AP_HAL::HAL &hal;

8


9
/*

10
  hardpoint support

11
 */

12


13
#include <dronecan_msgs.h>

14


15
void AP_Periph_FW::pwm_hardpoint_init()

16
{

17
    hal.gpio->attach_interrupt(

18
        PWM_HARDPOINT_PIN,

19
        FUNCTOR_BIND_MEMBER(&AP_Periph_FW::pwm_irq_handler, void, uint8_t, bool, uint32_t), AP_HAL::GPIO::INTERRUPT_BOTH);

20
}

21


22
/*

23
  called on PWM pin transition

24
 */

25
void AP_Periph_FW::pwm_irq_handler(uint8_t pin, bool pin_state, uint32_t timestamp)

26
{

27
    if (pin_state == 0 && pwm_hardpoint.last_state == 1 && pwm_hardpoint.last_ts_us != 0) {

28
        uint32_t width = timestamp - pwm_hardpoint.last_ts_us;

29
        if (width > 500 && width < 2500) {

30
            pwm_hardpoint.pwm_value = width;

31
            if (width > pwm_hardpoint.highest_pwm) {

32
                pwm_hardpoint.highest_pwm = width;

33
            }

34
        }

35
    }

36
    pwm_hardpoint.last_state = pin_state;

37
    pwm_hardpoint.last_ts_us = timestamp;

38
}

39


40
void AP_Periph_FW::pwm_hardpoint_update()

41
{

42
    uint32_t now = AP_HAL::millis();

43
    // send at 10Hz

44
    void *save = hal.scheduler->disable_interrupts_save();

45
    uint16_t value = pwm_hardpoint.highest_pwm;

46
    pwm_hardpoint.highest_pwm = 0;

47
    hal.scheduler->restore_interrupts(save);

48
    float rate = g.hardpoint_rate;

49
    rate = constrain_float(rate, 10, 100);

50
    if (value > 0 && now - pwm_hardpoint.last_send_ms >= 1000U/rate) {

51
        pwm_hardpoint.last_send_ms = now;

52
        uavcan_equipment_hardpoint_Command cmd {};

53
        cmd.hardpoint_id = g.hardpoint_id;

54
        cmd.command = value;

55


56
        uint8_t buffer[UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_MAX_SIZE];

57
        uint16_t total_size = uavcan_equipment_hardpoint_Command_encode(&cmd, buffer, !canfdout());

58
        canard_broadcast(UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_SIGNATURE,

59
                        UAVCAN_EQUIPMENT_HARDPOINT_COMMAND_ID,

60
                        CANARD_TRANSFER_PRIORITY_LOW,

61
                        &buffer[0],

62
                        total_size);

63
    }

64
}

65


66
#endif // HAL_PERIPH_ENABLE_PWM_HARDPOINT

67


68