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

Merge pull request #2216 from DonLakeFlyer/CalESC 

ESC Calibration
This commit is contained in:
Lorenz Meier 2015-05-22 22:27:11 +02:00
parent d385a7f483 fe357a9a66
commit 0bd0f7dae5
4 changed files with 114 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/modules/commander/calibration_messages.h
View File

@ -52,6 +52,7 @@
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s"
#define CAL_QGC_DONE_MSG "[cal] calibration done: %s"
#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s"
#define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s"
#define CAL_QGC_CANCELLED_MSG "[cal] calibration cancelled"
#define CAL_QGC_PROGRESS_MSG "[cal] progress <%u>"
#define CAL_QGC_ORIENTATION_DETECTED_MSG "[cal] %s orientation detected"

13
src/modules/commander/commander.cpp
View File

@ -326,6 +326,8 @@ int commander_main(int argc, char *argv[])
calib_ret = do_gyro_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "level")) {
calib_ret = do_level_calibration(mavlink_fd);
} else if (!strcmp(argv[2], "esc")) {
calib_ret = do_esc_calibration(mavlink_fd, &armed);
} else {
warnx("argument %s unsupported.", argv[2]);
}
@ -2744,14 +2746,9 @@ void *commander_low_prio_loop(void *arg)
} 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;
}
answer_command(cmd,VEHICLE_CMD_RESULT_ACCEPTED);
calib_ret = do_esc_calibration(mavlink_fd, &armed);
} else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */
if (status.rc_input_blocked) {

175
src/modules/commander/esc_calibration.cpp
View File

@ -40,6 +40,8 @@
*/
#include "esc_calibration.h"
#include "calibration_messages.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
@ -63,91 +65,122 @@
#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;
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
{
int return_code = OK;
int fd = -1;
struct battery_status_s battery;
int batt_sub = -1;
bool batt_updated = false;
bool batt_connected = false;
hrt_abstime battery_connect_wait_timeout = 20000000;
hrt_abstime pwm_high_timeout = 5000000;
hrt_abstime timeout_start;
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
batt_sub = orb_subscribe(ORB_ID(battery_status));
if (batt_sub < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
}
return OK;
}
// Make sure battery is disconnected
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Disconnect battery and try again");
goto Error;
}
armed->in_esc_calibration_mode = true;
fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
int do_esc_calibration(int mavlink_fd) {
int fd = px4_open(PWM_OUTPUT0_DEVICE_PATH, 0);
int ret;
if(fd < 0) {
err(1,"Can't open %s", PWM_OUTPUT0_DEVICE_PATH);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
goto Error;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
ret = px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
if (ret != OK)
err(1, "PWM_SERVO_SET_ARM_OK");
if (px4_ioctl(fd, PWM_SERVO_SET_ARM_OK, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
ret = px4_ioctl(fd, PWM_SERVO_ARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_ARM");
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
ret = px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0);
if(ret!=0) {
err(1,"PWM_SERVO_SET_FORCE_SAFETY_OFF");
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
mavlink_log_info(mavlink_fd,"Please connect battery now");
mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
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;
while (true) {
// We are either waiting for the user to connect the battery. Or we are waiting to let the PWM
// sit high.
hrt_abstime timeout_wait = batt_connected ? pwm_high_timeout : battery_connect_wait_timeout;
if (hrt_absolute_time() - timeout_start > timeout_wait) {
if (!batt_connected) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
goto Error;
}
// PWM was high long enough
break;
}
else {
start_time = hrt_absolute_time();
if (!batt_connected) {
orb_check(batt_sub, &batt_updated);
if (batt_updated) {
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
// Battery is connected, signal to user and start waiting again
batt_connected = true;
timeout_start = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
}
}
}
usleep(50000);
}
/* disarm */
ret = px4_ioctl(fd, PWM_SERVO_DISARM, 0);
if (ret != OK)
err(1, "PWM_SERVO_DISARM");
mavlink_log_info(mavlink_fd,"ESC calibration finished");
return OK;
}
Out:
if (batt_sub != -1) {
orb_unsubscribe(batt_sub);
}
if (fd != -1) {
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_ON, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still off");
}
if (px4_ioctl(fd, PWM_SERVO_DISARM, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Servos still armed");
}
if (px4_ioctl(fd, PWM_SERVO_CLEAR_ARM_OK, 0) != OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_WARNING_MSG, "Safety switch still deactivated");
}
px4_close(fd);
}
armed->in_esc_calibration_mode = false;
if (return_code == OK) {
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
Error:
return_code = ERROR;
goto Out;
}

6
src/modules/commander/esc_calibration.h
View File

@ -41,7 +41,9 @@
#ifndef ESC_CALIBRATION_H_
#define ESC_CALIBRATION_H_
int check_if_batt_disconnected(int mavlink_fd);
int do_esc_calibration(int mavlink_fd);
#include <uORB/topics/actuator_armed.h>
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed);
#endif