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

Merged master

Browse Source
This commit is contained in:
Lorenz Meier
2013-02-26 21:17:48 +01:00
parent dffe05d893 a9b933b7e6
commit c848fd1d63
9 changed files with 227 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+119 -1
View File
@@ -80,6 +80,7 @@
#include <uORB/topics/subsystem_info.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <mavlink/mavlink_log.h>
#include <systemlib/param/param.h>
@@ -785,6 +786,79 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
close(sub_sensor_combined);
}
void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status)
{
/* announce change */
mavlink_log_info(mavlink_fd, "keep it still");
/* set to accel calibration mode */
status->flag_preflight_airspeed_calibration = true;
state_machine_publish(status_pub, status, mavlink_fd);
const int calibration_count = 2500;
int sub_differential_pressure = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
int calibration_counter = 0;
float airspeed_offset = 0.0f;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_differential_pressure, .events = POLLIN } };
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(differential_pressure), sub_differential_pressure, &differential_pressure);
airspeed_offset += differential_pressure.voltage;
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
return;
}
}
airspeed_offset = airspeed_offset / calibration_count;
if (isfinite(airspeed_offset)) {
if (param_set(param_find("SENS_VAIR_OFF"), &(airspeed_offset))) {
mavlink_log_critical(mavlink_fd, "Setting offs failed!");
}
/* auto-save to EEPROM */
int save_ret = param_save_default();
if (save_ret != 0) {
warn("WARNING: auto-save of params to storage failed");
}
//char buf[50];
//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
//mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "airspeed calibration done");
tune_confirm();
sleep(2);
tune_confirm();
sleep(2);
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
}
/* exit airspeed calibration mode */
status->flag_preflight_airspeed_calibration = false;
state_machine_publish(status_pub, status, mavlink_fd);
close(sub_differential_pressure);
}
void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd)
@@ -980,6 +1054,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta
handled = true;
}
/* airspeed calibration */
if ((int)(cmd->param6) == 1) { //xxx: this is not defined by the mavlink protocol
/* transition to calibration state */
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT);
if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) {
mavlink_log_info(mavlink_fd, "CMD starting airspeed cal");
tune_confirm();
do_airspeed_calibration(status_pub, &current_status);
tune_confirm();
mavlink_log_info(mavlink_fd, "CMD finished airspeed cal");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = VEHICLE_CMD_RESULT_ACCEPTED;
} else {
mavlink_log_critical(mavlink_fd, "REJECTING airspeed cal");
result = VEHICLE_CMD_RESULT_DENIED;
}
handled = true;
}
/* none found */
if (!handled) {
//warnx("refusing unsupported calibration request\n");
@@ -1381,6 +1477,11 @@ int commander_thread_main(int argc, char *argv[])
struct sensor_combined_s sensors;
memset(&sensors, 0, sizeof(sensors));
int differential_pressure_sub = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
memset(&differential_pressure, 0, sizeof(differential_pressure));
uint64_t last_differential_pressure_time = 0;
/* Subscribe to command topic */
int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
struct vehicle_command_s cmd;
@@ -1434,6 +1535,13 @@ int commander_thread_main(int argc, char *argv[])
orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors);
}
orb_check(differential_pressure_sub, &new_data);
if (new_data) {
orb_copy(ORB_ID(differential_pressure), differential_pressure_sub, &differential_pressure);
last_differential_pressure_time = differential_pressure.timestamp;
}
orb_check(cmd_sub, &new_data);
if (new_data) {
@@ -1626,6 +1734,7 @@ int commander_thread_main(int argc, char *argv[])
bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
bool global_pos_valid = current_status.flag_global_position_valid;
bool local_pos_valid = current_status.flag_local_position_valid;
bool airspeed_valid = current_status.flag_airspeed_valid;
/* check for global or local position updates, set a timeout of 2s */
if (hrt_absolute_time() - last_global_position_time < 2000000) {
@@ -1644,6 +1753,14 @@ int commander_thread_main(int argc, char *argv[])
current_status.flag_local_position_valid = false;
}
/* Check for valid airspeed/differential pressure measurements */
if (hrt_absolute_time() - last_differential_pressure_time < 2000000) {
current_status.flag_airspeed_valid = true;
} else {
current_status.flag_airspeed_valid = false;
}
/*
* Consolidate global position and local position valid flags
* for vector flight mode.
@@ -1659,7 +1776,8 @@ int commander_thread_main(int argc, char *argv[])
/* consolidate state change, flag as changed if required */
if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok ||
global_pos_valid != current_status.flag_global_position_valid ||
local_pos_valid != current_status.flag_local_position_valid) {
local_pos_valid != current_status.flag_local_position_valid ||
airspeed_valid != current_status.flag_airspeed_valid) {
state_changed = true;
}