fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-09 17:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

drivers/modules: changes after mavlink_log change

Browse Source 
The mavlink_log API changes lead to changes in all drivers/modules using
it.
This commit is contained in:
Julian Oes
2016-03-24 13:10:02 +01:00
parent b49b012d35
commit bba0d0384d
62 changed files with 731 additions and 831 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/esc_calibration.cpp
+40 -40
View File
@@ -57,7 +57,7 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/uORB.h>
#include <drivers/drv_hrt.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/mavlink_log.h>
/* oddly, ERROR is not defined for c++ */
#ifdef ERROR
@@ -65,82 +65,82 @@
#endif
static const int ERROR = -1;
int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
int do_esc_calibration(orb_advert_t *mavlink_log_pub, 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 = 30000000;
hrt_abstime pwm_high_timeout = 10000000;
hrt_abstime timeout_start;
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, "esc");
mavlink_and_console_log_info(mavlink_log_pub, 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");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Subscribe to battery");
goto Error;
}
// 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");
mavlink_and_console_log_critical(mavlink_log_pub, 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);
if (fd < 0) {
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "Can't open PWM device");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Can't open PWM device");
goto Error;
}
/* tell IO/FMU that its ok to disable its safety with the switch */
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) */
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 */
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");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to disable safety switch");
goto Error;
}
mavlink_and_console_log_info(mavlink_fd, "[cal] Connect battery now");
/* tell IO/FMU that the system is armed (it will output values if safety is off) */
if (px4_ioctl(fd, PWM_SERVO_ARM, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to arm system");
goto Error;
}
/* tell IO to switch off safety without using the safety switch */
if (px4_ioctl(fd, PWM_SERVO_SET_FORCE_SAFETY_OFF, 0) != OK) {
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Unable to force safety off");
goto Error;
}
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Connect battery now");
timeout_start = hrt_absolute_time();
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");
mavlink_and_console_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "Timeout waiting for battery");
goto Error;
}
// PWM was high long enough
break;
}
if (!batt_connected) {
orb_check(batt_sub, &batt_updated);
if (batt_updated) {
@@ -149,7 +149,7 @@ int do_esc_calibration(int mavlink_fd, struct actuator_armed_s* armed)
// 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");
mavlink_and_console_log_info(mavlink_log_pub, "[cal] Battery connected");
}
}
}
@@ -162,24 +162,24 @@ Out:
}
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");
mavlink_and_console_log_info(mavlink_log_pub, 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");
mavlink_and_console_log_info(mavlink_log_pub, 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");
mavlink_and_console_log_info(mavlink_log_pub, 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");
mavlink_and_console_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "esc");
}
return return_code;
Error:
return_code = ERROR;
goto Out;