fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2509 from mcharleb/qurt-layer

Browse Source 
Qurt layer
This commit is contained in:
Lorenz Meier 2015-07-02 01:11:48 +02:00
commit 8ce38cefae
10 changed files with 318 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/platforms/qurt/include/hrt_work.h
View File

@ -46,16 +46,16 @@ void hrt_work_queue_init(void);
int hrt_work_queue(struct work_s *work, worker_t worker, void *arg, uint32_t usdelay);
void hrt_work_cancel(struct work_s *work);
inline void hrt_work_lock(void);
inline void hrt_work_unlock(void);
static inline void hrt_work_lock(void);
static inline void hrt_work_unlock(void);
inline void hrt_work_lock()
static inline void hrt_work_lock()
{
//PX4_INFO("hrt_work_lock");
sem_wait(&_hrt_work_lock);
}
inline void hrt_work_unlock()
static inline void hrt_work_unlock()
{
//PX4_INFO("hrt_work_unlock");
sem_post(&_hrt_work_lock);

107
src/platforms/qurt/px4_layer/commands_hil.c Normal file
View File

@ -0,0 +1,107 @@
/****************************************************************************
*
* 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 commands_muorb_test.c
* Commands to run for the "qurt_muorb_test" config
*
* @author Mark Charlebois <charlebm@gmail.com>
*/
const char *get_commands()
{
static const char *commands =
"uorb start\n"
"param set CAL_GYRO0_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
// "rgbled start\n"
// "tone_alarm start\n"
"commander start\n"
"sensors start\n"
//"ekf_att_pos_estimator start\n"
"attitude_estimator_q start\n"
"position_estimator_inav start\n"
"mc_pos_control start\n"
"mc_att_control start\n"
"sleep 1\n"
"hil mode_pwm\n"
"param set RC1_MAX 2015\n"
"param set RC1_MIN 996\n"
"param set RC1_TRIM 1502\n"
"param set RC1_REV -1\n"
"param set RC2_MAX 2016 \n"
"param set RC2_MIN 995\n"
"param set RC2_TRIM 1500\n"
"param set RC3_MAX 2003\n"
"param set RC3_MIN 992\n"
"param set RC3_TRIM 992\n"
"param set RC4_MAX 2011\n"
"param set RC4_MIN 997\n"
"param set RC4_TRIM 1504\n"
"param set RC4_REV -1\n"
"param set RC6_MAX 2016\n"
"param set RC6_MIN 992\n"
"param set RC6_TRIM 1504\n"
"param set RC_CHAN_CNT 8\n"
"param set RC_MAP_MODE_SW 5\n"
"param set RC_MAP_POSCTL_SW 7\n"
"param set RC_MAP_RETURN_SW 8\n"
"param set MC_YAW_P 1.5\n"
"param set MC_PITCH_P 3.0\n"
"param set MC_ROLL_P 3.0\n"
"param set MC_YAWRATE_P 0.2\n"
"param set MC_PITCHRATE_P 0.03\n"
"param set MC_ROLLRATE_P 0.03\n"
"param set ATT_W_ACC 0.0002\n"
"param set ATT_W_MAG 0.002\n"
"param set ATT_W_GYRO_BIAS 0.05\n"
"sleep 1\n"
"param set MAV_TYPE 2\n"
"mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n"
"list_devices\n"
"list_files\n"
"list_tasks\n"
"list_topics\n"
"sleep 10\n"
"list_tasks\n"
"sleep 10\n"
;
return commands;
}

33
src/platforms/qurt/px4_layer/commands_muorb_test.c
View File

@ -43,5 +43,38 @@ const char *get_commands()
"uorb start\n"
"muorb_test start\n";
/*
"hil mode_pwm\n"
"mixer load /dev/pwm_output0 /startup/quad_x.main.mix\n";
*/
/*
"param show\n"
"param set CAL_GYRO_ID 2293760\n"
"param set CAL_ACC0_ID 1310720\n"
"param set CAL_ACC1_ID 1376256\n"
"param set CAL_MAG0_ID 196608\n"
"gyrosim start\n"
"accelsim start\n"
"rgbled start\n"
"tone_alarm start\n"
"simulator start -s\n"
"commander start\n"
"sensors start\n"
"ekf_att_pos_estimator start\n"
"mc_pos_control start\n"
"mc_att_control start\n"
"param set MAV_TYPE 2\n"
"param set RC1_MAX 2015\n"
"param set RC1_MIN 996\n"
"param set RC_TRIM 1502\n"
*/
return commands;
/*====================================== Working set
======================================*/
//"muorb_test start\n"
//"gyrosim start\n"
//"adcsim start\n"
}

6
src/platforms/qurt/px4_layer/drv_hrt.c
View File

@ -46,6 +46,8 @@
static struct sq_queue_s callout_queue;
extern uint64_t get_abs_time_in_us();
/* latency histogram */
#define LATENCY_BUCKET_COUNT 8
__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT;
@ -81,11 +83,15 @@ static void hrt_unlock(void)
*/
hrt_abstime hrt_absolute_time(void)
{
return get_abs_time_in_us();
/*
struct timespec ts;
// FIXME - clock_gettime unsupported in QuRT
//clock_gettime(CLOCK_MONOTONIC, &ts);
return ts_to_abstime(&ts);
*/
}
/*

1
src/platforms/qurt/px4_layer/hrt_work_cancel.c
View File

@ -40,6 +40,7 @@
#include <px4_config.h>
#include <px4_defines.h>
#include <queue.h>
#include <semaphore.h>
#include <px4_workqueue.h>
#include "hrt_work.h"

79
src/platforms/qurt/px4_layer/main.cpp
View File

@ -41,6 +41,7 @@
#include <px4_tasks.h>
#include <px4_time.h>
#include <px4_posix.h>
#include <px4_defines.h>
#include <vector>
#include <string>
#include <map>
@ -50,6 +51,7 @@ using namespace std;
extern void init_app_map(map<string,px4_main_t> &apps);
extern void list_builtins(map<string,px4_main_t> &apps);
static px4_task_t g_dspal_task = -1;
__BEGIN_DECLS
// The commands to run are specified in a target file: commands_<target>.c
@ -65,14 +67,16 @@ static void run_cmd(map<string,px4_main_t> &apps, const vector<string> &appargs)
unsigned int i = 0;
while (i < appargs.size() && appargs[i].c_str()[0] != '\0') {
arg[i] = (char *)appargs[i].c_str();
//printf(" arg = '%s'\n", arg[i]);
PX4_WARN(" arg = '%s'\n", arg[i]);
++i;
}
arg[i] = (char *)0;
//PX4_DEBUG_PRINTF(i);
apps[command](i,(char **)arg);
}
else
{
PX4_WARN("NOT FOUND.");
list_builtins(apps);
}
}
@ -91,10 +95,16 @@ static void process_commands(map<string,px4_main_t> &apps, const char *cmds)
int i=0;
const char *b = cmds;
bool found_first_char = false;
char arg[20];
char arg[256];
// This is added because it is a parameter used by commander, yet created by mavlink. Since mavlink is not
// running on QURT, we need to manually define it so it is available to commander. "2" is for quadrotor.
PARAM_DEFINE_INT32(MAV_TYPE,2);
// Eat leading whitespace
eat_whitespace(b, i);
for(;;) {
// End of command line
@ -105,7 +115,10 @@ static void process_commands(map<string,px4_main_t> &apps, const char *cmds)
// If we have a command to run
if (appargs.size() > 0) {
run_cmd(apps, appargs);
PX4_WARN("Processing command: %s",appargs[0].c_str());
for(int ai=1;ai<(int)appargs.size();ai++)
PX4_WARN(" > arg: %s",appargs[ai].c_str());
run_cmd(apps, appargs);
}
appargs.clear();
if (b[i] == '\n') {
@ -133,17 +146,63 @@ extern void init_once(void);
};
__BEGIN_DECLS
void dspal_entry()
{
const char *argv[2] = { "dspal_client", NULL };
int argc = 1;
extern int dspal_main(int argc, char *argv[]);
__END_DECLS
printf("In main\n");
int dspal_entry( int argc, char* argv[] )
{
//const char *argv[2] = { "dspal_client", NULL };
//int argc = 1;
PX4_INFO("In main\n");
map<string,px4_main_t> apps;
init_app_map(apps);
px4::init_once();
px4::init(argc, (char **)argv, "mainapp");
process_commands(apps, get_commands());
for (;;) { sleep(100000); }
for( ;; )
{
volatile int x = 0;
++x;
}
return 0;
}
static void usage()
{
PX4_WARN("Usage: dspal {start |stop}");
}
extern "C" {
int dspal_main(int argc, char *argv[])
{
int ret = 0;
if (argc == 2 && strcmp(argv[1], "start") == 0) {
g_dspal_task = px4_task_spawn_cmd("dspal",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 5,
1500,
dspal_entry,
argv);
}
else if (argc == 2 && strcmp(argv[1], "stop") == 0) {
if (g_dspal_task < 0) {
PX4_WARN("start up thread not running");
}
else {
px4_task_delete(g_dspal_task);
g_dspal_task = -1;
}
}
else {
usage();
ret = -1;
}
return ret;
}
}
__END_DECLS

12
src/platforms/qurt/px4_layer/module.mk
View File

@ -35,6 +35,8 @@
# NuttX / uORB adapter library
#
MODULE_NAME = dspal
SRCDIR=$(dir $(MODULE_MK))
SRCS = \
@ -56,8 +58,10 @@ SRCS = \
sq_remfirst.c \
sq_addafter.c \
dq_rem.c \
main.cpp \
qurt_stubs.c
hrt_work.c \
qurt_stubs.c \
qurt_hacks.c \
main.cpp
ifeq ($(CONFIG),qurt_hello)
SRCS += commands_hello.c
endif
@ -67,5 +71,9 @@ endif
ifeq ($(CONFIG),qurt_muorb_test)
SRCS += commands_muorb_test.c
endif
ifeq ($(CONFIG),qurt_hil)
SRCS += commands_hil.c
endif
MAXOPTIMIZATION = -Os

16
src/platforms/qurt/px4_layer/px4_qurt_impl.cpp
View File

@ -50,14 +50,18 @@
#include <semaphore.h>
#include "systemlib/param/param.h"
#include "hrt_work.h"
#include "px4_log.h"
extern pthread_t _shell_task_id;
//extern pthread_t _shell_task_id;
__BEGIN_DECLS
extern uint64_t get_ticks_per_us();
// FIXME - sysconf(_SC_CLK_TCK) not supported
long PX4_TICKS_PER_SEC = 1000;
//long PX4_TICKS_PER_SEC = get_ticks_per_us();
long PX4_TICKS_PER_SEC = 800000000;
unsigned int sleep(unsigned int sec)
{
@ -94,12 +98,16 @@ void init_once(void)
{
// Required for QuRT
//_posix_init();
PX4_WARN( "Before calling work_queue_init" );
// _shell_task_id = pthread_self();
// PX4_INFO("Shell id is %lu", _shell_task_id);
_shell_task_id = pthread_self();
PX4_INFO("Shell id is %lu", _shell_task_id);
work_queues_init();
PX4_WARN( "Before calling hrt_init" );
hrt_work_queue_init();
hrt_init();
PX4_WARN( "after calling hrt_init" );
}
void init(int argc, char *argv[], const char *app_name)

42
src/platforms/qurt/px4_layer/px4_qurt_tasks.cpp
View File

@ -43,7 +43,11 @@
#include <stdlib.h>
#include <errno.h>
#include <stdbool.h>
#if !defined(__PX4_QURT)
#include <signal.h>
#endif
#include <fcntl.h>
#include <sched.h>
#include <unistd.h>
@ -86,6 +90,12 @@ static void *entry_adapter ( void *ptr )
data = (pthdata_t *) ptr;
data->entry(data->argc, data->argv);
PX4_WARN( "Before waiting infinte busy loop" );
//for( ;; )
//{
// volatile int x = 0;
// ++x;
// }
free(ptr);
px4_task_exit(0);
@ -157,7 +167,17 @@ px4_task_t px4_task_spawn_cmd(const char *name, int scheduler, int priority, int
return (rv < 0) ? rv : -rv;
}
#endif
size_t fixed_stacksize = -1;
pthread_attr_getstacksize(&attr, &fixed_stacksize);
PX4_WARN("stack size: %d passed stacksize(%d)", fixed_stacksize, stack_size );
fixed_stacksize = 8 * 1024;
fixed_stacksize = ( fixed_stacksize < (size_t)stack_size )? (size_t)stack_size:fixed_stacksize;
PX4_WARN("setting the thread[%s] stack size to[%d]", name, fixed_stacksize );
pthread_attr_setstacksize(&attr, fixed_stacksize);
//pthread_attr_setstacksize(&attr, stack_size);
param.sched_priority = priority;
rv = pthread_attr_setschedparam(&attr, &param);
@ -236,7 +256,7 @@ int px4_task_kill(px4_task_t id, int sig)
{
int rv = 0;
pthread_t pid;
PX4_DEBUG("Called px4_task_kill %d", sig);
PX4_DEBUG("Called px4_task_kill %d, taskname %s", sig, taskmap[id].name.c_str());
if (id < PX4_MAX_TASKS && taskmap[id].pid != 0)
pid = taskmap[id].pid;
@ -272,9 +292,9 @@ __BEGIN_DECLS
int px4_getpid()
{
pthread_t pid = pthread_self();
if (pid == _shell_task_id)
return SHELL_TASK_ID;
//
// if (pid == _shell_task_id)
// return SHELL_TASK_ID;
// Get pthread ID from the opaque ID
for (int i=0; i<PX4_MAX_TASKS; ++i) {
@ -286,5 +306,19 @@ int px4_getpid()
return -EINVAL;
}
const char *getprogname();
const char *getprogname()
{
pthread_t pid = pthread_self();
for (int i=0; i<PX4_MAX_TASKS; i++)
{
if (taskmap[i].isused && taskmap[i].pid == pid)
{
return taskmap[i].name.c_str();
}
}
return "Unknown App";
}
__END_DECLS

42
src/platforms/qurt/px4_layer/qurt_stubs.c
View File

@ -30,60 +30,94 @@
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "px4_log.h"
//extern "C" {
void block_indefinite( void )
{
for(;;)
{
volatile int x = 0;
++x;
}
}
void _Read_uleb( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Parse_fde_instr( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Parse_csd( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Locksyslock( void )
void _Locksyslock( int x )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Unlocksyslock( void )
void _Unlocksyslock( int x )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Valbytes( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Get_eh_data( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Parse_lsda( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void __cxa_guard_release( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Read_enc_ptr( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void _Read_sleb( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void __cxa_guard_acquire( void )
{
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
void __cxa_pure_virtual()
{
while (1);
PX4_WARN( "Error: Calling unresolved symbol stub[%s]", __FUNCTION__ );
block_indefinite();
}
//}