fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-20 05:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

src/platforms/qurt: move to src/drivers

Browse Source
This commit is contained in:
Beat Küng
2019-08-30 07:59:44 +02:00
parent 3bdfd8ce8d
commit 8cbb3852ba
21 changed files with 0 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/platforms/qurt/fc_addon/mpu_spi/CMakeLists.txt
-58
View File
@@ -1,58 +0,0 @@
############################################################################
#
# Copyright (C) 2015 Mark Charlebois. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(TOOLS_ERROR_MSG
"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
if ("$ENV{FC_ADDON}" STREQUAL "")
message(FATAL_ERROR ${TOOLS_ERROR_MSG})
else()
set(FC_ADDON $ENV{FC_ADDON})
endif()
add_library(mpu9x50 SHARED IMPORTED GLOBAL)
set_target_properties(mpu9x50 PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libmpu9x50.so)
px4_add_module(
MODULE platforms__qurt__fc_addon__mpu_spi
MAIN mpu9x50
STACK_MAIN 1200
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
mpu9x50_main.cpp
mpu9x50_params.c
DEPENDS
)
target_link_libraries(platforms__qurt__fc_addon__mpu_spi PRIVATE mpu9x50)
src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_main.cpp
-662
View File
@@ -1,662 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <stdint.h>
#include <fcntl.h>
#include <unistd.h>
#include <signal.h>
#include <px4_tasks.h>
#include <px4_log.h>
#include <px4_getopt.h>
// TODO-JYW:
// Include references to the driver framework. This will produce a duplicate definition
// of qurt_log. Use a #define for the qurt_log function to avoid redefinition. This code
// change must still be made. Document the latter change to be clear.
// #include <df_stubs.h>
// #include <SPIDevObj.hpp>
#include <uORB/uORB.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_accel.h>
#include <drivers/drv_gyro.h>
#include <drivers/drv_mag.h>
#include <drivers/drv_hrt.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <semaphore.h>
#include <mpu9x50.h>
#ifdef __cplusplus
}
#endif
/** driver 'main' command */
extern "C" { __EXPORT int mpu9x50_main(int argc, char *argv[]); }
#define MAX_LEN_DEV_PATH 32
// TODO - need to load this from parameter file
#define SPI_INT_GPIO 65 // GPIO pin for Data Ready Interrupt
namespace mpu9x50
{
/** SPI device path that mpu9x50 is connected to */
static char _device[MAX_LEN_DEV_PATH];
/** flag indicating if mpu9x50 app is running */
static bool _is_running = false;
/** flag indicating if measurement thread should exit */
static bool _task_should_exit = false;
/** handle to the task main thread */
static px4_task_t _task_handle = -1;
/** IMU measurement data */
static struct mpu9x50_data _data;
static orb_advert_t _gyro_pub = nullptr; /**< gyro data publication */
static int _gyro_orb_class_instance; /**< instance handle for gyro devices */
static orb_advert_t _accel_pub = nullptr; /**< accelerameter data publication */
static int _accel_orb_class_instance; /**< instance handle for accel devices */
static orb_advert_t _mag_pub = nullptr; /**< compass data publication */
static int _mag_orb_class_instance; /**< instance handle for mag devices */
static int _params_sub; /**< parameter update subscription */
static sensor_gyro_s _gyro; /**< gyro report */
static sensor_accel_s _accel; /**< accel report */
static struct mag_report _mag; /**< mag report */
static struct gyro_calibration_s _gyro_sc; /**< gyro scale */
static struct accel_calibration_s _accel_sc; /**< accel scale */
static struct mag_calibration_s _mag_sc; /**< mag scale */
static enum gyro_lpf_e _gyro_lpf = MPU9X50_GYRO_LPF_20HZ; /**< gyro lpf enum value */
static enum acc_lpf_e _accel_lpf = MPU9X50_ACC_LPF_20HZ; /**< accel lpf enum value */
static enum gyro_sample_rate_e _imu_sample_rate = MPU9x50_SAMPLE_RATE_500HZ; /**< IMU sample rate enum */
struct {
param_t accel_x_offset;
param_t accel_x_scale;
param_t accel_y_offset;
param_t accel_y_scale;
param_t accel_z_offset;
param_t accel_z_scale;
param_t gyro_x_offset;
param_t gyro_x_scale;
param_t gyro_y_offset;
param_t gyro_y_scale;
param_t gyro_z_offset;
param_t gyro_z_scale;
param_t mag_x_offset;
param_t mag_x_scale;
param_t mag_y_offset;
param_t mag_y_scale;
param_t mag_z_offset;
param_t mag_z_scale;
param_t gyro_lpf_enum;
param_t accel_lpf_enum;
param_t imu_sample_rate_enum;
} _params_handles; /**< parameter handles */
bool exit_mreasurement = false;
/** Print out the usage information */
static void usage();
/** mpu9x50 stop measurement */
static void stop();
/** task main trampoline function */
static void task_main_trampoline(int argc, char *argv[]);
/** mpu9x50 measurement thread primary entry point */
static void task_main(int argc, char *argv[]);
/**
* create and advertise all publicatoins
* @return true on success, false otherwise
*/
static bool create_pubs();
/** update all sensor reports with the latest sensor reading */
static void update_reports();
/** publish all sensor reports */
static void publish_reports();
/** update all parameters */
static void parameters_update();
/** initialize all parameter handles and load the initial parameter values */
static void parameters_init();
/** poll parameter update */
static void parameter_update_poll();
static int64_t _isr_data_ready_timestamp = 0;
/**
* MPU9x50 data ready interrupt service routine
* @param[out] context address of the context data provided by user whill
* registering the interrupt servcie routine
*/
static void *data_ready_isr(void *context);
void *data_ready_isr(void *context)
{
if (exit_mreasurement) {
return NULL;
}
_isr_data_ready_timestamp = hrt_absolute_time();
// send signal to measurement thread
px4_task_kill(_task_handle, SIGRTMIN);
return NULL;
}
void parameter_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
void parameters_update()
{
PX4_DEBUG("mpu9x50_parameters_update");
float v;
int v_int;
// accel params
if (param_get(_params_handles.accel_x_offset, &v) == 0) {
_accel_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_x_offset %f", v);
}
if (param_get(_params_handles.accel_x_scale, &v) == 0) {
_accel_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_x_scale %f", v);
}
if (param_get(_params_handles.accel_y_offset, &v) == 0) {
_accel_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_y_offset %f", v);
}
if (param_get(_params_handles.accel_y_scale, &v) == 0) {
_accel_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_y_scale %f", v);
}
if (param_get(_params_handles.accel_z_offset, &v) == 0) {
_accel_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update accel_z_offset %f", v);
}
if (param_get(_params_handles.accel_z_scale, &v) == 0) {
_accel_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update accel_z_scale %f", v);
}
// gyro params
if (param_get(_params_handles.gyro_x_offset, &v) == 0) {
_gyro_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_x_offset %f", v);
}
if (param_get(_params_handles.gyro_x_scale, &v) == 0) {
_gyro_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_x_scale %f", v);
}
if (param_get(_params_handles.gyro_y_offset, &v) == 0) {
_gyro_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_y_offset %f", v);
}
if (param_get(_params_handles.gyro_y_scale, &v) == 0) {
_gyro_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_y_scale %f", v);
}
if (param_get(_params_handles.gyro_z_offset, &v) == 0) {
_gyro_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_z_offset %f", v);
}
if (param_get(_params_handles.gyro_z_scale, &v) == 0) {
_gyro_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update gyro_z_scale %f", v);
}
// mag params
if (param_get(_params_handles.mag_x_offset, &v) == 0) {
_mag_sc.x_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_x_offset %f", v);
}
if (param_get(_params_handles.mag_x_scale, &v) == 0) {
_mag_sc.x_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_x_scale %f", v);
}
if (param_get(_params_handles.mag_y_offset, &v) == 0) {
_mag_sc.y_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_y_offset %f", v);
}
if (param_get(_params_handles.mag_y_scale, &v) == 0) {
_mag_sc.y_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_y_scale %f", v);
}
if (param_get(_params_handles.mag_z_offset, &v) == 0) {
_mag_sc.z_offset = v;
PX4_DEBUG("mpu9x50_parameters_update mag_z_offset %f", v);
}
if (param_get(_params_handles.mag_z_scale, &v) == 0) {
_mag_sc.z_scale = v;
PX4_DEBUG("mpu9x50_parameters_update mag_z_scale %f", v);
}
// LPF params
if (param_get(_params_handles.gyro_lpf_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_GYRO_LPF) {
PX4_WARN("invalid gyro_lpf_enum %d use default %d", v_int, _gyro_lpf);
} else {
_gyro_lpf = (enum gyro_lpf_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update gyro_lpf_enum %d", _gyro_lpf);
}
}
if (param_get(_params_handles.accel_lpf_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_ACC_LPF) {
PX4_WARN("invalid accel_lpf_enum %d use default %d", v_int, _accel_lpf);
} else {
_accel_lpf = (enum acc_lpf_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update accel_lpf_enum %d", _accel_lpf);
}
}
if (param_get(_params_handles.imu_sample_rate_enum, &v_int) == 0) {
if (v_int >= NUM_MPU9X50_SAMPLE_RATE) {
PX4_WARN("invalid imu_sample_rate %d use default %d", v_int, _imu_sample_rate);
} else {
_imu_sample_rate = (enum gyro_sample_rate_e)v_int;
PX4_DEBUG("mpu9x50_parameters_update imu_sample_rate %d", _imu_sample_rate);
}
}
}
void parameters_init()
{
_params_handles.accel_x_offset = param_find("CAL_ACC0_XOFF");
_params_handles.accel_x_scale = param_find("CAL_ACC0_XSCALE");
_params_handles.accel_y_offset = param_find("CAL_ACC0_YOFF");
_params_handles.accel_y_scale = param_find("CAL_ACC0_YSCALE");
_params_handles.accel_z_offset = param_find("CAL_ACC0_ZOFF");
_params_handles.accel_z_scale = param_find("CAL_ACC0_ZSCALE");
_params_handles.gyro_x_offset = param_find("CAL_GYRO0_XOFF");
_params_handles.gyro_x_scale = param_find("CAL_GYRO0_XSCALE");
_params_handles.gyro_y_offset = param_find("CAL_GYRO0_YOFF");
_params_handles.gyro_y_scale = param_find("CAL_GYRO0_YSCALE");
_params_handles.gyro_z_offset = param_find("CAL_GYRO0_ZOFF");
_params_handles.gyro_z_scale = param_find("CAL_GYRO0_ZSCALE");
_params_handles.mag_x_offset = param_find("CAL_MAG0_XOFF");
_params_handles.mag_x_scale = param_find("CAL_MAG0_XSCALE");
_params_handles.mag_y_offset = param_find("CAL_MAG0_YOFF");
_params_handles.mag_y_scale = param_find("CAL_MAG0_YSCALE");
_params_handles.mag_z_offset = param_find("CAL_MAG0_ZOFF");
_params_handles.mag_z_scale = param_find("CAL_MAG0_ZSCALE");
_params_handles.gyro_lpf_enum = param_find("MPU_GYRO_LPF_ENM");
_params_handles.accel_lpf_enum = param_find("MPU_ACC_LPF_ENM");
_params_handles.imu_sample_rate_enum = param_find("MPU_SAMPLE_R_ENM");
parameters_update();
}
bool create_pubs()
{
// initialize the reports
memset(&_gyro, 0, sizeof(sensor_gyro_s));
memset(&_accel, 0, sizeof(sensor_accel_s));
memset(&_mag, 0, sizeof(struct mag_report));
_gyro_pub = orb_advertise_multi(ORB_ID(sensor_gyro), &_gyro,
&_gyro_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_gyro_pub == nullptr) {
PX4_ERR("sensor_gyro advert fail");
return false;
}
_accel_pub = orb_advertise_multi(ORB_ID(sensor_accel), &_accel,
&_accel_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_accel_pub == nullptr) {
PX4_ERR("sensor_accel advert fail");
return false;
}
_mag_pub = orb_advertise_multi(ORB_ID(sensor_mag), &_mag,
&_mag_orb_class_instance, ORB_PRIO_HIGH - 1);
if (_mag_pub == nullptr) {
PX4_ERR("sensor_mag advert fail");
return false;
}
return true;
}
void update_reports()
{
_gyro.timestamp = _data.timestamp;
_gyro.x = ((_data.gyro_raw[0] * _data.gyro_scaling) - _gyro_sc.x_offset) * _gyro_sc.x_scale;
_gyro.y = ((_data.gyro_raw[1] * _data.gyro_scaling) - _gyro_sc.y_offset) * _gyro_sc.y_scale;
_gyro.z = ((_data.gyro_raw[2] * _data.gyro_scaling) - _gyro_sc.z_offset) * _gyro_sc.z_scale;
_gyro.temperature = _data.temperature;
_gyro.scaling = _data.gyro_scaling;
_gyro.x_raw = _data.gyro_raw[0];
_gyro.y_raw = _data.gyro_raw[1];
_gyro.z_raw = _data.gyro_raw[2];
_accel.timestamp = _data.timestamp;
_accel.x = ((_data.accel_raw[0] * _data.accel_scaling) - _accel_sc.x_offset) * _accel_sc.x_scale;
_accel.y = ((_data.accel_raw[1] * _data.accel_scaling) - _accel_sc.y_offset) * _accel_sc.y_scale;
_accel.z = ((_data.accel_raw[2] * _data.accel_scaling) - _accel_sc.z_offset) * _accel_sc.z_scale;
_accel.temperature = _data.temperature;
_accel.scaling = _data.accel_scaling;
_accel.x_raw = _data.accel_raw[0];
_accel.y_raw = _data.accel_raw[1];
_accel.z_raw = _data.accel_raw[2];
if (_data.mag_data_ready) {
_mag.timestamp = _data.timestamp;
_mag.x = ((_data.mag_raw[0] * _data.mag_scaling) - _mag_sc.x_offset) * _mag_sc.x_scale;
_mag.y = ((_data.mag_raw[1] * _data.mag_scaling) - _mag_sc.y_offset) * _mag_sc.y_scale;
_mag.z = ((_data.mag_raw[2] * _data.mag_scaling) - _mag_sc.z_offset) * _mag_sc.z_scale;
_mag.scaling = _data.mag_scaling;
_mag.temperature = _data.temperature;
_mag.x_raw = _data.mag_raw[0];
_mag.y_raw = _data.mag_raw[1];
_mag.z_raw = _data.mag_raw[2];
}
}
void publish_reports()
{
if (orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &_gyro) != OK) {
PX4_WARN("failed to publish gyro report");
} else {
//PX4_DEBUG("MPU_GYRO_RAW: %d %d %d", _gyro.x_raw, _gyro.y_raw, _gyro.z_raw)
//PX4_DEBUG("MPU_GYRO: %f %f %f", _gyro.x, _gyro.y, _gyro.z)
}
if (orb_publish(ORB_ID(sensor_accel), _accel_pub, &_accel) != OK) {
PX4_WARN("failed to publish accel report");
} else {
//PX4_DEBUG("MPU_ACCEL_RAW: %d %d %d", _accel.x_raw, _accel.y_raw, _accel.z_raw)
//PX4_DEBUG("MPU_ACCEL: %f %f %f", _accel.x, _accel.y, _accel.z)
}
if (_data.mag_data_ready) {
if (orb_publish(ORB_ID(sensor_mag), _mag_pub, &_mag) != OK) {
PX4_WARN("failed to publish mag report");
} else {
//PX4_DEBUG("MPU_MAG_RAW: %d %d %d", _mag.x_raw, _mag.y_raw, _mag.z_raw)
//PX4_DEBUG("MPU_MAG: %f %f %f", _mag.x, _mag.y, _mag.z)
}
}
}
void task_main(int argc, char *argv[])
{
PX4_WARN("enter mpu9x50 task_main");
sigset_t set;
int sig = 0;
int rv;
exit_mreasurement = false;
parameters_init();
// create the mpu9x50 default configuration structure
struct mpu9x50_config config = {
.gyro_lpf = _gyro_lpf,
.acc_lpf = _accel_lpf,
.gyro_fsr = MPU9X50_GYRO_FSR_500DPS,
.acc_fsr = MPU9X50_ACC_FSR_4G,
.gyro_sample_rate = _imu_sample_rate,
.compass_enabled = true,
.compass_sample_rate = MPU9x50_COMPASS_SAMPLE_RATE_100HZ,
.spi_dev_path = _device,
};
// TODO-JYW:
// manually register with the DriverFramework to allow this driver to
// be found by other modules
// DriverFramework::StubSensor<DriverFramework::SPIDevObj> stub_sensor("mpu9x50", "/dev/accel0", "/dev/accel");
if (mpu9x50_initialize(&config) != 0) {
PX4_WARN("error initializing mpu9x50. Quit!");
goto exit;
}
if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
//if (mpu9x50_register_interrupt(SPI_INT_GPIO, &mpu9x50::data_ready_isr, NULL)
!= 0) {
PX4_WARN("error registering data ready interrupt. Quit!");
goto exit;
}
// create all uorb publications
if (!create_pubs()) {
goto exit;
}
// subscribe to parameter_update topic
_params_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize signal
sigemptyset(&set);
sigaddset(&set, SIGRTMIN);
while (!_task_should_exit) {
// wait on signal
rv = sigwait(&set, &sig);
// check if we are waken up by the proper signal
if (rv != 0 || sig != SIGRTMIN) {
PX4_WARN("mpu9x50 sigwait failed rv %d sig %d", rv, sig);
continue;
}
// read new IMU data and store the results in data
if (mpu9x50_get_data(&_data) != 0) {
PX4_WARN("mpu9x50_get_data() failed");
continue;
}
// since the context switch takes long time, override the timestamp provided by mpu9x50_get_data()
// with the ts of isr.
// Note: This is ok for MPU sample rate of < 1000; Higer than 1000 Sample rates will be an issue
// as the data is not consistent.
_data.timestamp = _isr_data_ready_timestamp;
// poll parameter update
parameter_update_poll();
// data is ready
update_reports();
// publish all sensor reports
publish_reports();
}
exit_mreasurement = true;
exit:
PX4_WARN("closing mpu9x50");
mpu9x50_close();
}
/** mpu9x50 main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("mpu9x50_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("mpu9x50_main task start failed");
return;
}
_is_running = true;
}
void stop()
{
// TODO - set thread exit signal to terminate the task main thread
_is_running = false;
_task_handle = -1;
}
void usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device");
}
}; // namespace mpu9x50
int mpu9x50_main(int argc, char *argv[])
{
const char *device = NULL;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
default:
mpu9x50::usage();
return 1;
}
}
// Check on required arguments
if (device == NULL || strlen(device) == 0) {
mpu9x50::usage();
return 1;
}
memset(mpu9x50::_device, 0, MAX_LEN_DEV_PATH);
strncpy(mpu9x50::_device, device, strlen(device));
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (mpu9x50::_is_running) {
PX4_WARN("mpu9x50 already running");
return 1;
}
mpu9x50::start();
} else if (!strcmp(verb, "stop")) {
if (mpu9x50::_is_running) {
PX4_WARN("mpu9x50 is not running");
return 1;
}
mpu9x50::stop();
} else if (!strcmp(verb, "status")) {
PX4_WARN("mpu9x50 is %s", mpu9x50::_is_running ? "running" : "stopped");
return 0;
} else {
mpu9x50::usage();
return 1;
}
return 0;
}
src/platforms/qurt/fc_addon/mpu_spi/mpu9x50_params.c
-86
View File
@@ -1,86 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file mpu9x50_params.c
*
* Parameters defined by the mpu9x50 driver
*/
#include <px4_config.h>
#include <parameters/param.h>
/**
* Low pass filter frequency for Gyro
*
* @value 0 MPU9X50_GYRO_LPF_250HZ
* @value 1 MPU9X50_GYRO_LPF_184HZ
* @value 2 MPU9X50_GYRO_LPF_92HZ
* @value 3 MPU9X50_GYRO_LPF_41HZ
* @value 4 MPU9X50_GYRO_LPF_20HZ
* @value 5 MPU9X50_GYRO_LPF_10HZ
* @value 6 MPU9X50_GYRO_LPF_5HZ
* @value 7 MPU9X50_GYRO_LPF_3600HZ_NOLPF
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_GYRO_LPF_ENM, 4);
/**
* Low pass filter frequency for Accelerometer
*
*
* @value 0 MPU9X50_ACC_LPF_460HZ
* @value 1 MPU9X50_ACC_LPF_184HZ
* @value 2 MPU9X50_ACC_LPF_92HZ
* @value 3 MPU9X50_ACC_LPF_41HZ
* @value 4 MPU9X50_ACC_LPF_20HZ
* @value 5 MPU9X50_ACC_LPF_10HZ
* @value 6 MPU9X50_ACC_LPF_5HZ
* @value 7 MPU9X50_ACC_LPF_460HZ_NOLPF
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_ACC_LPF_ENM, 4);
/**
* Sample rate in Hz
*
* @value 0 MPU9x50_SAMPLE_RATE_100HZ
* @value 1 MPU9x50_SAMPLE_RATE_200HZ
* @value 2 MPU9x50_SAMPLE_RATE_500HZ
* @value 3 MPU9x50_SAMPLE_RATE_1000HZ
*
* @group MPU9x50 Configuration
*/
PARAM_DEFINE_INT32(MPU_SAMPLE_R_ENM, 2);
src/platforms/qurt/fc_addon/rc_receiver/CMakeLists.txt
-57
View File
@@ -1,57 +0,0 @@
############################################################################
#
# Copyright (c) 2016 Mark Charlebois. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name AtlFlight nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(TOOLS_ERROR_MSG
"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
if ("$ENV{FC_ADDON}" STREQUAL "")
message(FATAL_ERROR ${TOOLS_ERROR_MSG})
else()
set(FC_ADDON $ENV{FC_ADDON})
endif()
add_library(rc_receiver SHARED IMPORTED GLOBAL)
set_target_properties(rc_receiver PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/librc_receiver.so)
px4_add_module(
MODULE platforms__qurt__fc_addon__rc_receiver
MAIN rc_receiver
STACK_MAIN 1200
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
rc_receiver_main.cpp
rc_receiver_params.c
)
target_link_libraries(platforms__qurt__fc_addon__rc_receiver PRIVATE rc_receiver)
src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_main.cpp
-347
View File
@@ -1,347 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <px4_getopt.h>
#include <px4_tasks.h>
#include <px4_log.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/input_rc.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_rc_input.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <semaphore.h>
#include "rc_receiver_api.h"
#ifdef __cplusplus
}
#endif
/** driver 'main' command */
extern "C" { __EXPORT int rc_receiver_main(int argc, char *argv[]); }
#define MAX_LEN_DEV_PATH 32
namespace rc_receiver
{
/** Threshold value to detect rc receiver signal lost in millisecond */
#define SIGNAL_LOST_THRESHOLD_MS 500
/** serial device path that rc receiver is connected to */
static char _device[MAX_LEN_DEV_PATH];
/** flag indicating if rc_receiver module is running */
static bool _is_running = false;
/** flag indicating if task thread should exit */
static bool _task_should_exit = false;
/** handle to the task main thread */
static px4_task_t _task_handle = -1;
/** RC receiver type */
static enum RC_RECEIVER_TYPES _rc_receiver_type;
/** RC input channel value array accomondating up to RC_INPUT_MAX_CHANNELS */
static uint16_t rc_inputs[input_rc_s::RC_INPUT_MAX_CHANNELS];
/** rc_input uorb topic publication handle */
static orb_advert_t _input_rc_pub = nullptr;
/** rc_input uorb topic data */
static struct input_rc_s _rc_in;
/**< parameter update subscription */
static int _params_sub;
struct {
param_t rc_receiver_type;
} _params_handles; /**< parameter handles */
/** Print out the usage information */
static void usage();
/** mpu9x50 start measurement */
static void start();
/** mpu9x50 stop measurement */
static void stop();
/** task main trampoline function */
static void task_main_trampoline(int argc, char *argv[]);
/** mpu9x50 measurement thread primary entry point */
static void task_main(int argc, char *argv[]);
/** update all parameters */
static void parameters_update();
/** poll parameter update */
static void parameter_update_poll();
void parameters_update()
{
PX4_DEBUG("rc_receiver_parameters_update");
float v;
// accel params
if (param_get(_params_handles.rc_receiver_type, &v) == 0) {
_rc_receiver_type = (enum RC_RECEIVER_TYPES)v;
PX4_DEBUG("rc_receiver_parameters_update rc_receiver_type %f", v);
}
}
void parameters_init()
{
_params_handles.rc_receiver_type = param_find("RC_RECEIVER_TYPE");
parameters_update();
}
void parameter_update_poll()
{
bool updated;
/* Check if parameters have changed */
orb_check(_params_sub, &updated);
if (updated) {
struct parameter_update_s param_update;
orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
parameters_update();
}
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("rc_receiver_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("task start failed");
return;
}
_is_running = true;
}
void stop()
{
// TODO - set thread exit signal to terminate the task main thread
_is_running = false;
_task_handle = -1;
}
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void task_main(int argc, char *argv[])
{
PX4_WARN("enter rc_receiver task_main");
uint32_t fd;
// clear the rc_input report for topic advertise
memset(&_rc_in, 0, sizeof(struct input_rc_s));
_input_rc_pub = orb_advertise(ORB_ID(input_rc), &_rc_in);
if (_input_rc_pub == nullptr) {
PX4_WARN("failed to advertise input_rc uorb topic. Quit!");
return ;
}
// subscribe to parameter_update topic
_params_sub = orb_subscribe(ORB_ID(parameter_update));
// Open the RC receiver device on the specified serial port
fd = rc_receiver_open(_rc_receiver_type, _device);
if (fd <= 0) {
PX4_WARN("failed to open rc receiver type %d dev %s. Quit!",
_rc_receiver_type, _device);
return ;
}
// Continuously receive RC packet from serial device, until task is signaled
// to exit
uint32_t num_channels;
uint64_t ts = hrt_absolute_time();
int ret;
int counter = 0;
_rc_in.timestamp_last_signal = ts;
while (!_task_should_exit) {
// poll parameter update
parameter_update_poll();
// read RC packet from serial device in blocking mode.
num_channels = input_rc_s::RC_INPUT_MAX_CHANNELS;
ret = rc_receiver_get_packet(fd, rc_inputs, &num_channels);
ts = hrt_absolute_time();
_rc_in.input_source = input_rc_s::RC_INPUT_SOURCE_QURT;
if (ret < 0) {
// enum RC_RECEIVER_ERRORS error_code = rc_receiver_get_last_error(fd);
// PX4_WARN("RC packet receiving timed out. error code %d", error_code);
uint64_t time_diff_us = ts - _rc_in.timestamp_last_signal;
if (time_diff_us > SIGNAL_LOST_THRESHOLD_MS * 1000) {
_rc_in.rc_lost = true;
if (++counter == 500) {
PX4_WARN("RC signal lost for %u ms", time_diff_us / 1000);
counter = 0;
}
} else {
continue;
}
}
// populate the input_rc_s structure
if (ret == 0) {
_rc_in.timestamp = ts;
_rc_in.timestamp_last_signal = _rc_in.timestamp;
_rc_in.channel_count = num_channels;
// TODO - need to add support for RSSI, failsafe mode
_rc_in.rssi = RC_INPUT_RSSI_MAX;
_rc_in.rc_failsafe = false;
_rc_in.rc_lost = false;
_rc_in.rc_lost_frame_count = 0;
_rc_in.rc_total_frame_count = 1;
}
for (uint32_t i = 0; i < num_channels; i++) {
// Scale the Spektrum DSM value to ppm encoding. This is for the
// consistency with PX4 which internally translates DSM to PPM.
// See modules/px4iofirmware/dsm.c for details
// NOTE: rc_receiver spektrum driver outputs the data in 10bit DSM
// format, so we need to double the channel value before the scaling
_rc_in.values[i] = ((((int)(rc_inputs[i] * 2) - 1024) * 1000) / 1700) + 1500;
}
orb_publish(ORB_ID(input_rc), _input_rc_pub, &_rc_in);
}
rc_receiver_close(fd);
}
void usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device");
}
} // namespace rc_receiver
int rc_receiver_main(int argc, char *argv[])
{
const char *device = NULL;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
/* jump over start/off/etc and look at options first */
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
default:
rc_receiver::usage();
return 1;
}
}
// Check on required arguments
if (device == NULL || strlen(device) == 0) {
rc_receiver::usage();
return 1;
}
memset(rc_receiver::_device, 0, MAX_LEN_DEV_PATH);
strncpy(rc_receiver::_device, device, strlen(device));
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (rc_receiver::_is_running) {
PX4_WARN("rc_receiver already running");
return 1;
}
rc_receiver::start();
} else if (!strcmp(verb, "stop")) {
if (rc_receiver::_is_running) {
PX4_WARN("rc_receiver is not running");
return 1;
}
rc_receiver::stop();
} else if (!strcmp(verb, "status")) {
PX4_WARN("rc_receiver is %s", rc_receiver::_is_running ? "running" : "stopped");
return 0;
} else {
rc_receiver::usage();
return 1;
}
return 0;
}
src/platforms/qurt/fc_addon/rc_receiver/rc_receiver_params.c
-53
View File
@@ -1,53 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file rc_receiver_params.c
*
* Parameters defined by the rc_receiver driver
*/
#include <px4_config.h>
#include <parameters/param.h>
/**
* RC receiver type
*
* Acceptable values:
*
* - RC_RECEIVER_SPEKTRUM = 1,
* - RC_RECEIVER_LEMONRX = 2,
*
* @group RC Receiver Configuration
*/
PARAM_DEFINE_INT32(RC_RECEIVER_TYPE, 1);
src/platforms/qurt/fc_addon/uart_esc/CMakeLists.txt
-58
View File
@@ -1,58 +0,0 @@
############################################################################
#
# Copyright (C) 2015 Mark Charlebois. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
set(TOOLS_ERROR_MSG
"The FC_ADDON must be installed and the environment variable FC_ADDON must be set"
"(e.g. export FC_ADDON=${HOME}/Qualcomm/addon)")
if ("$ENV{FC_ADDON}" STREQUAL "")
message(FATAL_ERROR ${TOOLS_ERROR_MSG})
else()
set(FC_ADDON $ENV{FC_ADDON})
endif()
add_library(uart_esc SHARED IMPORTED GLOBAL)
set_target_properties(uart_esc PROPERTIES IMPORTED_LOCATION ${FC_ADDON}/flight_controller/hexagon/libs/libuart_esc.so)
px4_add_module(
MODULE platforms__qurt__fc_addon__uart_esc
MAIN uart_esc
STACK_MAIN 1200
INCLUDES
${FC_ADDON}/flight_controller/hexagon/inc
SRCS
uart_esc_main.cpp
uart_esc_params.c
DEPENDS
)
target_link_libraries(platforms__qurt__fc_addon__uart_esc PRIVATE uart_esc)
src/platforms/qurt/fc_addon/uart_esc/uart_esc_main.cpp
-521
View File
@@ -1,521 +0,0 @@
/****************************************************************************
*
* Copyright (c) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <stdint.h>
#include <px4_tasks.h>
#include <px4_getopt.h>
#include <px4_posix.h>
#include <errno.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_outputs.h>
#include <uORB/topics/parameter_update.h>
#include <drivers/drv_hrt.h>
#include <drivers/drv_mixer.h>
#include <lib/mixer/mixer.h>
#include <lib/mixer/mixer_multirotor_normalized.generated.h>
#include <parameters/param.h>
#ifdef __cplusplus
extern "C" {
#endif
#include <uart_esc.h>
#ifdef __cplusplus
}
#endif
/** driver 'main' command */
extern "C" { __EXPORT int uart_esc_main(int argc, char *argv[]); }
#define MAX_LEN_DEV_PATH 32
namespace uart_esc
{
#define UART_ESC_MAX_MOTORS 4
volatile bool _task_should_exit = false; // flag indicating if uart_esc task should exit
static char _device[MAX_LEN_DEV_PATH];
static bool _is_running = false; // flag indicating if uart_esc app is running
static px4_task_t _task_handle = -1; // handle to the task main thread
UartEsc *esc; // esc instance
void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors); // motor re-mapping
// subscriptions
int _controls_sub;
int _armed_sub;
int _param_sub;
// filenames
// /dev/fs/ is mapped to /usr/share/data/adsp/
static const char *MIXER_FILENAME = "/dev/fs/mixer_config.mix";
// publications
orb_advert_t _outputs_pub;
// topic structures
actuator_controls_s _controls;
actuator_armed_s _armed;
parameter_update_s _param_update;
actuator_outputs_s _outputs;
/** Print out the usage information */
static void usage();
/** uart_esc start */
static void start(const char *device) __attribute__((unused));
/** uart_esc stop */
static void stop();
/** task main trampoline function */
static void task_main_trampoline(int argc, char *argv[]);
/** uart_esc thread primary entry point */
static void task_main(int argc, char *argv[]);
/** mixer initialization */
static MultirotorMixer *mixer;
static int initialize_mixer(const char *mixer_filename);
static int mixer_control_callback(uintptr_t handle, uint8_t control_group, uint8_t control_index, float &input);
static void parameters_init();
static void parameters_update();
struct {
int model;
int baudrate;
int px4_motor_mapping[UART_ESC_MAX_MOTORS];
} _parameters;
struct {
param_t model;
param_t baudrate;
param_t px4_motor_mapping[UART_ESC_MAX_MOTORS];
} _parameter_handles;
void parameters_init()
{
_parameter_handles.model = param_find("UART_ESC_MODEL");
_parameter_handles.baudrate = param_find("UART_ESC_BAUD");
/* PX4 motor mapping parameters */
for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
char nbuf[20];
/* min values */
sprintf(nbuf, "UART_ESC_MOTOR%d", i + 1);
_parameter_handles.px4_motor_mapping[i] = param_find(nbuf);
}
parameters_update();
}
void parameters_update()
{
PX4_WARN("uart_esc_main parameters_update");
int32_t v_int;
if (param_get(_parameter_handles.model, &v_int) == 0) {
_parameters.model = v_int;
PX4_WARN("UART_ESC_MODEL %d", _parameters.model);
}
if (param_get(_parameter_handles.baudrate, &v_int) == 0) {
_parameters.baudrate = v_int;
PX4_WARN("UART_ESC_BAUD %d", _parameters.baudrate);
}
for (unsigned int i = 0; i < UART_ESC_MAX_MOTORS; i++) {
if (param_get(_parameter_handles.px4_motor_mapping[i], &v_int) == 0) {
_parameters.px4_motor_mapping[i] = v_int;
PX4_WARN("UART_ESC_MOTOR%d %d", i + 1, _parameters.px4_motor_mapping[i]);
}
}
}
int mixer_control_callback(uintptr_t handle,
uint8_t control_group,
uint8_t control_index,
float &input)
{
const actuator_controls_s *controls = (actuator_controls_s *)handle;
input = controls[control_group].control[control_index];
/* motor spinup phase - lock throttle to zero *
if (_pwm_limit.state == PWM_LIMIT_STATE_RAMP) {
if (control_group == actuator_controls_s::GROUP_INDEX_ATTITUDE &&
control_index == actuator_controls_s::INDEX_THROTTLE) {
* limit the throttle output to zero during motor spinup,
* as the motors cannot follow any demand yet
*
input = 0.0f;
}
}
*/
return 0;
}
int initialize_mixer(const char *mixer_filename)
{
mixer = nullptr;
int mixer_initialized = -1;
char buf[2048];
unsigned int buflen = sizeof(buf);
PX4_INFO("Initializing mixer from config file in %s", mixer_filename);
int fd_load = open(mixer_filename, O_RDONLY);
if (fd_load != -1) {
int nRead = read(fd_load, buf, buflen);
close(fd_load);
if (nRead > 0) {
mixer = MultirotorMixer::from_text(mixer_control_callback, (uintptr_t)&_controls, buf, buflen);
if (mixer != nullptr) {
PX4_INFO("Successfully initialized mixer from config file");
mixer_initialized = 0;
} else {
PX4_WARN("Unable to parse from mixer config file");
}
} else {
PX4_WARN("Unable to read from mixer config file");
}
} else {
PX4_WARN("Unable to open mixer config file");
}
// mixer file loading failed, fall back to default mixer configuration for
// QUAD_X airframe
if (mixer_initialized < 0) {
float roll_scale = 1;
float pitch_scale = 1;
float yaw_scale = 1;
float deadband = 0;
mixer = new MultirotorMixer(mixer_control_callback, (uintptr_t)&_controls,
MultirotorGeometry::QUAD_X,
roll_scale, pitch_scale, yaw_scale, deadband);
if (mixer == nullptr) {
PX4_ERR("mixer initialization failed");
mixer_initialized = -1;
return mixer_initialized;
}
PX4_WARN("mixer config file not found, successfully initialized default quad x mixer");
mixer_initialized = 0;
}
return mixer_initialized;
}
/**
* Rotate the motor rpm values based on the motor mappings configuration stored
* in motor_mapping
*/
void uart_esc_rotate_motors(int16_t *motor_rpm, int num_rotors)
{
ASSERT(num_rotors <= UART_ESC_MAX_MOTORS);
int i;
int16_t motor_rpm_copy[UART_ESC_MAX_MOTORS];
for (i = 0; i < num_rotors; i++) {
motor_rpm_copy[i] = motor_rpm[i];
}
for (i = 0; i < num_rotors; i++) {
motor_rpm[_parameters.px4_motor_mapping[i] - 1] = motor_rpm_copy[i];
}
}
void task_main(int argc, char *argv[])
{
PX4_INFO("enter uart_esc task_main");
_outputs_pub = nullptr;
parameters_init();
esc = UartEsc::get_instance();
if (esc == NULL) {
PX4_ERR("failed to new UartEsc instance");
} else if (esc->initialize((enum esc_model_t)_parameters.model,
_device, _parameters.baudrate) < 0) {
PX4_ERR("failed to initialize UartEsc");
} else {
// Subscribe for orb topics
_controls_sub = orb_subscribe(ORB_ID(actuator_controls_0)); // single group for now
_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
_param_sub = orb_subscribe(ORB_ID(parameter_update));
// initialize publication structures
memset(&_outputs, 0, sizeof(_outputs));
// set up poll topic and limit poll interval
px4_pollfd_struct_t fds[1];
fds[0].fd = _controls_sub;
fds[0].events = POLLIN;
//orb_set_interval(_controls_sub, 10); // max actuator update period, ms
// set up mixer
if (initialize_mixer(MIXER_FILENAME) < 0) {
PX4_ERR("Mixer initialization failed.");
_task_should_exit = true;
}
// Main loop
while (!_task_should_exit) {
int pret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
/* timed out - periodic check for _task_should_exit */
if (pret == 0) {
continue;
}
/* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) {
PX4_WARN("poll error %d, %d", pret, errno);
/* sleep a bit before next try */
usleep(100000);
continue;
}
// Handle new actuator controls data
if (fds[0].revents & POLLIN) {
// Grab new controls data
orb_copy(ORB_ID(actuator_controls_0), _controls_sub, &_controls);
// Mix to the outputs
_outputs.timestamp = hrt_absolute_time();
int16_t motor_rpms[UART_ESC_MAX_MOTORS];
if (_armed.armed && !_armed.lockdown && !_armed.manual_lockdown) {
_outputs.noutputs = mixer->mix(&_outputs.output[0], actuator_controls_s::NUM_ACTUATOR_CONTROLS);
// Make sure we support only up to UART_ESC_MAX_MOTORS motors
if (_outputs.noutputs > UART_ESC_MAX_MOTORS) {
PX4_ERR("Unsupported motors %d, up to %d motors supported",
_outputs.noutputs, UART_ESC_MAX_MOTORS);
continue;
}
// Send outputs to the ESCs
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
// map -1.0 - 1.0 outputs to RPMs
motor_rpms[outIdx] = (int16_t)(((_outputs.output[outIdx] + 1.0) / 2.0) *
(esc->max_rpm() - esc->min_rpm()) + esc->min_rpm());
}
uart_esc_rotate_motors(motor_rpms, _outputs.noutputs);
} else {
_outputs.noutputs = UART_ESC_MAX_MOTORS;
for (unsigned outIdx = 0; outIdx < _outputs.noutputs; outIdx++) {
motor_rpms[outIdx] = 0;
_outputs.output[outIdx] = -1.0;
}
}
esc->send_rpms(&motor_rpms[0], _outputs.noutputs);
// TODO-JYW: TESTING-TESTING
// MAINTAIN FOR REFERENCE, COMMENT OUT FOR RELEASE BUILDS
// static int count=0;
// count++;
// if (!(count % 100)) {
// PX4_DEBUG(" ");
// PX4_DEBUG("Time t: %13lu, Armed: %d ",(unsigned long)_outputs.timestamp,_armed.armed);
// PX4_DEBUG("Act Controls: 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_controls.control[0],_controls.control[1],_controls.control[2],_controls.control[3]);
// PX4_DEBUG("Act Outputs : 0: %+8.4f, 1: %+8.4f, 2: %+8.4f, 3: %+8.4f ",_outputs.output[0],_outputs.output[1],_outputs.output[2],_outputs.output[3]);
// }
// TODO-JYW: TESTING-TESTING
/* Publish mixed control outputs */
if (_outputs_pub != nullptr) {
orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &_outputs);
} else {
_outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &_outputs);
}
}
// Check for updates in other subscripions
bool updated = false;
orb_check(_armed_sub, &updated);
if (updated) {
orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
PX4_DEBUG("arming status updated, _armed.armed: %d", _armed.armed);
}
orb_check(_param_sub, &updated);
if (updated) {
// Even though we are only interested in the update status of the parameters, copy
// the subscription to clear the update status.
orb_copy(ORB_ID(parameter_update), _param_sub, &_param_update);
parameters_update();
}
}
}
PX4_WARN("closing uart_esc");
delete esc;
}
/** uart_esc main entrance */
void task_main_trampoline(int argc, char *argv[])
{
PX4_WARN("task_main_trampoline");
task_main(argc, argv);
}
void start()
{
ASSERT(_task_handle == -1);
/* start the task */
_task_handle = px4_task_spawn_cmd("uart_esc_main",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX,
1500,
(px4_main_t)&task_main_trampoline,
nullptr);
if (_task_handle < 0) {
warn("task start failed");
return;
}
_is_running = true;
}
void stop()
{
// TODO - set thread exit signal to terminate the task main thread
_is_running = false;
_task_handle = -1;
}
void usage()
{
PX4_WARN("missing command: try 'start', 'stop', 'status'");
PX4_WARN("options:");
PX4_WARN(" -D device");
}
}; // namespace uart_esc
int uart_esc_main(int argc, char *argv[])
{
const char *device = NULL;
int ch;
int myoptind = 1;
const char *myoptarg = NULL;
while ((ch = px4_getopt(argc, argv, "D:", &myoptind, &myoptarg)) != EOF) {
switch (ch) {
case 'D':
device = myoptarg;
break;
default:
uart_esc::usage();
return 1;
}
}
// Check on required arguments
if (device == NULL || strlen(device) == 0) {
uart_esc::usage();
return 1;
}
memset(uart_esc::_device, 0, MAX_LEN_DEV_PATH);
strncpy(uart_esc::_device, device, strlen(device));
const char *verb = argv[myoptind];
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start")) {
if (uart_esc::_is_running) {
PX4_WARN("uart_esc already running");
return 1;
}
uart_esc::start();
}
else if (!strcmp(verb, "stop")) {
if (uart_esc::_is_running) {
PX4_WARN("uart_esc is not running");
return 1;
}
uart_esc::stop();
}
else if (!strcmp(verb, "status")) {
PX4_WARN("uart_esc is %s", uart_esc::_is_running ? "running" : "stopped");
return 0;
} else {
uart_esc::usage();
return 1;
}
return 0;
}
src/platforms/qurt/fc_addon/uart_esc/uart_esc_params.c
-139
View File
@@ -1,139 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Ramakrishna Kintada. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file uart_esc_params.c
*
* Parameters defined for the uart esc driver
*/
#include <px4_config.h>
#include <parameters/param.h>
/**
* ESC model
*
* See esc_model_t enum definition in uart_esc_dev.h for all supported ESC
* model enum values.
*
* @value 0 ESC_200QX
* @value 1 ESC_350QX
* @value 2 ESC_210QC
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MODEL, 2);
/**
* ESC UART baud rate
*
* Default rate is 250Kbps, whic is used in off-the-shelf QRP ESC products.
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_BAUD, 250000);
/**
* The PX4 default motor mappings are
* 1 4
* [front]
* 3 2
*
* The following paramters define the motor mappings in reference to the
* PX4 motor mapping convention.
*/
/**
* Default PX4 motor mappings
* 1 4
* [front]
* 3 2
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 3);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 4);
/**
* Motor mappings for 350QX
* 4 3
* [front]
* 1 2
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
/**
* Motor mappings for 200QX
* 2 3
* [front]
* 1 4
*/
// PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 2);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 4);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
// PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
/**
* Motor mappings for 210QC
* 4 3
* [front]
* 1 2
*/
/**
* Motor 1 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR1, 4);
/**
* Motor 2 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR2, 2);
/**
* Motor 3 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR3, 1);
/**
* Motor 4 Mapping
*
* @group Snapdragon UART ESC
*/
PARAM_DEFINE_INT32(UART_ESC_MOTOR4, 3);
src/platforms/qurt/stubs/stubs_posix.c
-159
View File
@@ -1,159 +0,0 @@
#include <semaphore.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <time.h>
int sem_init(sem_t *sem, int pshared, unsigned int value)
{
return 1;
}
int sem_wait(sem_t *sem)
{
return 1;
}
int sem_destroy(sem_t *sem)
{
return 1;
}
int sem_post(sem_t *sem)
{
return 1;
}
int sem_getvalue(sem_t *sem, int *sval)
{
return 1;
}
int usleep(useconds_t usec)
{
return 0;
}
pthread_t pthread_self(void)
{
pthread_t x = 0;
return x;
}
int pthread_kill(pthread_t thread, int sig)
{
return 1;
}
void pthread_exit(void *retval)
{
}
int pthread_join(pthread_t thread, void **retval)
{
return 1;
}
int pthread_cancel(pthread_t thread)
{
return 1;
}
int pthread_attr_init(pthread_attr_t *attr)
{
return 1;
}
int pthread_attr_setstacksize(pthread_attr_t *attr, size_t stacksize)
{
return 1;
}
int pthread_attr_getstacksize(const pthread_attr_t *attr, size_t *stacksize)
{
return 1;
}
int pthread_attr_setschedparam(pthread_attr_t *attr, const struct sched_param *param)
{
return 1;
}
int pthread_create(pthread_t *thread, const pthread_attr_t *attr, void *(*start_routine)(void *), void *arg)
{
return 1;
}
int pthread_attr_getschedparam(const pthread_attr_t *attr, struct sched_param *param)
{
return 1;
}
int pthread_attr_destroy(pthread_attr_t *attr)
{
return 1;
}
int clock_gettime(clockid_t clk_id, struct timespec *tp)
{
return 1;
}
int pthread_mutex_lock(pthread_mutex_t *mutex)
{
return 1;
}
int pthread_mutex_unlock(pthread_mutex_t *mutex)
{
return 1;
}
int pthread_cond_signal(pthread_cond_t *cond)
{
return 1;
}
int pthread_mutex_destroy(pthread_mutex_t *mutex)
{
return 1;
}
int pthread_mutex_init(pthread_mutex_t *mutex, const pthread_mutexattr_t *attr)
{
return 1;
}
int pthread_cond_wait(pthread_cond_t *cond, pthread_mutex_t *mutex)
{
return 1;
}
int pthread_cond_timedwait(pthread_cond_t *cond, pthread_mutex_t *mutex, const struct timespec *abstime)
{
return 1;
}
int pthread_cond_init(pthread_cond_t *cond, const pthread_condattr_t *attr)
{
return 1;
}
int pthread_mutexattr_init(pthread_mutexattr_t *attr)
{
return -1;
}
int pthread_mutexattr_destroy(pthread_mutexattr_t *attr)
{
return -1;
}
int pthread_mutexattr_settype(pthread_mutexattr_t *attr, int type)
{
return -1;
}
int pthread_condattr_init(pthread_condattr_t *attr)
{
return -1;
}
int fsync(int fd)
{
return -1;
}
src/platforms/qurt/stubs/stubs_qurt.c
-64
View File
@@ -1,64 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <dlfcn.h>
#include <qurt_log.h>
#include <dspal_platform.h>
// This code is never run. It is used solely to test linking in the
// TravisCI build test to make sure all symbols are resolved.
__EXPORT int _start_main(void);
void HAP_debug(const char *msg, int level, const char *filename, int line)
{
}
void HAP_power_request(int a, int b, int c)
{
}
int dlinit(int a, char **b)
{
return 1;
}
int main(int argc, char *argv[])
{
int ret = 0;
return ret;
}
int _start_main()
{
return -1;
}
src/platforms/qurt/tests/hello/CMakeLists.txt
-41
View File
@@ -1,41 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE platforms__qurt__tests__hello
MAIN hello
SRCS
hello_main.cpp
hello_start_qurt.cpp
hello_example.cpp
DEPENDS
)
src/platforms/qurt/tests/hello/hello_example.cpp
-61
View File
@@ -1,61 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_example.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "hello_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
px4::AppState HelloExample::appState;
int HelloExample::main()
{
appState.setRunning(true);
int i = 0;
while (!appState.exitRequested() && i < 5) {
PX4_DEBUG(" Doing work...");
++i;
}
return 0;
}
src/platforms/qurt/tests/hello/hello_example.h
-54
View File
@@ -1,54 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_example.h
* Example app for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#pragma once
#include <px4_app.h>
class HelloExample
{
public:
HelloExample() {}
~HelloExample() {}
int main();
static px4::AppState appState; /* track requests to terminate app */
};
src/platforms/qurt/tests/hello/hello_main.cpp
-56
View File
@@ -1,56 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_main.cpp
* Example for Linux
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "hello_example.h"
#include <stdio.h>
int PX4_MAIN(int argc, char **argv)
{
px4::init(argc, argv, "hello");
PX4_DEBUG("hello");
HelloExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}
src/platforms/qurt/tests/hello/hello_start_qurt.cpp
-101
View File
@@ -1,101 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file hello_start_linux.cpp
*
* @author Thomas Gubler <thomasgubler@gmail.com>
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "hello_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int hello_main(int argc, char *argv[]);
static void usage()
{
PX4_DEBUG("usage: hello {start|stop|status}");
}
int hello_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (HelloExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
daemon_task = px4_task_spawn_cmd("hello",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
16000,
PX4_MAIN,
(char *const *)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
HelloExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (HelloExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}
src/platforms/qurt/tests/muorb/CMakeLists.txt
-40
View File
@@ -1,40 +0,0 @@
############################################################################
#
# Copyright (c) 2015 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
px4_add_module(
MODULE platforms__qurt__tests__muorb
MAIN muorb_test
SRCS
muorb_test_start_qurt.cpp
muorb_test_example.cpp
DEPENDS
)
src/platforms/qurt/tests/muorb/muorb_test_example.cpp
-215
View File
@@ -1,215 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file muorb_test_example.cpp
* Example for muorb
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <unistd.h>
#include <stdio.h>
#include <string.h>
#include "uORB/topics/sensor_combined.h"
#include "uORB/topics/pwm_input.h"
#include "modules/uORB/uORB.h"
#include "px4_middleware.h"
#include "px4_defines.h"
#include <stdlib.h>
#include <drivers/drv_hrt.h>
px4::AppState MuorbTestExample::appState;
int MuorbTestExample::main()
{
int rc;
appState.setRunning(true);
rc = PingPongTest();
rc = FileReadTest();
rc = uSleepTest();
appState.setRunning(false);
return rc;
}
int MuorbTestExample::DefaultTest()
{
struct pwm_input_s pwm;
struct sensor_combined_s sc;
memset(&pwm, 0, sizeof(pwm_input_s));
memset(&sc, 0, sizeof(sensor_combined_s));
PX4_WARN("Successful after memset... ");
orb_advert_t pub_fd = orb_advertise(ORB_ID(pwm_input), &pwm);
if (pub_fd == nullptr) {
PX4_WARN("Error: advertizing pwm_input topic");
return -1;
}
orb_advert_t pub_sc = orb_advertise(ORB_ID(sensor_combined), &sc);
if (pub_sc == nullptr) {
PX4_WARN("Error: advertizing sensor_combined topic");
return -1;
}
int i = 0;
pwm.error_count++;
/*sc.gyro_errcount[i]++;*/ // no member named 'gyro_errcount' in 'sensor_combined_s'
while (!appState.exitRequested() && i < 10) {
PX4_INFO(" Doing work...");
orb_publish(ORB_ID(pwm_input), pub_fd, &pwm);
orb_publish(ORB_ID(sensor_combined), pub_sc, &sc);
usleep(1000000);
++i;
}
return 0;
}
int MuorbTestExample::PingPongTest()
{
int i = 0;
orb_advert_t pub_id_esc_status = orb_advertise(ORB_ID(esc_status), & m_esc_status);
if (pub_id_esc_status == 0) {
PX4_ERR("error publishing esc_status");
return -1;
}
if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the esc_status message", i);
return -1;
}
int sub_vc = orb_subscribe(ORB_ID(vehicle_command));
if (sub_vc == PX4_ERROR) {
PX4_ERR("Error subscribing to vehicle_command topic");
return -1;
}
while (!appState.exitRequested()) {
PX4_DEBUG("[%d] Doing work...", i);
bool updated = false;
if (orb_check(sub_vc, &updated) == 0) {
if (updated) {
PX4_WARN("[%d]vechile command status is updated... reading new value", i);
if (orb_copy(ORB_ID(vehicle_command), sub_vc, &m_vc) != 0) {
PX4_ERR("[%d]Error calling orb copy for vechicle... ", i);
break;
}
if (orb_publish(ORB_ID(esc_status), pub_id_esc_status, &m_esc_status) == PX4_ERROR) {
PX4_ERR("[%d]Error publishing the esc_status message", i);
break;
}
} else {
PX4_WARN("[%d] vechicle command topic is not updated", i);
}
} else {
PX4_ERR("[%d]Error checking the updated status for vehicle command ", i);
break;
}
// sleep for 1 sec.
usleep(1000000);
++i;
}
return 0;
}
int MuorbTestExample::uSleepTest()
{
PX4_WARN("before usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time());
usleep(1000000);
PX4_INFO("After usleep for 1 sec [%" PRIu64 "]", hrt_absolute_time());
for (int i = 0; i < 10; ++i) {
PX4_INFO("In While Loop: B4 Sleep for[%d] seconds [%" PRIu64 "]", i + 1, hrt_absolute_time());
usleep((i + 1) * 1000000);
PX4_INFO("In While Loop: After Sleep for[%d] seconds [%" PRIu64 "]", i + 1, hrt_absolute_time());
}
PX4_INFO("exiting sleep test...");
return 0;
}
int MuorbTestExample::FileReadTest()
{
int rc = OK;
static const char TEST_FILE_PATH[] = "./test.txt";
FILE *fp;
char *line = NULL;
/*size_t len = 0;
ssize_t read;*/
fp = fopen(TEST_FILE_PATH, "r");
if (fp == NULL) {
PX4_WARN("unable to open file[%s] for reading", TEST_FILE_PATH);
rc = PX4_ERROR;
} else {
/*
int i = 0;
while( ( read = getline( &line, &len, fp ) ) != -1 )
{
++i;
PX4_INFO( "LineNum[%d] LineLength[%d]", i, len );
PX4_INFO( "LineNum[%d] Line[%s]", i, line );
}
*/
PX4_INFO("Successfully opened file [%s]", TEST_FILE_PATH);
fclose(fp);
if (line != NULL) {
free(line);
}
}
return rc;
}
src/platforms/qurt/tests/muorb/muorb_test_example.h
-59
View File
@@ -1,59 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <px4_app.h>
#include "uORB/topics/esc_status.h"
#include "uORB/topics/vehicle_command.h"
class MuorbTestExample
{
public:
MuorbTestExample() {}
~MuorbTestExample() {}
int main();
static px4::AppState appState; /* track requests to terminate app */
private:
int DefaultTest();
int PingPongTest();
int FileReadTest();
int uSleepTest();
struct esc_status_s m_esc_status;
struct vehicle_command_s m_vc = {};
};
src/platforms/qurt/tests/muorb/muorb_test_main.cpp
-57
View File
@@ -1,57 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file muorb_test_main.cpp
* Test of Multi-uORB supoprt
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
#include <px4_middleware.h>
#include <px4_log.h>
#include <px4_app.h>
#include "muorb_test_example.h"
extern "C" __EXPORT int muorb_test_entry(int argc, char **argv);
int muorb_test_entry(int argc, char **argv)
{
px4::init(argc, argv, "muorb_test");
PX4_DEBUG("muorb_test");
MuorbTestExample hello;
hello.main();
PX4_DEBUG("goodbye");
return 0;
}
src/platforms/qurt/tests/muorb/muorb_test_start_qurt.cpp
-114
View File
@@ -1,114 +0,0 @@
/****************************************************************************
*
* Copyright (C) 2015 Mark Charlebois. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file muorb_test_start_qurt.cpp
*
* @author Mark Charlebois <mcharleb@gmail.com>
*/
#include "muorb_test_example.h"
#include <px4_log.h>
#include <px4_app.h>
#include <px4_tasks.h>
#include <stdio.h>
#include <string.h>
#include <sched.h>
static int daemon_task; /* Handle of deamon task / thread */
//using namespace px4;
extern "C" __EXPORT int muorb_test_main(int argc, char *argv[]);
int muorb_test_entry(int argc, char **argv)
{
//px4::init(argc, argv, "muorb_test");
PX4_INFO("muorb_test entry.....");
MuorbTestExample hello;
hello.main();
PX4_INFO("goodbye");
return 0;
}
static void usage()
{
PX4_DEBUG("usage: muorb_test {start|stop|status}");
}
int muorb_test_main(int argc, char *argv[])
{
if (argc < 2) {
usage();
return 1;
}
if (!strcmp(argv[1], "start")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("already running");
/* this is not an error */
return 0;
}
PX4_INFO("before starting the muorb_test_entry task");
daemon_task = px4_task_spawn_cmd("muorb_test",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
8192,
muorb_test_entry,
(char *const *)argv);
return 0;
}
if (!strcmp(argv[1], "stop")) {
MuorbTestExample::appState.requestExit();
return 0;
}
if (!strcmp(argv[1], "status")) {
if (MuorbTestExample::appState.isRunning()) {
PX4_DEBUG("is running");
} else {
PX4_DEBUG("not started");
}
return 0;
}
usage();
return 1;
}