fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Separate timeouts for battery and high pwm

Browse Source
This commit is contained in:
Don Gagne 2015-05-22 11:24:07 -07:00
parent 25e7a1a49e
commit fe357a9a66
1 changed files with 15 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -68,12 +68,17 @@ static const int ERROR = -1;
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 start_time;
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");
@ -119,16 +124,20 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
start_time = hrt_absolute_time();
timeout_start = hrt_absolute_time();
while (true) {
if (hrt_absolute_time() - start_time > 5000000) {
// 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;
}
// 5 seconds at high pwm
// PWM was high long enough
break;
}
@ -137,9 +146,9 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
if (batt_updated) {
orb_copy(ORB_ID(battery_status), batt_sub, &battery);
if (battery.voltage_filtered_v > 3.0f) {
// Battery connected, wait for 5 seconds at high pwm
// Battery is connected, signal to user and start waiting again
batt_connected = true;
start_time = hrt_absolute_time();
timeout_start = hrt_absolute_time();
mavlink_and_console_log_info(mavlink_fd, "[cal] Battery connected");
}
}