ardupilot & px4 书写自己的app & drivers (一)

此blog应该是连载，先记录下最简单的部分，添加将自己的app&drivers添加进px4和ardupilot系统中。

添加px4 app & dirvers

新建一个文件夹“my_app”，里面包含CmakeLists.txt和my_app_main.cpp

CmakeLists.txt里面的内容

px4_add_module(
    MODULE modules__my_app  //用my_app替换
    MAIN my_app             //用my_app替换
    COMPILE_FLAGS
    STACK_MAIN 1200
    SRCS
        my_app_main.cpp     //用my_app_main.cpp替换
    DEPENDS
        platforms__common
    )

my_app_main.cpp内容

#include <stdio.h>
#include <termios.h>
#include <unistd.h>
#include <stdbool.h>
#include <errno.h>
#include <drivers/drv_hrt.h>

extern "C" __EXPORT int my_app_main(int argc, char *argv[]);//注意添加extern "C" 

int my_app_main(int argc, char *argv[])
{

    printf("czyv587\n");
    return 0;
}

在nuttx_px4fmu-v2_default.cmake里面添加

modules/my_app

效果图

添加ardupilot dirvers

新建一个文件夹“my_drivers”，里面包含CmakeLists.txt、module.mk和my_dirvers.cpp

CmakeLists.txt里面的内容

px4_add_module(
    MODULE drivers__my_drivers  //用my_drivers替换
    MAIN my_drivers             //用my_drivers替换
    STACK 1200
    COMPILE_FLAGS
        -Weffc++
        -Os
    SRCS
        my_drivers.cpp          //用my_drivers.cpp替换
    DEPENDS
        platforms__common
    )

module.mk里面的内容

MODULE_COMMAND  = my_drivers   //用my_drivers替换

SRCS        = my_drivers.cpp   //用my_drivers.cpp替换

MODULE_STACKSIZE    = 1200

EXTRACXXFLAGS   = -Weffc++

MAXOPTIMIZATION  = -Os

my_dirvers.cpp里面的内容

#include <px4_config.h>

#include <syspes.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>

#include <nuttx/arch.h>
#include <nuttx/clock.h>

#include <board_config.h>
#include <drivers/drv_hrt.h>

#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>

/** driver 'main' command */
extern "C" { __EXPORT int my_drivers_main(int argc, char *argv[]); }//注意添加extern "C" 


int
my_drivers_main(int argc, char *argv[])
{
    printf("czyv587!!!");
    return 0;
}

px4_common.mk里面添加

MODULES     += drivers/my_drivers

效果图

控制fmu_led

输入一次指令“my_drivers”led亮，再输入一次指令“my_drivers”led灭……

#include <px4_config.h>
#include <stdint.h>
#include <stdbool.h>
#include <stddef.h>
#include <stdlib.h>
#include <semaphore.h>
#include <string.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <getopt.h>

#include <systemlib/perf_counter.h>
#include <systemlib/err.h>
#include <systemlib/conversions.h>

#include <nuttx/arch.h>
#include <nuttx/clock.h>

#include <board_config.h>
#include <drivers/drv_hrt.h>

#include <drivers/device/spi.h>
#include <drivers/device/ringbuffer.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <lib/conversion/rotation.h>

/** driver 'main' command */
extern "C" { __EXPORT int my_drivers_main(int argc, char *argv[]); }//注意添加extern "C" 

__BEGIN_DECLS
extern void led_init();
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS

int l_i=0;
int my_drivers_main(int argc, char *argv[])
{
    if(l_i==0)
    {
//      led_on(1);//led亮
        led_toggle(1);//led翻转
        l_i=1;
        printf("czy");
    }
    else if(l_i==1)
    {
//      led_off(1);//led灭
        led_toggle(1);//led翻转
        l_i=0;
        printf("v587!!!");
    }
    return 0;
}

其中led函数为

/**
 * @file px4fmu2_led.c
 *
 * PX4FMU LED backend.
 */
#include <px4_config.h>
#include <stdbool.h>
#include "stm32.h"
#include "board_config.h"
#include <arch/board/board.h>
/*
 * Ideally we'd be able to get these from up_internal.h,
 * but since we want to be able to disable the NuttX use
 * of leds for system indication at will and there is no
 * separate switch, we need to build independent of the
 * CONFIG_ARCH_LEDS configuration switch.
 */
__BEGIN_DECLS
extern void led_init(void);
extern void led_on(int led);
extern void led_off(int led);
extern void led_toggle(int led);
__END_DECLS

__EXPORT void led_init()
{
    /* Configure LED1 GPIO for output */

    stm32_configgpio(GPIO_LED1);
}

__EXPORT void led_on(int led)
{
    if (led == 1) {
        /* Pull down to switch on */
        stm32_gpiowrite(GPIO_LED1, false);
    }
}

__EXPORT void led_off(int led)
{
    if (led == 1) {
        /* Pull up to switch off */
        stm32_gpiowrite(GPIO_LED1, true);
    }
}

__EXPORT void led_toggle(int led)
{
    if (led == 1) {
        if (stm32_gpioread(GPIO_LED1)) {
            stm32_gpiowrite(GPIO_LED1, false);

        } else {
            stm32_gpiowrite(GPIO_LED1, true);
        }
    }
}

如果您觉得此文对您的发展有用，请随意打赏。
您的鼓励将是笔者书写高质量文章的最大动力^_^!!