From c3c76ef3d50f71a7bef2994df462c5add11658d9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Oct 2012 09:44:59 +0100 Subject: [PATCH 1/3] Hardened the EEPROM attach routine for param storage --- apps/systemcmds/eeprom/24xxxx_mtd.c | 27 +++++++++++++++++++++++++++ apps/systemcmds/eeprom/eeprom.c | 14 ++++++++++++-- 2 files changed, 39 insertions(+), 2 deletions(-) diff --git a/apps/systemcmds/eeprom/24xxxx_mtd.c b/apps/systemcmds/eeprom/24xxxx_mtd.c index 79149caa0a..781b010651 100644 --- a/apps/systemcmds/eeprom/24xxxx_mtd.c +++ b/apps/systemcmds/eeprom/24xxxx_mtd.c @@ -502,6 +502,33 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_dev_s *dev) { priv->perf_write_errors = perf_alloc(PC_COUNT, "EEPROM write errors"); } + /* attempt to read to validate device is present */ + unsigned char buf[5]; + uint8_t addrbuf[2] = {0, 0}; + + struct i2c_msg_s msgv[2] = { + { + .addr = priv->addr, + .flags = 0, + .buffer = &addrbuf[0], + .length = sizeof(addrbuf), + }, + { + .addr = priv->addr, + .flags = I2C_M_READ, + .buffer = &buf[0], + .length = sizeof(buf), + } + }; + + perf_begin(priv->perf_reads); + int ret = I2C_TRANSFER(priv->dev, &msgv[0], 2); + perf_end(priv->perf_reads); + + if (ret < 0) { + return NULL; + } + /* Return the implementation-specific state structure as the MTD device */ fvdbg("Return %p\n", priv); diff --git a/apps/systemcmds/eeprom/eeprom.c b/apps/systemcmds/eeprom/eeprom.c index 19a14aa02b..b4257cda9b 100644 --- a/apps/systemcmds/eeprom/eeprom.c +++ b/apps/systemcmds/eeprom/eeprom.c @@ -118,9 +118,19 @@ eeprom_attach(void) if (i2c == NULL) errx(1, "failed to locate I2C bus"); - /* start the MTD driver */ - eeprom_mtd = at24c_initialize(i2c); + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + eeprom_mtd = at24c_initialize(i2c); + if (eeprom_mtd) { + /* abort on first valid result */ + if (i > 0) { + warnx("warning: EEPROM needed %d attempts to attach", i+1); + } + break; + } + } + /* if last attempt is still unsuccessful, abort */ if (eeprom_mtd == NULL) errx(1, "failed to initialize EEPROM driver"); From 574eb96a2ebafeb03d2933c68cb7f7b60269601a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 29 Oct 2012 16:41:53 +0100 Subject: [PATCH 2/3] Calibration improvement --- ROMFS/scripts/rcS | 2 ++ apps/commander/commander.c | 25 +++++++++++++++- apps/drivers/hmc5883/hmc5883.cpp | 49 +++++++++++++++++++++++++++++++- apps/sensors/sensor_params.c | 3 ++ apps/sensors/sensors.cpp | 4 +++ 5 files changed, 81 insertions(+), 2 deletions(-) diff --git a/ROMFS/scripts/rcS b/ROMFS/scripts/rcS index 409bde33b2..b5fbfe0f51 100755 --- a/ROMFS/scripts/rcS +++ b/ROMFS/scripts/rcS @@ -46,6 +46,8 @@ if [ -f /fs/microsd/etc/rc ] then echo "[init] reading /fs/microsd/etc/rc" sh /fs/microsd/etc/rc +else + echo "[init] script /fs/microsd/etc/rc not present" fi # diff --git a/apps/commander/commander.c b/apps/commander/commander.c index a7550b54b9..d93a48e313 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -360,6 +360,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) float *y = malloc(sizeof(float) * calibration_maxcount); float *z = malloc(sizeof(float) * calibration_maxcount); + tune_confirm(); + sleep(2); + tune_confirm(); + while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -504,6 +508,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + } else { mavlink_log_info(mavlink_fd, "[commander] mag calibration FAILED (NaN)"); } @@ -607,6 +618,12 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "[commander] gyro calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "[commander] gyro calibration FAILED (NaN)"); } @@ -721,6 +738,12 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) //sprintf(buf, "[commander] 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, "[commander] accel calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "[commander] accel calibration FAILED (NaN)"); } @@ -740,7 +763,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta uint8_t result = MAV_RESULT_UNSUPPORTED; /* announce command handling */ - ioctl(buzzer, TONE_SET_ALARM, 1); + tune_confirm(); /* supported command handling start */ diff --git a/apps/drivers/hmc5883/hmc5883.cpp b/apps/drivers/hmc5883/hmc5883.cpp index f9079c3ff1..6fefbfafcc 100644 --- a/apps/drivers/hmc5883/hmc5883.cpp +++ b/apps/drivers/hmc5883/hmc5883.cpp @@ -66,6 +66,8 @@ #include #include +#include + /* * HMC5883 internal constants and data structures. */ @@ -159,6 +161,10 @@ private: perf_counter_t _comms_errors; perf_counter_t _buffer_overflows; + /* status reporting */ + bool _sensor_ok; /**< sensor was found and reports ok */ + bool _calibrated; /**< the calibration is valid */ + /** * Test whether the device supported by the driver is present at a * specific address. @@ -272,6 +278,13 @@ private: */ float meas_to_float(uint8_t in[2]); + /** + * Check the current calibration and update device status + * + * @return 0 if calibration is ok, 1 else + */ + int check_calibration(); + }; /* helper macro for handling report buffer indices */ @@ -295,7 +308,9 @@ HMC5883::HMC5883(int bus) : _mag_topic(-1), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), - _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")) + _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), + _sensor_ok(false), + _calibrated(false) { // enable debug() calls _debug_enabled = true; @@ -351,6 +366,8 @@ HMC5883::init() set_range(_range_ga); ret = OK; + /* sensor is ok, but not calibrated */ + _sensor_ok = true; out: return ret; } @@ -1000,6 +1017,36 @@ out: return ret; } +int HMC5883::check_calibration() +{ + bool scale_valid, offset_valid; + + if ((-2.0f * FLT_EPSILON + 1.0f < _scale.x_scale && _scale.x_scale < 2.0f * FLT_EPSILON + 1.0f) && + (-2.0f * FLT_EPSILON + 1.0f < _scale.y_scale && _scale.y_scale < 2.0f * FLT_EPSILON + 1.0f) && + (-2.0f * FLT_EPSILON + 1.0f < _scale.z_scale && _scale.z_scale < 2.0f * FLT_EPSILON + 1.0f)) { + /* scale is different from one */ + scale_valid = true; + } else { + scale_valid = false; + } + + if ((-2.0f * FLT_EPSILON < _scale.x_offset && _scale.x_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.y_offset && _scale.y_offset < 2.0f * FLT_EPSILON) && + (-2.0f * FLT_EPSILON < _scale.z_offset && _scale.z_offset < 2.0f * FLT_EPSILON)) { + /* offset is different from zero */ + offset_valid = true; + } else { + offset_valid = false; + } + + if (_calibrated && !(offset_valid && scale_valid)) { + warnx("warning: mag %s%s", (scale_valid) ? "" : "scale invalid. ", + (offset_valid) ? "" : "offset invalid."); + _calibrated = false; + // XXX Notify system via uORB + } +} + int HMC5883::set_excitement(unsigned enable) { int ret; diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 5e8c5746c5..892ec975a4 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -130,6 +130,9 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_AUX1, 6); +PARAM_DEFINE_INT32(RC_MAP_AUX2, 7); +PARAM_DEFINE_INT32(RC_MAP_AUX3, 8); PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.4f); PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.4f); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 54d2f6a0b5..9522bd31a2 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -1030,6 +1030,10 @@ Sensors::task_main() manual_control.pitch = 0.0f; manual_control.yaw = 0.0f; manual_control.throttle = 0.0f; + manual_control.aux1_cam_pan_flaps = 0.0f; + manual_control.aux2_cam_tilt = 0.0f; + manual_control.aux3_cam_zoom = 0.0f; + manual_control.aux4_cam_roll = 0.0f; _manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control); } From 18831db444df3af80a5b362189b53dc42382d2d6 Mon Sep 17 00:00:00 2001 From: Doug Weibel Date: Tue, 30 Oct 2012 11:01:56 -0600 Subject: [PATCH 3/3] Work in process - beginning of navigation/position control implementation. Compiles, but has not been tested. --- .../fixedwing_pos_control.h | 73 +++++++++++++++ .../fixedwing_pos_control_main.c | 93 +++++++++++++++++-- 2 files changed, 156 insertions(+), 10 deletions(-) create mode 100644 apps/fixedwing_pos_control/fixedwing_pos_control.h diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control.h b/apps/fixedwing_pos_control/fixedwing_pos_control.h new file mode 100644 index 0000000000..f631c90a1b --- /dev/null +++ b/apps/fixedwing_pos_control/fixedwing_pos_control.h @@ -0,0 +1,73 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Doug Weibel + * @author Lorenz Meier + @author Thomas Gubler + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file fixedwing_pos_control.h + * Position control for fixed wing airframes. + */ + +#ifndef FIXEDWING_POS_CONTROL_H_ +#define FIXEDWING_POS_CONTROL_H_ + +#include +#include +#include +#include +#include + +#endif + + +struct planned_path_segments_s { + bool segment_type; + double start_lat; // Start of line or center of arc + double start_lon; + double end_lat; + double end_lon; + float radius; // Radius of arc + float arc_start_bearing; // Bearing from center to start of arc + float arc_sweep; // Angle (radians) swept out by arc around center. + // Positive for clockwise, negative for counter-clockwise +}; + + +float _wrap180(float bearing); +float _wrap360(float bearing); +float _wrapPI(float bearing); +float _wrap2PI(float bearing); + +/* FIXEDWING_CONTROL_H_ */ + diff --git a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c index eb4945464f..9eb34ae449 100644 --- a/apps/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/apps/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -67,6 +67,7 @@ * */ PARAM_DEFINE_FLOAT(FW_HEADING_P, 0.1f); +PARAM_DEFINE_FLOAT(FW_XTRACK_P, 0.01745f); // Radians per meter off track PARAM_DEFINE_FLOAT(FW_ALT_P, 0.1f); PARAM_DEFINE_FLOAT(FW_ROLL_LIM, 0.7f); // Roll angle limit in radians PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians @@ -74,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_PITCH_LIM, 0.35f); // Pitch angle limit in radians struct fw_pos_control_params { float heading_p; + float xtrack_p; float altitude_p; float roll_lim; float pitch_lim; @@ -81,6 +83,7 @@ struct fw_pos_control_params { struct fw_pos_control_param_handles { param_t heading_p; + param_t xtrack_p; param_t altitude_p; param_t roll_lim; param_t pitch_lim; @@ -121,6 +124,7 @@ static int parameters_init(struct fw_pos_control_param_handles *h) { /* PID parameters */ h->heading_p = param_find("FW_HEADING_P"); + h->xtrack_p = param_find("FW_XTRACK_P"); h->altitude_p = param_find("FW_ALT_P"); h->roll_lim = param_find("FW_ROLL_LIM"); h->pitch_lim = param_find("FW_PITCH_LIM"); @@ -132,6 +136,7 @@ static int parameters_init(struct fw_pos_control_param_handles *h) static int parameters_update(const struct fw_pos_control_param_handles *h, struct fw_pos_control_params *p) { param_get(h->heading_p, &(p->heading_p)); + param_get(h->xtrack_p, &(p->xtrack_p)); param_get(h->altitude_p, &(p->altitude_p)); param_get(h->roll_lim, &(p->roll_lim)); param_get(h->pitch_lim, &(p->pitch_lim)); @@ -158,11 +163,15 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); + struct vehicle_global_position_s start_pos; // Temporary variable, replace with + memset(&start_pos, 0, sizeof(start_pos)); // previous waypoint when available struct vehicle_global_position_setpoint_s global_setpoint; memset(&global_setpoint, 0, sizeof(global_setpoint)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - + crosstrack_error_s xtrack_err; + //memset(&xtrack_err, 0, sizeof(xtrack_err)); + /* output structs */ struct vehicle_attitude_setpoint_s attitude_setpoint; memset(&attitude_setpoint, 0, sizeof(attitude_setpoint)); @@ -181,6 +190,8 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* Setup of loop */ struct pollfd fds = { .fd = att_sub, .events = POLLIN }; bool global_sp_updated_set_once = false; + bool start_point_set = false; // This is a temporary flag till the + // previous waypoint is available for computations while(!thread_should_exit) { @@ -221,7 +232,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) orb_check(global_setpoint_sub, &global_sp_updated); if(global_sp_updated) global_sp_updated_set_once = true; - + if(global_sp_updated_set_once && !start_point_set) { + start_pos = global_pos; + start_point_set = true; + } /* Load local copies */ orb_copy(ORB_ID(vehicle_attitude), att_sub, &att); @@ -239,17 +253,18 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* calculate bearing error */ float target_bearing = get_bearing_to_next_waypoint(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); + + /* calculate crosstrack error */ + // Only the case of a straight line track following handled so far + xtrack_err = get_distance_to_line(global_pos.lat / (double)1e7d, global_pos.lon / (double)1e7d, + start_pos.lat / (double)1e7d, start_pos.lon / (double)1e7d, + global_setpoint.lat / (double)1e7d, global_setpoint.lon / (double)1e7d); /* shift error to prevent wrapping issues */ float bearing_error = target_bearing - att.yaw; - - if (bearing_error < M_PI_F) { - bearing_error += 2.0f * M_PI_F; - } - - if (bearing_error > M_PI_F) { - bearing_error -= 2.0f * M_PI_F; - } + if(!(xtrack_err.error || xtrack_err.past_end)) + bearing_error -= p.xtrack_p * xtrack_err.distance; + bearing_error = _wrapPI(bearing_error); /* calculate roll setpoint, do this artificially around zero */ attitude_setpoint.roll_tait_bryan = pid_calculate(&heading_controller, bearing_error, 0.0f, 0.0f, 0.0f); @@ -344,4 +359,62 @@ int fixedwing_pos_control_main(int argc, char *argv[]) } +float _wrapPI(float bearing) +{ + + while (bearing > M_PI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing <= -M_PI_F) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +float _wrap2PI(float bearing) +{ + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + + return bearing; +} + +float _wrap180(float bearing) +{ + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + +float _wrap360(float bearing) +{ + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + + return bearing; +} + + +