diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-03-11 11:01:49 -0700 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-03-11 11:01:49 -0700 |
commit | 591cc0ac4d17f7ade66fb7b9b248e403a8172d56 (patch) | |
tree | 7382ef04263fb4df6c7603c4db9f6c290b24323b /apps/commander/commander.c | |
parent | 0fe5aeb02c6ab0ac9c50f54d028cd5dde908e051 (diff) | |
parent | 1d444f80a3b9b575681e41b7a3a9b26a4b3d606d (diff) | |
download | px4-firmware-591cc0ac4d17f7ade66fb7b9b248e403a8172d56.tar.gz px4-firmware-591cc0ac4d17f7ade66fb7b9b248e403a8172d56.tar.bz2 px4-firmware-591cc0ac4d17f7ade66fb7b9b248e403a8172d56.zip |
Merge remote-tracking branch 'upstream/master' into new_state_machine
Conflicts:
apps/commander/commander.c
apps/uORB/topics/vehicle_status.h
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 125 |
1 files changed, 119 insertions, 6 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a3e8fb745..aaabe7f4b 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -76,6 +76,8 @@ #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 <drivers/drv_led.h> #include <drivers/drv_hrt.h> @@ -799,6 +801,72 @@ 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"); + + 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)"); + } + + close(sub_differential_pressure); +} + void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) @@ -1018,6 +1086,28 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta handled = true; } + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting airspeed cal"); + tune_confirm(); + do_airspeed_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished airspeed cal"); + tune_confirm(); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + /* none found */ if (!handled) { //warnx("refusing unsupported calibration request\n"); @@ -1438,6 +1528,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; @@ -1491,6 +1586,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) { @@ -1691,6 +1793,8 @@ 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.condition_global_position_valid; bool local_pos_valid = current_status.condition_local_position_valid; + bool airspeed_valid = current_status.condition_airspeed_valid; + /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { @@ -1709,6 +1813,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_local_position_valid = false; } + /* Check for valid airspeed/differential pressure measurements */ + if (hrt_absolute_time() - last_differential_pressure_time < 2000000) { + current_status.condition_airspeed_valid = true; + + } else { + current_status.condition_airspeed_valid = false; + } + /* * Consolidate global position and local position valid flags * for vector flight mode. @@ -1721,12 +1833,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; // } - // /* 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) { - // state_changed = true; - // } + // XXX why is this needed? + /* consolidate state change, flag as changed if required */ + if (global_pos_valid != current_status.condition_global_position_valid || + local_pos_valid != current_status.condition_local_position_valid || + airspeed_valid != current_status.condition_airspeed_valid) { + state_changed = true; + } /* * Mark the position of the first position lock as return to launch (RTL) |