mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-11-05 03:21:36 +08:00
Fresh import of the PX4 firmware sources.
This commit is contained in:
px4dev
@@ -0,0 +1,42 @@
|
||||
############################################################################
|
||||
#
|
||||
# Copyright (C) 2012 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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Makefile to build the calibration tool
|
||||
#
|
||||
|
||||
APPNAME = calibration
|
||||
PRIORITY = SCHED_PRIORITY_MAX - 1
|
||||
STACKSIZE = 4096
|
||||
|
||||
include $(APPDIR)/mk/app.mk
|
||||
Executable
+147
@@ -0,0 +1,147 @@
|
||||
/****************************************************************************
|
||||
* calibration.c
|
||||
*
|
||||
* Copyright (C) 2012 Ivan Ovinnikov. All rights reserved.
|
||||
* Authors: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include "calibration.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Private Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
static int calibrate_help(int argc, char *argv[]);
|
||||
static int calibrate_all(int argc, char *argv[]);
|
||||
|
||||
__EXPORT int calibration_main(int argc, char *argv[]);
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
|
||||
struct {
|
||||
const char *name;
|
||||
int (* fn)(int argc, char *argv[]);
|
||||
unsigned options;
|
||||
#define OPT_NOHELP (1<<0)
|
||||
#define OPT_NOALLTEST (1<<1)
|
||||
} calibrates[] = {
|
||||
{"range", range_cal, 0},
|
||||
{"servo", servo_cal, 0},
|
||||
{"all", calibrate_all, OPT_NOALLTEST},
|
||||
{"help", calibrate_help, OPT_NOALLTEST | OPT_NOHELP},
|
||||
{NULL, NULL}
|
||||
};
|
||||
|
||||
/****************************************************************************
|
||||
* Public Data
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
static int
|
||||
calibrate_help(int argc, char *argv[])
|
||||
{
|
||||
unsigned i;
|
||||
|
||||
printf("Available calibration routines:\n");
|
||||
|
||||
for (i = 0; calibrates[i].name; i++)
|
||||
printf(" %s\n", calibrates[i].name);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static int
|
||||
calibrate_all(int argc, char *argv[])
|
||||
{
|
||||
unsigned i;
|
||||
char *args[2] = {"all", NULL};
|
||||
|
||||
printf("Running all calibration routines...\n\n");
|
||||
|
||||
for (i = 0; calibrates[i].name; i++) {
|
||||
printf(" %s:\n", calibrates[i].name);
|
||||
|
||||
if (calibrates[i].fn(1, args)) {
|
||||
printf(" FAIL\n");
|
||||
|
||||
} else {
|
||||
printf(" DONE\n");
|
||||
}
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
|
||||
void press_enter(void)
|
||||
{
|
||||
int c;
|
||||
printf("Press CTRL+ENTER to continue... \n");
|
||||
fflush(stdout);
|
||||
|
||||
do c = getchar(); while ((c != '\n') && (c != EOF));
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Name: calibrate_main
|
||||
****************************************************************************/
|
||||
|
||||
int calibration_main(int argc, char *argv[])
|
||||
{
|
||||
unsigned i;
|
||||
|
||||
if (argc < 2) {
|
||||
printf("calibration: missing name - 'calibration help' for a list of routines\n");
|
||||
return 1;
|
||||
}
|
||||
|
||||
for (i = 0; calibrates[i].name; i++) {
|
||||
if (!strcmp(calibrates[i].name, argv[1]))
|
||||
return calibrates[i].fn(argc - 1, argv + 1);
|
||||
}
|
||||
|
||||
printf("calibrate: no routines called '%s' - 'calibration help' for a list of routines\n", argv[1]);
|
||||
return 1;
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2012 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#ifndef _CALIBRATION_H
|
||||
#define _CALIBRATION_H
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Definitions
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Types
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Variables
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Public Function Prototypes
|
||||
****************************************************************************/
|
||||
|
||||
extern int range_cal(int argc, char *argv[]);
|
||||
extern int servo_cal(int argc, char *argv[]);
|
||||
|
||||
#endif
|
||||
Executable
+196
@@ -0,0 +1,196 @@
|
||||
/****************************************************************************
|
||||
* channels_cal.c
|
||||
*
|
||||
* Copyright (C) 2012 Nils Wenzler. All rights reserved.
|
||||
* Authors: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <string.h>
|
||||
#include <stdlib.h>
|
||||
#include "calibration.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Defines
|
||||
****************************************************************************/
|
||||
#define ABS(a) (((a) < 0) ? -(a) : (a))
|
||||
#define MIN(a,b) (((a)<(b))?(a):(b))
|
||||
#define MAX(a,b) (((a)>(b))?(a):(b))
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
//Store the values here before writing them to global_data_rc_channels
|
||||
uint16_t old_values[NR_CHANNELS];
|
||||
uint16_t cur_values[NR_CHANNELS];
|
||||
uint8_t function_map[NR_CHANNELS];
|
||||
char names[12][9];
|
||||
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
void press_enter_2(void)
|
||||
{
|
||||
int c;
|
||||
printf("Press CTRL+ENTER to continue... \n");
|
||||
fflush(stdout);
|
||||
|
||||
do c = getchar(); while ((c != '\n') && (c != EOF));
|
||||
}
|
||||
|
||||
/**This gets all current values and writes them to min or max
|
||||
*/
|
||||
uint8_t get_val(uint16_t *buffer)
|
||||
{
|
||||
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
printf("Channel: %i\t RAW Value: %i: \n", i, global_data_rc_channels->chan[i].raw);
|
||||
buffer[i] = global_data_rc_channels->chan[i].raw;
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
return 0;
|
||||
|
||||
} else
|
||||
return -1;
|
||||
}
|
||||
|
||||
void set_channel(void)
|
||||
{
|
||||
static uint8_t j = 0;
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
if (ABS(old_values - cur_values) > 50) {
|
||||
function_map[j] = i;
|
||||
strcpy(names[i], global_data_rc_channels->function_names[j]);
|
||||
j++;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void write_dat(void)
|
||||
{
|
||||
global_data_lock(&global_data_rc_channels->access_conf);
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
global_data_rc_channels->function[i] = function_map[i];
|
||||
strcpy(global_data_rc_channels->chan[i].name, names[i]);
|
||||
|
||||
printf("Channel %i\t Function %s\n", i,
|
||||
global_data_rc_channels->chan[i].name);
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
int channels_cal(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("This routine maps the input functions on the remote control\n");
|
||||
printf("to the corresponding function (Throttle, Roll,..)\n");
|
||||
printf("Always move the stick all the way\n");
|
||||
press_enter_2();
|
||||
|
||||
printf("Pull the THROTTLE stick down\n");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(old_values));
|
||||
|
||||
printf("Move the THROTTLE stick up\n ");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(cur_values));
|
||||
|
||||
set_channel();
|
||||
|
||||
printf("Pull the PITCH stick down\n");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(old_values));
|
||||
|
||||
printf("Move the PITCH stick up\n ");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(cur_values));
|
||||
|
||||
set_channel();
|
||||
|
||||
printf("Put the ROLL stick to the left\n");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(old_values));
|
||||
|
||||
printf("Put the ROLL stick to the right\n ");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(cur_values));
|
||||
|
||||
set_channel();
|
||||
|
||||
printf("Put the YAW stick to the left\n");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(old_values));
|
||||
|
||||
printf("Put the YAW stick to the right\n ");
|
||||
press_enter_2();
|
||||
|
||||
while (get_val(cur_values));
|
||||
|
||||
set_channel();
|
||||
|
||||
uint8_t k;
|
||||
|
||||
for (k = 5; k < NR_CHANNELS; k++) {
|
||||
function_map[k] = k;
|
||||
strcpy(names[k], global_data_rc_channels->function_names[k]);
|
||||
}
|
||||
|
||||
//write the values to global_data_rc_channels
|
||||
write_dat();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
Executable
+224
@@ -0,0 +1,224 @@
|
||||
/****************************************************************************
|
||||
* range_cal.c
|
||||
*
|
||||
* Copyright (C) 2012 Nils Wenzler. All rights reserved.
|
||||
* Authors: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include "calibration.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Defines
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
//Store the values here before writing them to global_data_rc_channels
|
||||
uint16_t max_values[NR_CHANNELS];
|
||||
uint16_t min_values[NR_CHANNELS];
|
||||
uint16_t mid_values[NR_CHANNELS];
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/**This sets the middle values
|
||||
*/
|
||||
uint8_t set_mid(void)
|
||||
{
|
||||
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
if (i == global_data_rc_channels->function[ROLL] ||
|
||||
i == global_data_rc_channels->function[YAW] ||
|
||||
i == global_data_rc_channels->function[PITCH]) {
|
||||
|
||||
mid_values[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else {
|
||||
mid_values[i] = (max_values[i] + min_values[i]) / 2;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
return 0;
|
||||
|
||||
} else
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**This gets all current values and writes them to min or max
|
||||
*/
|
||||
uint8_t get_value(void)
|
||||
{
|
||||
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
//see if the value is bigger or smaller than 1500 (roughly neutral)
|
||||
//and write to the appropriate array
|
||||
if (global_data_rc_channels->chan[i].raw != 0 &&
|
||||
global_data_rc_channels->chan[i].raw < 1500) {
|
||||
min_values[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else if (global_data_rc_channels->chan[i].raw != 0 &&
|
||||
global_data_rc_channels->chan[i].raw > 1500) {
|
||||
max_values[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else {
|
||||
max_values[i] = 0;
|
||||
min_values[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
return 0;
|
||||
|
||||
} else
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
void write_data(void)
|
||||
{
|
||||
// global_data_lock(&global_data_rc_channels->access_conf);
|
||||
// uint8_t i;
|
||||
// for(i=0; i < NR_CHANNELS; i++){
|
||||
// //Write the data to global_data_rc_channels (if not 0)
|
||||
// if(mid_values[i]!=0 && min_values[i]!=0 && max_values[i]!=0){
|
||||
// global_data_rc_channels->chan[i].scaling_factor =
|
||||
// 10000/((max_values[i] - min_values[i])/2);
|
||||
//
|
||||
// global_data_rc_channels->chan[i].mid = mid_values[i];
|
||||
// }
|
||||
//
|
||||
// printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
|
||||
// i,
|
||||
// global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
|
||||
// min_values[i], max_values[i],
|
||||
// global_data_rc_channels->chan[i].scaling_factor,
|
||||
// global_data_rc_channels->chan[i].mid);
|
||||
// }
|
||||
// global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
|
||||
//Write to the Parameter storage
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC1_MIN] = min_values[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC2_MIN] = min_values[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC3_MIN] = min_values[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC4_MIN] = min_values[3];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC5_MIN] = min_values[4];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC6_MIN] = min_values[5];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC7_MIN] = min_values[6];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC8_MIN] = min_values[7];
|
||||
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC1_MAX] = max_values[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC2_MAX] = max_values[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC3_MAX] = max_values[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC4_MAX] = max_values[3];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC5_MAX] = max_values[4];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC6_MAX] = max_values[5];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC7_MAX] = max_values[6];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC8_MAX] = max_values[7];
|
||||
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC1_TRIM] = mid_values[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC2_TRIM] = mid_values[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC3_TRIM] = mid_values[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC4_TRIM] = mid_values[3];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC5_TRIM] = mid_values[4];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC6_TRIM] = mid_values[5];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC7_TRIM] = mid_values[6];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_RC8_TRIM] = mid_values[7];
|
||||
|
||||
usleep(3e6);
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
|
||||
i,
|
||||
min_values[i], max_values[i],
|
||||
global_data_rc_channels->chan[i].scaling_factor,
|
||||
global_data_rc_channels->chan[i].mid);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
int range_cal(int argc, char *argv[])
|
||||
{
|
||||
|
||||
printf("The range calibration routine assumes you already did the channel\n");
|
||||
printf("assignment\n");
|
||||
printf("This routine chooses the minimum, maximum and middle\n");
|
||||
printf("value for each channel separatly. The ROLL, PITCH and YAW function\n");
|
||||
printf("get their middle value from the RC direct, for the rest it is\n");
|
||||
printf("calculated out of the min and max values.\n");
|
||||
press_enter();
|
||||
|
||||
printf("Hold both sticks in lower left corner and continue\n ");
|
||||
press_enter();
|
||||
usleep(500000);
|
||||
|
||||
while (get_value());
|
||||
|
||||
printf("Hold both sticks in upper right corner and continue\n");
|
||||
press_enter();
|
||||
usleep(500000);
|
||||
|
||||
while (get_value());
|
||||
|
||||
printf("Set the trim to 0 and leave the sticks in the neutral position\n");
|
||||
press_enter();
|
||||
|
||||
//Loop until successfull
|
||||
while (set_mid());
|
||||
|
||||
//write the values to global_data_rc_channels
|
||||
write_data();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,264 @@
|
||||
/****************************************************************************
|
||||
* servo_cal.c
|
||||
*
|
||||
* Copyright (C) 2012 Nils Wenzler. All rights reserved.
|
||||
* Authors: Nils Wenzler <wenzlern@ethz.ch>
|
||||
*
|
||||
* 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 NuttX 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Included Files
|
||||
****************************************************************************/
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <arch/board/drv_pwm_servo.h>
|
||||
#include <fcntl.h>
|
||||
#include "calibration.h"
|
||||
|
||||
/****************************************************************************
|
||||
* Defines
|
||||
****************************************************************************/
|
||||
|
||||
/****************************************************************************
|
||||
* Private Data
|
||||
****************************************************************************/
|
||||
//Store the values here before writing them to global_data_rc_channels
|
||||
uint16_t max_values_servo[PWM_SERVO_MAX_CHANNELS];
|
||||
uint16_t min_values_servo[PWM_SERVO_MAX_CHANNELS];
|
||||
uint16_t mid_values_servo[PWM_SERVO_MAX_CHANNELS];
|
||||
|
||||
// Servo loop thread
|
||||
|
||||
pthread_t servo_calib_thread;
|
||||
|
||||
/****************************************************************************
|
||||
* Private Functions
|
||||
****************************************************************************/
|
||||
|
||||
/**This sets the middle values
|
||||
*/
|
||||
uint8_t set_mid_s(void)
|
||||
{
|
||||
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
|
||||
if (i == global_data_rc_channels->function[ROLL] ||
|
||||
i == global_data_rc_channels->function[YAW] ||
|
||||
i == global_data_rc_channels->function[PITCH]) {
|
||||
|
||||
mid_values_servo[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else {
|
||||
mid_values_servo[i] = (max_values_servo[i] + min_values_servo[i]) / 2;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
return 0;
|
||||
|
||||
} else
|
||||
return -1;
|
||||
}
|
||||
|
||||
/**This gets all current values and writes them to min or max
|
||||
*/
|
||||
uint8_t get_value_s(void)
|
||||
{
|
||||
if (0 == global_data_trylock(&global_data_rc_channels->access_conf)) {
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < PWM_SERVO_MAX_CHANNELS; i++) {
|
||||
//see if the value is bigger or smaller than 1500 (roughly neutral)
|
||||
//and write to the appropriate array
|
||||
if (global_data_rc_channels->chan[i].raw != 0 &&
|
||||
global_data_rc_channels->chan[i].raw < 1500) {
|
||||
min_values_servo[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else if (global_data_rc_channels->chan[i].raw != 0 &&
|
||||
global_data_rc_channels->chan[i].raw > 1500) {
|
||||
max_values_servo[i] = global_data_rc_channels->chan[i].raw;
|
||||
|
||||
} else {
|
||||
max_values_servo[i] = 0;
|
||||
min_values_servo[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
return 0;
|
||||
|
||||
} else
|
||||
return -1;
|
||||
}
|
||||
|
||||
|
||||
void write_data_s(void)
|
||||
{
|
||||
// global_data_lock(&global_data_rc_channels->access_conf);
|
||||
// uint8_t i;
|
||||
// for(i=0; i < NR_CHANNELS; i++){
|
||||
// //Write the data to global_data_rc_channels (if not 0)
|
||||
// if(mid_values_servo[i]!=0 && min_values_servo[i]!=0 && max_values_servo[i]!=0){
|
||||
// global_data_rc_channels->chan[i].scaling_factor =
|
||||
// 10000/((max_values_servo[i] - min_values_servo[i])/2);
|
||||
//
|
||||
// global_data_rc_channels->chan[i].mid = mid_values_servo[i];
|
||||
// }
|
||||
//
|
||||
// printf("Channel %i\t Function %s \t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
|
||||
// i,
|
||||
// global_data_rc_channels->function_name[global_data_rc_channels->function[i]],
|
||||
// min_values_servo[i], max_values_servo[i],
|
||||
// global_data_rc_channels->chan[i].scaling_factor,
|
||||
// global_data_rc_channels->chan[i].mid);
|
||||
// }
|
||||
// global_data_unlock(&global_data_rc_channels->access_conf);
|
||||
|
||||
//Write to the Parameter storage
|
||||
|
||||
|
||||
|
||||
global_data_lock(&global_data_parameter_storage->access_conf);
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MIN] = min_values_servo[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MIN] = min_values_servo[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MIN] = min_values_servo[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MIN] = min_values_servo[3];
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_MAX] = max_values_servo[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_MAX] = max_values_servo[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_MAX] = max_values_servo[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_MAX] = max_values_servo[3];
|
||||
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM] = mid_values_servo[0];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM] = mid_values_servo[1];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM] = mid_values_servo[2];
|
||||
global_data_parameter_storage->pm.param_values[PARAM_SERVO4_TRIM] = mid_values_servo[3];
|
||||
|
||||
global_data_unlock(&global_data_parameter_storage->access_conf);
|
||||
|
||||
usleep(3e6);
|
||||
uint8_t i;
|
||||
|
||||
for (i = 0; i < NR_CHANNELS; i++) {
|
||||
printf("Channel %i:\t\t Min %i\t\t Max %i\t\t Scaling Factor: %i\t Middle Value %i\n",
|
||||
i,
|
||||
min_values_servo[i], max_values_servo[i],
|
||||
global_data_rc_channels->chan[i].scaling_factor,
|
||||
global_data_rc_channels->chan[i].mid);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static void *servo_loop(void *arg)
|
||||
{
|
||||
|
||||
// Set thread name
|
||||
prctl(1, "calibration servo", getpid());
|
||||
|
||||
// initialize servos
|
||||
int fd;
|
||||
servo_position_t data[PWM_SERVO_MAX_CHANNELS];
|
||||
|
||||
fd = open("/dev/pwm_servo", O_RDWR);
|
||||
|
||||
if (fd < 0) {
|
||||
printf("failed opening /dev/pwm_servo\n");
|
||||
}
|
||||
|
||||
ioctl(fd, PWM_SERVO_ARM, 0);
|
||||
|
||||
while (1) {
|
||||
int i;
|
||||
|
||||
for (i = 0; i < 4; i++) {
|
||||
data[i] = global_data_rc_channels->chan[global_data_rc_channels->function[THROTTLE]].raw;
|
||||
}
|
||||
|
||||
int result = write(fd, &data, sizeof(data));
|
||||
|
||||
if (result != sizeof(data)) {
|
||||
printf("failed bulk-reading channel values\n");
|
||||
}
|
||||
|
||||
// 5Hz loop
|
||||
usleep(200000);
|
||||
}
|
||||
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
/****************************************************************************
|
||||
* Public Functions
|
||||
****************************************************************************/
|
||||
int servo_cal(int argc, char *argv[])
|
||||
{
|
||||
// pthread_attr_t servo_loop_attr;
|
||||
// pthread_attr_init(&servo_loop_attr);
|
||||
// pthread_attr_setstacksize(&servo_loop_attr, 1024);
|
||||
pthread_create(&servo_calib_thread, NULL, servo_loop, NULL);
|
||||
pthread_join(servo_calib_thread, NULL);
|
||||
|
||||
printf("The servo calibration routine assumes you already did the channel\n");
|
||||
printf("assignment with 'calibration channels'\n");
|
||||
printf("This routine chooses the minimum, maximum and middle\n");
|
||||
printf("value for each channel separately. The ROLL, PITCH and YAW function\n");
|
||||
printf("get their middle value from the RC direct, for the rest it is\n");
|
||||
printf("calculated out of the min and max values.\n");
|
||||
press_enter();
|
||||
|
||||
printf("Hold both sticks in lower left corner and continue\n ");
|
||||
press_enter();
|
||||
usleep(500000);
|
||||
|
||||
while (get_value_s());
|
||||
|
||||
printf("Hold both sticks in upper right corner and continue\n");
|
||||
press_enter();
|
||||
usleep(500000);
|
||||
|
||||
while (get_value_s());
|
||||
|
||||
printf("Set the trim to 0 and leave the sticks in the neutral position\n");
|
||||
press_enter();
|
||||
|
||||
//Loop until successfull
|
||||
while (set_mid_s());
|
||||
|
||||
//write the values to global_data_rc_channels
|
||||
write_data_s();
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user