aboutsummaryrefslogtreecommitdiff
path: root/apps/systemcmds/calibration/servo_cal.c
diff options
context:
space:
mode:
Diffstat (limited to 'apps/systemcmds/calibration/servo_cal.c')
-rw-r--r--apps/systemcmds/calibration/servo_cal.c264
1 files changed, 0 insertions, 264 deletions
diff --git a/apps/systemcmds/calibration/servo_cal.c b/apps/systemcmds/calibration/servo_cal.c
deleted file mode 100644
index 96b3045a9..000000000
--- a/apps/systemcmds/calibration/servo_cal.c
+++ /dev/null
@@ -1,264 +0,0 @@
-/****************************************************************************
- * 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;
-
-}
-
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-08 14:09:41 +0000