dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Code cleanup and ifdefs required for qurt build 

Code that was previously out of tree that was #if 0, is now #ifdef __PX4_QURT.
These changes were required for flight using the qurt build.

Changes include code cleanup for shmem_posix.c.

Signed-off-by: Mark Charlebois <charlebm@gmail.com>
This commit is contained in:
Mark Charlebois 2016-01-21 20:41:42 -08:00 committed by Julian Oes
parent 8634452d6d
commit fea910d45a
5 changed files with 87 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/modules/commander/accelerometer_calibration.cpp
View File

@ -175,7 +175,9 @@ typedef struct {
int do_accel_calibration(int mavlink_fd)
{
#ifndef __PX4_QURT
int fd;
#endif
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
@ -192,6 +194,7 @@ int do_accel_calibration(int mavlink_fd)
char str[30];
#ifndef __PX4_QURT
/* reset all sensors */
for (unsigned s = 0; s < max_accel_sens; s++) {
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, s);
@ -211,6 +214,7 @@ int do_accel_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
}
}
#endif
float accel_offs[max_accel_sens][3];
float accel_T[max_accel_sens][3][3];
@ -277,14 +281,17 @@ int do_accel_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.y_scale)));
(void)sprintf(str, "CAL_ACC%u_ZSCALE", i);
failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.z_scale)));
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_ACC%u_ID", i);
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
#endif
if (failed) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
return ERROR;
}
#ifndef __PX4_QURT
sprintf(str, "%s%u", ACCEL_BASE_DEVICE_PATH, i);
fd = px4_open(str, 0);
@ -299,6 +306,7 @@ int do_accel_calibration(int mavlink_fd)
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
}
#endif
}
if (res == OK) {

15
src/modules/commander/gyro_calibration.cpp
View File

@ -173,7 +173,7 @@ int do_gyro_calibration(int mavlink_fd)
for (unsigned s = 0; s < max_gyros; s++) {
char str[30];
#ifndef __PX4_QURT
// Reset gyro ids to unavailable
worker_data.device_id[s] = 0;
(void)sprintf(str, "CAL_GYRO%u_ID", s);
@ -182,9 +182,18 @@ int do_gyro_calibration(int mavlink_fd)
mavlink_and_console_log_critical(mavlink_fd, "[cal] Unable to reset CAL_GYRO%u_ID", s);
return ERROR;
}
#else
(void)sprintf(str, "CAL_GYRO%u_ID", s);
res = param_get(param_find(str), &(worker_data.device_id[s]));
if (res != OK) {
mavlink_log_critical(mavlink_fd, "[cal] Unable to get CAL_GYRO%u_ID", s);
return ERROR;
}
#endif
// Reset all offsets to 0 and scales to 1
(void)memcpy(&worker_data.gyro_scale[s], &gyro_scale_zero, sizeof(gyro_scale));
#ifndef __PX4_QURT
sprintf(str, "%s%u", GYRO_BASE_DEVICE_PATH, s);
int fd = px4_open(str, 0);
if (fd >= 0) {
@ -197,6 +206,7 @@ int do_gyro_calibration(int mavlink_fd)
return ERROR;
}
}
#endif
}
@ -286,6 +296,8 @@ int do_gyro_calibration(int mavlink_fd)
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].y_offset)));
(void)sprintf(str, "CAL_GYRO%u_ZOFF", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.gyro_scale[s].z_offset)));
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_GYRO%u_ID", s);
failed |= (OK != param_set_no_notification(param_find(str), &(worker_data.device_id[s])));
@ -304,6 +316,7 @@ int do_gyro_calibration(int mavlink_fd)
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, 1);
}
#endif
}
}

17
src/modules/commander/mag_calibration.cpp
View File

@ -101,6 +101,7 @@ int do_mag_calibration(int mavlink_fd)
{
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
#ifndef __PX4_QURT
struct mag_scale mscale_null = {
0.0f,
1.0f,
@ -109,6 +110,7 @@ int do_mag_calibration(int mavlink_fd)
0.0f,
1.0f,
};
#endif
int result = OK;
@ -121,6 +123,7 @@ int do_mag_calibration(int mavlink_fd)
}
for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) {
#ifndef __PX4_QURT
// Reset mag id to mag not available
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
@ -128,7 +131,17 @@ int do_mag_calibration(int mavlink_fd)
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#else
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_get(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
#endif
/* for calibration, commander will run on apps, so orb messages are used to get info from dsp */
#ifndef __PX4_QURT
// Attempt to open mag
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
int fd = px4_open(str, O_RDONLY);
@ -158,6 +171,7 @@ int do_mag_calibration(int mavlink_fd)
}
px4_close(fd);
#endif
}
// Calibrate all mags at the same time
@ -615,8 +629,11 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
bool failed = false;
/* set parameters */
#ifndef __PX4_QURT
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(device_ids[cur_mag])));
#endif
(void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag);
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset)));
(void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag);

20
src/modules/systemlib/param/param_shmem.c
View File

@ -72,11 +72,7 @@
#include "shmem.h"
#if 0
# define debug(fmt, args...) do { warnx(fmt, ##args); } while(0)
#else
# define debug(fmt, args...) do { } while(0)
#endif
#define debug(fmt, args...) do { } while(0)
#define PARAM_OPEN open
#define PARAM_CLOSE close
@ -89,7 +85,7 @@ extern struct param_info_s param_array[];
extern struct param_info_s *param_info_base;
extern struct param_info_s *param_info_limit;
#else
// FIXME - start and end are reversed
// TODO: start and end are reversed
static struct param_info_s *param_info_base = (struct param_info_s *) &px4_parameters;
#endif
@ -187,7 +183,7 @@ param_unlock(void)
static void
param_assert_locked(void)
{
/* XXX */
/* TODO */
}
/**
@ -196,7 +192,6 @@ param_assert_locked(void)
* @param param The parameter handle to test.
* @return True if the handle is valid.
*/
//static bool
bool
handle_in_range(param_t param)
{
@ -241,19 +236,11 @@ param_find_changed(param_t param)
param_assert_locked();
if (param_values != NULL) {
#if 0 /* utarray_find requires bsearch, not available */
struct param_wbuf_s key;
key.param = param;
s = utarray_find(param_values, &key, param_compare_values);
#else
while ((s = (struct param_wbuf_s *)utarray_next(param_values, s)) != NULL) {
if (s->param == param) {
break;
}
}
#endif
}
return s;
@ -282,7 +269,6 @@ param_find_internal(const char *name, bool notification)
param_t param;
/* perform a linear search of the known parameters */
for (param = 0; handle_in_range(param); param++) {
if (!strcmp(param_info_base[param].name, name)) {
if (notification) {

71
src/platforms/posix/px4_layer/shmem_posix.c
View File

@ -1,7 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2015 Ramakrishna Kintada. All rights reserved.
* Copyright (c) 2015 Vijay Venkatraman. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -32,7 +31,6 @@
*
****************************************************************************/
#include <px4_defines.h>
#include <px4_posix.h>
#include <string.h>
@ -71,9 +69,9 @@ uint64_t update_from_shmem_prev_time = 0, update_from_shmem_current_time = 0;
static unsigned char adsp_changed_index[MAX_SHMEM_PARAMS / 8 + 1];
struct param_wbuf_s {
param_t param;
union param_value_u val;
bool unsaved;
param_t param;
union param_value_u val;
bool unsaved;
};
extern struct param_wbuf_s *param_find_changed(param_t param);
@ -88,7 +86,8 @@ static void *map_memory(off_t target)
}
/* Map one page */
map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, mem_fd, target & ~MAP_MASK);
map_base = (unsigned char *) mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE,
MAP_SHARED, mem_fd, target & ~MAP_MASK);
if (map_base == (void *) - 1) {
PX4_ERR("Cannot mmap /dev/atl_mem\n");
@ -109,10 +108,14 @@ int get_shmem_lock(void)
usleep(100000); //sleep for 100 msec
i++;
if (i > 100) { break; }
if (i > 100) {
break;
}
}
if (i > 100) { return -1; }
if (i > 100) {
return -1;
}
return 0; //got the lock
}
@ -126,14 +129,14 @@ void init_shared_memory(void)
{
virt_addr = map_memory(MAP_ADDRESS); //16K space
shmem_info_p = (struct shmem_info *)virt_addr;
shmem_info_p = (struct shmem_info *) virt_addr;
//PX4_INFO("linux memory mapped\n");
}
void copy_params_to_shmem(struct param_info_s *param_info_base)
{
param_t param;
param_t param;
unsigned int i;
if (get_shmem_lock() != 0) {
@ -145,17 +148,26 @@ void copy_params_to_shmem(struct param_info_s *param_info_base)
for (param = 0; param < param_count(); param++) {
struct param_wbuf_s *s = param_find_changed(param);
if (s == NULL) { shmem_info_p->params_val[param] = param_info_base[param].val; }
if (s == NULL) {
shmem_info_p->params_val[param] = param_info_base[param].val;
else { shmem_info_p->params_val[param] = s->val; }
} else {
shmem_info_p->params_val[param] = s->val;
}
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32) {
{PX4_INFO("%d: written %d for param %s to shared mem", param, shmem_info_p->params_val[param].i, param_name(param));}
{
PX4_INFO("%d: written %d for param %s to shared mem",
param, shmem_info_p->params_val[param].i, param_name(param));
}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
{PX4_INFO("%d: written %f for param %s to shared mem", param, (double)shmem_info_p->params_val[param].f, param_name(param));}
{
PX4_INFO("%d: written %f for param %s to shared mem",
param, (double)shmem_info_p->params_val[param].f, param_name(param));
}
}
#endif
@ -191,11 +203,14 @@ void update_to_shmem(param_t param, union param_value_u value)
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32)
{PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n", value.i, param_name(param), byte_changed, bit_changed);}
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO("Set value %d for param %s to shmem, set krait index %d:%d\n",
value.i, param_name(param), byte_changed, bit_changed);
else if (param_type(param) == PARAM_TYPE_FLOAT)
{PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n", (double)value.f, param_name(param), byte_changed, bit_changed);}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO("Set value %f for param %s to shmem, set krait index %d:%d\n",
(double)value.f, param_name(param), byte_changed, bit_changed);
}
#endif
@ -221,7 +236,6 @@ static void update_index_from_shmem(void)
release_shmem_lock();
}
static void update_value_from_shmem(param_t param, union param_value_u *value)
{
unsigned int byte_changed, bit_changed;
@ -242,11 +256,16 @@ static void update_value_from_shmem(param_t param, union param_value_u *value)
#ifdef SHMEM_DEBUG
if (param_type(param) == PARAM_TYPE_INT32)
{PX4_INFO("Got value %d for param %s from shmem, cleared adsp index %d:%d\n", value->i, param_name(param), byte_changed, bit_changed);}
if (param_type(param) == PARAM_TYPE_INT32) {
PX4_INFO(
"Got value %d for param %s from shmem, cleared adsp index %d:%d\n",
value->i, param_name(param), byte_changed, bit_changed);
else if (param_type(param) == PARAM_TYPE_FLOAT)
{PX4_INFO("Got value %f for param %s from shmem, cleared adsp index %d:%d\n", (double)value->f, param_name(param), byte_changed, bit_changed);}
} else if (param_type(param) == PARAM_TYPE_FLOAT) {
PX4_INFO(
"Got value %f for param %s from shmem, cleared adsp index %d:%d\n",
(double)value->f, param_name(param), byte_changed, bit_changed);
}
#endif
}
@ -258,7 +277,8 @@ int update_from_shmem(param_t param, union param_value_u *value)
update_from_shmem_current_time = hrt_absolute_time();
if ((update_from_shmem_current_time - update_from_shmem_prev_time) > 1000000) { //update every 1 second
if ((update_from_shmem_current_time - update_from_shmem_prev_time)
> 1000000) { //update every 1 second
update_from_shmem_prev_time = update_from_shmem_current_time;
update_index_from_shmem();
}
@ -279,4 +299,3 @@ int update_from_shmem(param_t param, union param_value_u *value)
return retval;
}