forked from Archive/PX4-Autopilot
added esc calibration routines
This commit is contained in:
Roman Bapst
parent
bd1c3363df
commit
be03f98d64
|
|
@ -114,6 +114,7 @@
|
|||
#include "baro_calibration.h"
|
||||
#include "rc_calibration.h"
|
||||
#include "airspeed_calibration.h"
|
||||
#include "esc_calibration.h"
|
||||
#include "PreflightCheck.h"
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
|
|
@ -2708,6 +2709,16 @@ void *commander_low_prio_loop(void *arg)
|
|||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_airspeed_calibration(mavlink_fd);
|
||||
|
||||
} else if ((int)(cmd.param7) == 1) {
|
||||
/* do esc calibration */
|
||||
calib_ret = check_if_batt_disconnected(mavlink_fd);
|
||||
if(calib_ret == OK) {
|
||||
answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
armed.in_esc_calibration_mode = true;
|
||||
calib_ret = do_esc_calibration(mavlink_fd);
|
||||
armed.in_esc_calibration_mode = false;
|
||||
}
|
||||
|
||||
} else if ((int)(cmd.param4) == 0) {
|
||||
/* RC calibration ended - have we been in one worth confirming? */
|
||||
if (status.rc_input_blocked) {
|
||||
|
|
@ -2717,6 +2728,8 @@ void *commander_low_prio_loop(void *arg)
|
|||
mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN");
|
||||
calib_ret = OK;
|
||||
}
|
||||
/* this always succeeds */
|
||||
calib_ret = OK;
|
||||
}
|
||||
|
||||
if (calib_ret == OK) {
|
||||
|
|
|
|||
|
|
@ -0,0 +1,152 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file esc_calibration.cpp
|
||||
*
|
||||
* Definition of esc calibration
|
||||
*
|
||||
* @author Roman Bapst <bapstr@ethz.ch>
|
||||
*/
|
||||
|
||||
#include "esc_calibration.h"
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string.h>
|
||||
#include <stdbool.h>
|
||||
#include <unistd.h>
|
||||
#include <sys/ioctl.h>
|
||||
#include <systemlib/err.h>
|
||||
#include <fcntl.h>
|
||||
#include <poll.h>
|
||||
#include "drivers/drv_pwm_output.h"
|
||||
#include <uORB/topics/battery_status.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/uORB.h>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <mavlink/mavlink_log.h>
|
||||
|
||||
/* oddly, ERROR is not defined for c++ */
|
||||
#ifdef ERROR
|
||||
# undef ERROR
|
||||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
int check_if_batt_disconnected(int mavlink_fd) {
|
||||
struct battery_status_s battery;
|
||||
memset(&battery,0,sizeof(battery));
|
||||
int batt_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
|
||||
|
||||
if (battery.voltage_filtered_v > 3.0f && !(hrt_absolute_time() - battery.timestamp > 500000)) {
|
||||
mavlink_log_info(mavlink_fd, "Please disconnect battery and try again!");
|
||||
return ERROR;
|
||||
}
|
||||
return OK;
|
||||
}
|
||||
|
||||
|
||||
int do_esc_calibration(int mavlink_fd) {
|
||||
|
||||
int fd = open(PWM_OUTPUT0_DEVICE_PATH, 0);
|
||||
int ret;
|
||||
|
||||
if(fd < 0) {
|
||||
err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
|
||||
}
|
||||
|
||||
/* tell IO/FMU that its ok to disable its safety with the switch */
|
||||
ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_SET_ARM_OK");
|
||||
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
|
||||
ret = ioctl(fd, PWM_SERVO_ARM, 0);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_ARM");
|
||||
/* tell IO to switch off safety without using the safety switch */
|
||||
ret = ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
|
||||
if(ret!=0) {
|
||||
err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd,"Please connect battery now");
|
||||
|
||||
struct battery_status_s battery;
|
||||
memset(&battery,0,sizeof(battery));
|
||||
int batt_sub = orb_subscribe(ORB_ID(battery_status));
|
||||
orb_copy(ORB_ID(vehicle_command),batt_sub, &battery);
|
||||
bool updated = false;
|
||||
|
||||
struct vehicle_command_s cmd;
|
||||
memset(&cmd, 0, sizeof(cmd));
|
||||
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
|
||||
orb_copy(ORB_ID(vehicle_command),cmd_sub, &cmd);
|
||||
|
||||
/* wait for one of the following events:
|
||||
1) user has pressed the button in QGroundControl
|
||||
2) timeout of 5 seconds is reached
|
||||
*/
|
||||
hrt_abstime start_time = hrt_absolute_time();
|
||||
|
||||
while(true) {
|
||||
orb_check(batt_sub,&updated);
|
||||
if(updated) {
|
||||
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
|
||||
}
|
||||
// user has connected battery
|
||||
if(battery.voltage_filtered_v > 3.0f) {
|
||||
orb_check(cmd_sub,&updated);
|
||||
if(updated) {
|
||||
orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
|
||||
}
|
||||
if((int)(cmd.param7) == 2 && cmd.command == VEHICLE_CMD_PREFLIGHT_CALIBRATION) {
|
||||
break;
|
||||
} else if (hrt_absolute_time() - start_time > 5000000) {
|
||||
// waited for 5 seconds, switch to low pwm
|
||||
break;
|
||||
}
|
||||
}
|
||||
else {
|
||||
start_time = hrt_absolute_time();
|
||||
}
|
||||
usleep(50000);
|
||||
}
|
||||
|
||||
/* disarm */
|
||||
ret = ioctl(fd, PWM_SERVO_DISARM, 0);
|
||||
if (ret != OK)
|
||||
err(1, "PWM_SERVO_DISARM");
|
||||
|
||||
mavlink_log_info(mavlink_fd,"ESC calibration finished");
|
||||
return OK;
|
||||
}
|
||||
|
|
@ -0,0 +1,47 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2015 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file esc_calibration.h
|
||||
*
|
||||
* Definition of esc calibration
|
||||
*
|
||||
* @author Roman Bapst <bapstr@ethz.ch>
|
||||
*/
|
||||
|
||||
#ifndef ESC_CALIBRATION_H_
|
||||
#define ESC_CALIBRATION_H_
|
||||
int check_if_batt_disconnected(int mavlink_fd);
|
||||
int do_esc_calibration(int mavlink_fd);
|
||||
|
||||
#endif
|
||||
|
|
@ -47,6 +47,7 @@ SRCS = commander.cpp \
|
|||
baro_calibration.cpp \
|
||||
rc_calibration.cpp \
|
||||
airspeed_calibration.cpp \
|
||||
esc_calibration.cpp \
|
||||
PreflightCheck.cpp
|
||||
|
||||
MODULE_STACKSIZE = 5000
|
||||
|
|
|
|||
Loading…
Reference in New Issue