diff options
Diffstat (limited to 'apps/systemcmds/calibration')
-rw-r--r-- | apps/systemcmds/calibration/Makefile | 42 | ||||
-rwxr-xr-x | apps/systemcmds/calibration/calibration.c | 147 | ||||
-rw-r--r-- | apps/systemcmds/calibration/calibration.h | 60 | ||||
-rwxr-xr-x | apps/systemcmds/calibration/channels_cal.c | 196 | ||||
-rwxr-xr-x | apps/systemcmds/calibration/range_cal.c | 224 | ||||
-rw-r--r-- | apps/systemcmds/calibration/servo_cal.c | 264 |
6 files changed, 933 insertions, 0 deletions
diff --git a/apps/systemcmds/calibration/Makefile b/apps/systemcmds/calibration/Makefile new file mode 100644 index 000000000..aa1aa7761 --- /dev/null +++ b/apps/systemcmds/calibration/Makefile @@ -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 diff --git a/apps/systemcmds/calibration/calibration.c b/apps/systemcmds/calibration/calibration.c new file mode 100755 index 000000000..7f8b9240f --- /dev/null +++ b/apps/systemcmds/calibration/calibration.c @@ -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; +} diff --git a/apps/systemcmds/calibration/calibration.h b/apps/systemcmds/calibration/calibration.h new file mode 100644 index 000000000..028853ec8 --- /dev/null +++ b/apps/systemcmds/calibration/calibration.h @@ -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 diff --git a/apps/systemcmds/calibration/channels_cal.c b/apps/systemcmds/calibration/channels_cal.c new file mode 100755 index 000000000..575138b12 --- /dev/null +++ b/apps/systemcmds/calibration/channels_cal.c @@ -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; + +} + diff --git a/apps/systemcmds/calibration/range_cal.c b/apps/systemcmds/calibration/range_cal.c new file mode 100755 index 000000000..159a0d06b --- /dev/null +++ b/apps/systemcmds/calibration/range_cal.c @@ -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; + +} + diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c new file mode 100644 index 000000000..96b3045a9 --- /dev/null +++ b/apps/systemcmds/calibration/servo_cal.c @@ -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; + +} + |