Skip to content

Commit bbb0635

Browse files
committed
kinetis:Add kinetis_gpiosetevent in PX4 layer
Adds a compatible gpiosetevent interface. The helper call the low level functions while providing a consistent API with xxxx_ gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg); This wrapper was rjected upstream.
1 parent 6ea8a23 commit bbb0635

File tree

4 files changed

+95
-2
lines changed

4 files changed

+95
-2
lines changed

platforms/nuttx/src/px4/nxp/k66/include/px4_arch/micro_hal.h

+3-1
Original file line numberDiff line numberDiff line change
@@ -105,7 +105,9 @@ __BEGIN_DECLS
105105
#define px4_arch_gpioread(pinset) kinetis_gpioread(pinset)
106106
#define px4_arch_gpiowrite(pinset, value) kinetis_gpiowrite(pinset, value)
107107

108-
/* kinetis_gpiosetevent is not implemented and will need to be added */
108+
/* kinetis_gpiosetevent is added at PX4 level */
109+
110+
int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge, bool event, xcpt_t func, void *arg);
109111

110112
#define px4_arch_gpiosetevent(pinset,r,f,e,fp,a) kinetis_gpiosetevent(pinset,r,f,e,fp,a)
111113

platforms/nuttx/src/px4/nxp/kinetis/io_pins/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -36,4 +36,5 @@ px4_add_library(arch_io_pins
3636
pwm_servo.c
3737
pwm_trigger.c
3838
input_capture.c
39+
kinetis_pinirq.c
3940
)
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
/****************************************************************************
2+
*
3+
* Copyright (C) 2020 PX4 Development Team. All rights reserved.
4+
*
5+
* Redistribution and use in source and binary forms, with or without
6+
* modification, are permitted provided that the following conditions
7+
* are met:
8+
*
9+
* 1. Redistributions of source code must retain the above copyright
10+
* notice, this list of conditions and the following disclaimer.
11+
* 2. Redistributions in binary form must reproduce the above copyright
12+
* notice, this list of conditions and the following disclaimer in
13+
* the documentation and/or other materials provided with the
14+
* distribution.
15+
* 3. Neither the name PX4 nor the names of its contributors may be
16+
* used to endorse or promote products derived from this software
17+
* without specific prior written permission.
18+
*
19+
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20+
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21+
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22+
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23+
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24+
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25+
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26+
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27+
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28+
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29+
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30+
* POSSIBILITY OF SUCH DAMAGE.
31+
*
32+
****************************************************************************/
33+
34+
#include <px4_platform_common/px4_config.h>
35+
#include <systemlib/px4_macros.h>
36+
37+
#include <arch/board/board.h>
38+
39+
#include <errno.h>
40+
41+
#include "kinetis.h"
42+
#include "hardware/kinetis_port.h"
43+
44+
/****************************************************************************
45+
* Name: kinetis_gpiosetevent
46+
*
47+
* Description:
48+
* Sets/clears GPIO based event and interrupt triggers.
49+
*
50+
* Input Parameters:
51+
* - pinset: gpio pin configuration
52+
* - rising/falling edge: enables
53+
* - event: generate event when set
54+
* - func: when non-NULL, generate interrupt
55+
* - arg: Argument passed to the interrupt callback
56+
*
57+
* Returned Value:
58+
* Zero (OK) on success; a negated errno value on failure indicating the
59+
* nature of the failure.
60+
*
61+
****************************************************************************/
62+
#if defined(CONFIG_KINETIS_GPIOIRQ)
63+
int kinetis_gpiosetevent(uint32_t pinset, bool risingedge, bool fallingedge,
64+
bool event, xcpt_t func, void *arg)
65+
{
66+
int ret = -ENOSYS;
67+
68+
if (func == NULL) {
69+
kinetis_pinirqdisable(pinset);
70+
ret = kinetis_pinirqattach(pinset, NULL, NULL);
71+
72+
} else {
73+
ret = kinetis_pinirqattach(pinset, func, arg);
74+
pinset &= ~_PIN_INT_MASK
75+
DEBUGASSERT(port < KINETIS_NPORTS);
76+
77+
if (risingedge) {
78+
pinset |= PIN_INT_RISING;
79+
}
80+
81+
if (fallingedge) {
82+
pinset |= PIN_INT_FALLING;
83+
}
84+
85+
kinetis_pinirqenable(pinset);
86+
}
87+
88+
return ret;
89+
}
90+
#endif /* CONFIG_KINETIS_GPIOIRQ */

0 commit comments

Comments
 (0)