From 2876bc72f979b4596fbe97cfae71c0d04d4753b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 28 May 2013 17:46:24 +0200 Subject: Slightly reworked IO internal failsafe, added command to activate it (px4io failsafe), does not parse commandline arguments yet --- src/drivers/px4io/px4io.cpp | 52 +++++++++++++++++++++++++++++++++++++++------ 1 file changed, 45 insertions(+), 7 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 8b2fae12b..f25d471aa 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -106,7 +106,7 @@ public: * @param rate The rate in Hz actuator outpus are sent to IO. * Min 10 Hz, max 400 Hz */ - int set_update_rate(int rate); + int set_update_rate(int rate); /** * Set the battery current scaling and bias @@ -114,7 +114,15 @@ public: * @param amp_per_volt * @param amp_bias */ - void set_battery_current_scaling(float amp_per_volt, float amp_bias); + void set_battery_current_scaling(float amp_per_volt, float amp_bias); + + /** + * Push failsafe values to IO. + * + * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full) + * @param len Number of channels, could up to 8 + */ + int set_failsafe_values(const uint16_t *vals, unsigned len); /** * Print the current status of IO @@ -326,11 +334,11 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0), - _primary_pwm_device(false) + _battery_last_timestamp(0) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -689,6 +697,21 @@ PX4IO::io_set_control_state() return io_reg_set(PX4IO_PAGE_CONTROLS, 0, regs, _max_controls); } +int +PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + unsigned max = (len < _max_actuators) ? len : _max_actuators; + + /* set failsafe values */ + for (unsigned i = 0; i < max; i++) + regs[i] = FLOAT_TO_REG(vals[i]); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max); +} + int PX4IO::io_set_arming_state() { @@ -1250,7 +1273,7 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), @@ -1262,7 +1285,8 @@ PX4IO::print_status() ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), ((flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ? " MIXER_OK" : " MIXER_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC) ? " ARM_SYNC" : " ARM_NO_SYNC"), - ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL")); + ((flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ? " INIT_OK" : " INIT_FAIL"), + ((flags & PX4IO_P_STATUS_FLAGS_FAILSAFE) ? " FAILSAFE" : "")); uint16_t alarms = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_ALARMS); printf("alarms 0x%04x%s%s%s%s%s%s%s\n", alarms, @@ -1718,6 +1742,20 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "failsafe")) { + + /* XXX parse arguments here */ + + if (g_dev != nullptr) { + /* XXX testing values */ + uint16_t failsafe[4] = {1500, 1500, 1200, 1200}; + g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -1845,5 +1883,5 @@ px4io_main(int argc, char *argv[]) monitor(); out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); } -- cgit v1.2.3 From f6570172da02bba79b5e050c6e02b86dc96551c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 May 2013 17:07:26 +0200 Subject: Set default failsafe value to 0 of mixer --- src/drivers/px4io/px4io.cpp | 29 +++++++++++++++++++-------- src/modules/px4iofirmware/mixer.cpp | 37 +++++++++++++++++++++++++++++++++++ src/modules/px4iofirmware/registers.c | 5 ++++- 3 files changed, 62 insertions(+), 9 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f25d471aa..873d7eb82 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -704,12 +704,8 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) unsigned max = (len < _max_actuators) ? len : _max_actuators; - /* set failsafe values */ - for (unsigned i = 0; i < max; i++) - regs[i] = FLOAT_TO_REG(vals[i]); - /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, regs, max); + return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max); } int @@ -1744,11 +1740,28 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "failsafe")) { - /* XXX parse arguments here */ + if (argc < 3) { + errx(1, "failsafe command needs at least one channel value (ppm)"); + } if (g_dev != nullptr) { - /* XXX testing values */ - uint16_t failsafe[4] = {1500, 1500, 1200, 1200}; + + /* set values for first 8 channels, fill unassigned channels with 1500. */ + uint16_t failsafe[8]; + + for (int i = 0; i < sizeof(failsafe) / sizeof(failsafe[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + failsafe[i] = atoi(argv[i+2]); + if (failsafe[i] < 800 || failsafe[i] > 2200) { + errx(1, "value out of range of 800 < value < 2200. Aborting."); + } + } else { + failsafe[i] = 1500; + } + } + g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); } else { errx(1, "not loaded"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 448d2355f..0b8ed6dc5 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -85,6 +85,9 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +static void mixer_set_failsafe(); + void mixer_tick(void) { @@ -243,6 +246,7 @@ mixer_callback(uintptr_t handle, case MIX_FAILSAFE: case MIX_NONE: + control = 0.0f; return -1; } @@ -320,8 +324,41 @@ mixer_handle_text(const void *buffer, size_t length) memcpy(&mixer_text[0], &mixer_text[mixer_text_length - resid], resid); mixer_text_length = resid; + + /* update failsafe values */ + mixer_set_failsafe(); } break; } } + +static void +mixer_set_failsafe() +{ + /* + * Check if a custom failsafe value has been written, + * else use the opportunity to set decent defaults. + */ + if (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + return; + + float outputs[IO_SERVO_COUNT]; + unsigned mixed; + + /* mix */ + mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + + /* scale to PWM and update the servo outputs as required */ + for (unsigned i = 0; i < mixed; i++) { + + /* scale to servo output */ + r_page_servo_failsafe[i] = (outputs[i] * 600.0f) + 1500; + + } + + /* disable the rest of the outputs */ + for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) + r_page_servo_failsafe[i] = 0; + +} diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 3abf56a18..49a723352 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -231,11 +231,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num case PX4IO_PAGE_FAILSAFE_PWM: /* copy channel data */ - while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) { + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { /* XXX range-check value? */ r_page_servo_failsafe[offset] = *values; + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM; + offset++; num_values--; values++; -- cgit v1.2.3 From 5f2571dd01c90ba1209d263c52d818225644ce07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 29 May 2013 18:29:41 +0200 Subject: Set unknown channels to zero, since centering them is a slightly dangerous guess --- src/drivers/px4io/px4io.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 873d7eb82..0a31831f0 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1758,7 +1758,8 @@ px4io_main(int argc, char *argv[]) errx(1, "value out of range of 800 < value < 2200. Aborting."); } } else { - failsafe[i] = 1500; + /* a zero value will result in stopping to output any pulse */ + failsafe[i] = 0; } } -- cgit v1.2.3 From 1addb9f6c56f68f600acd4bdd609ef14697bda44 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 20:42:43 +0400 Subject: Fixed bug in UBX::configure_message_rate() --- src/drivers/gps/ubx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/drivers') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b3093b0f6..f2e7ca67d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) msg.msg_class = msg_class; msg.msg_id = msg_id; msg.rate = rate; - send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void -- cgit v1.2.3 From f435025d26f49d1d9069282aa72c7e1cb9201773 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 00:10:58 +0200 Subject: Completed main implementation and debugging --- src/drivers/hott_telemetry/hott_telemetry_main.c | 3 + src/drivers/hott_telemetry/messages.c | 194 +++++++++++++---------- src/drivers/hott_telemetry/messages.h | 1 + src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 4 +- src/modules/systemlib/geo/geo.c | 8 +- src/modules/systemlib/geo/geo.h | 16 ++ 6 files changed, 133 insertions(+), 93 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 827cb862a..4318244f8 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -138,6 +138,7 @@ recv_req_id(int uart, uint8_t *id) /* if we have a binary mode request */ if (mode != BINARY_MODE_REQUEST_ID) { + warnx("Non binary request ID detected: %d", mode); return ERROR; } @@ -232,6 +233,8 @@ hott_telemetry_thread_main(int argc, char *argv[]) } send_data(uart, buffer, size); + } else { + warnx("NOK"); } } diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index feec8998c..1d746506e 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -40,8 +40,9 @@ #include "messages.h" #include +#include #include -#include +#include #include #include #include @@ -51,11 +52,17 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 +#define ALT_OFFSET 500.0f + static int battery_sub = -1; static int gps_sub = -1; static int home_sub = -1; static int sensor_sub = -1; +static bool home_position_set = false; +static double home_lat = 0.0d; +static double home_lon = 0.0d; + void messages_init(void) { @@ -69,15 +76,18 @@ void build_eam_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ - struct sensor_combined_s raw = { 0 }; + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); /* get a local copy of the battery data */ - struct battery_status_s battery = { 0 }; + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); orb_copy(ORB_ID(battery_status), battery_sub, &battery); - struct eam_module_msg msg = { 0 }; + struct eam_module_msg msg; *size = sizeof(msg); + memset(&msg, 0, *size); msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; @@ -92,12 +102,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - // TODO: flight time - // TODO: climb rate - - msg.stop = STOP_BYTE; - memcpy(buffer, &msg, *size); } @@ -105,102 +110,113 @@ void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ - struct sensor_combined_s raw = { 0 }; + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); /* get a local copy of the battery data */ - struct vehicle_gps_position_s gps = { 0 }; + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); struct gps_module_msg msg = { 0 }; *size = sizeof(msg); + memset(&msg, 0, *size); msg.start = START_BYTE; msg.sensor_id = GPS_SENSOR_ID; msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - /* Current flight direction */ - msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); - - /* GPS speed */ - uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); - msg.gps_speed_L = (uint8_t)speed & 0xff; - msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; - - /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat)) * 1e-7d; - - msg.latitude_ns = 0; - if (lat < 0) { - msg.latitude_ns = 1; - lat = -lat; - } - - int deg; - int min; - int sec; - convert_to_degrees_minutes_seconds(lat, °, &min, &sec); - - uint16_t lat_min = (uint16_t)(deg * 100 + min); - msg.latitude_min_L = (uint8_t)lat_min & 0xff; - msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff; - uint16_t lat_sec = (uint16_t)(sec); - msg.latitude_sec_L = (uint8_t)lat_sec & 0xff; - msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; - - - /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon)) * 1e-7d; - - msg.longitude_ew = 0; - if (lon < 0) { - msg.longitude_ew = 1; - lon = -lon; - } - - convert_to_degrees_minutes_seconds(lon, °, &min, &sec); - - uint16_t lon_min = (uint16_t)(deg * 100 + min); - msg.longitude_min_L = (uint8_t)lon_min & 0xff; - msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; - uint16_t lon_sec = (uint16_t)(sec); - msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; - msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; - - /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt * 1e-3 + 500.0f); - msg.altitude_L = (uint8_t)alt & 0xff; - msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - - /* Distance from home */ - bool updated; - orb_check(home_sub, &updated); - if (updated) { - /* get a local copy of the home position data */ - struct home_position_s home = { 0 }; - orb_copy(ORB_ID(home_position), home_sub, &home); - - uint16_t dist = (uint16_t)get_distance_to_next_waypoint( - (double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon); - warnx("dist %d home.lat %3.6f home.lon %3.6f lat %3.6f lon %3.6f ", - dist, (double)home.lat*1e-7d, (double)home.lon*1e-7d, lat, lon); - msg.distance_L = (uint8_t)dist & 0xff; - msg.distance_H = (uint8_t)(dist >> 8) & 0xff; - - /* Direction back to home */ - uint16_t bearing = (uint16_t)get_bearing_to_next_waypoint( - (double)home.lat*1e-7, (double)home.lon*1e-7, lat, lon) * M_RAD_TO_DEG_F; - msg.home_direction = (uint8_t)bearing >> 1; - } - msg.gps_num_sat = gps.satellites_visible; /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); msg.gps_fix = (uint8_t)(gps.fix_type + 48); - msg.stop = STOP_BYTE; + /* No point collecting more data if we don't have a 3D fix yet */ + if (gps.fix_type > 2) { + /* Current flight direction */ + msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); + + /* GPS speed */ + uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); + msg.gps_speed_L = (uint8_t)speed & 0xff; + msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; + + /* Get latitude in degrees, minutes and seconds */ + double lat = ((double)(gps.lat))*1e-7d; + + /* Prepend N or S specifier */ + msg.latitude_ns = 0; + if (lat < 0) { + msg.latitude_ns = 1; + lat = abs(lat); + } + + int deg; + int min; + int sec; + convert_to_degrees_minutes_seconds(lat, °, &min, &sec); + + uint16_t lat_min = (uint16_t)(deg * 100 + min); + msg.latitude_min_L = (uint8_t)lat_min & 0xff; + msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff; + uint16_t lat_sec = (uint16_t)(sec); + msg.latitude_sec_L = (uint8_t)lat_sec & 0xff; + msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; + + /* Get longitude in degrees, minutes and seconds */ + double lon = ((double)(gps.lon))*1e-7d; + + /* Prepend E or W specifier */ + msg.longitude_ew = 0; + if (lon < 0) { + msg.longitude_ew = 1; + lon = abs(lon); + } + + convert_to_degrees_minutes_seconds(lon, °, &min, &sec); + + uint16_t lon_min = (uint16_t)(deg * 100 + min); + msg.longitude_min_L = (uint8_t)lon_min & 0xff; + msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; + uint16_t lon_sec = (uint16_t)(sec); + msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; + msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; + + /* Altitude */ + uint16_t alt = (uint16_t)(gps.alt*1e-3 + ALT_OFFSET ); + msg.altitude_L = (uint8_t)alt & 0xff; + msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + + /* Get any (and probably only ever one) home_sub postion report */ + bool updated; + orb_check(home_sub, &updated); + if (updated) { + /* get a local copy of the home position data */ + struct home_position_s home; + memset(&home, 0, sizeof(home)); + orb_copy(ORB_ID(home_position), home_sub, &home); + + home_lat = ((double)(home.lat))*1e-7d; + home_lon = ((double)(home.lon))*1e-7d; + home_position_set = true; + } + + /* Distance from home */ + if (home_position_set) { + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + + msg.distance_L = (uint8_t)dist & 0xff; + msg.distance_H = (uint8_t)(dist >> 8) & 0xff; + + /* Direction back to home */ + uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); + msg.home_direction = (uint8_t)bearing >> 1; + } + } + msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); } @@ -210,6 +226,8 @@ convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec) *deg = (int)val; double delta = val - *deg; - *min = (int)(delta * 60.0); - *sec = (int)(delta * 3600.0); + const double min_d = delta * 60.0d; + *min = (int)min_d; + delta = min_d - *min; + *sec = (int)(delta * 10000.0d); } diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h index 5f7dd5c14..e6d5cc723 100644 --- a/src/drivers/hott_telemetry/messages.h +++ b/src/drivers/hott_telemetry/messages.h @@ -189,6 +189,7 @@ struct gps_module_msg { void messages_init(void); void build_eam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); +float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 4ef150da1..c3836bdfa 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -683,7 +683,8 @@ int KalmanNav::correctPos() // fault detetcion float beta = y.dot(S.inverse() * y); - if (beta > _faultPos.get()) { + static int counter = 0; + if (beta > _faultPos.get() && (counter % 10 == 0)) { printf("fault in gps: beta = %8.4f\n", (double)beta); printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(y(0) / sqrtf(RPos(0, 0))), @@ -693,6 +694,7 @@ int KalmanNav::correctPos() double(y(4) / sqrtf(RPos(4, 4))), double(y(5) / sqrtf(RPos(5, 5)))); } + counter++; return ret_ok; } diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c index 6746e8ff3..3ac4bd168 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -46,7 +46,7 @@ #include #include -#include +//#include #include #include #include @@ -185,12 +185,12 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); const double radius_earth = 6371000.0d; - - return radius_earth * c; + printf("DIST: %.4f\n", radius_earth * c); + return radius_earth * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h index 0c0b5c533..84097b49f 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/modules/systemlib/geo/geo.h @@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y) */ __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); -- cgit v1.2.3 From 30d17cf0ba9da7587a2087674c37cda5399ab851 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 00:18:23 +0200 Subject: Fix whitespace --- src/drivers/hott_telemetry/messages.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index 1d746506e..e10a22dc4 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -193,7 +193,7 @@ build_gps_response(uint8_t *buffer, size_t *size) bool updated; orb_check(home_sub, &updated); if (updated) { - /* get a local copy of the home position data */ + /* get a local copy of the home position data */ struct home_position_s home; memset(&home, 0, sizeof(home)); orb_copy(ORB_ID(home_position), home_sub, &home); @@ -205,7 +205,7 @@ build_gps_response(uint8_t *buffer, size_t *size) /* Distance from home */ if (home_position_set) { - uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); msg.distance_L = (uint8_t)dist & 0xff; msg.distance_H = (uint8_t)(dist >> 8) & 0xff; @@ -213,7 +213,7 @@ build_gps_response(uint8_t *buffer, size_t *size) /* Direction back to home */ uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); msg.home_direction = (uint8_t)bearing >> 1; - } + } } msg.stop = STOP_BYTE; @@ -223,11 +223,11 @@ build_gps_response(uint8_t *buffer, size_t *size) void convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec) { - *deg = (int)val; + *deg = (int)val; - double delta = val - *deg; - const double min_d = delta * 60.0d; - *min = (int)min_d; - delta = min_d - *min; - *sec = (int)(delta * 10000.0d); + double delta = val - *deg; + const double min_d = delta * 60.0d; + *min = (int)min_d; + delta = min_d - *min; + *sec = (int)(delta * 10000.0d); } -- cgit v1.2.3 From 9374e4b1f221d571d56686d7d92ecb6cabcca8b6 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 00:52:48 +0200 Subject: Formatting and comments --- src/drivers/hott_telemetry/messages.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index e10a22dc4..369070f8c 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -52,8 +52,6 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -#define ALT_OFFSET 500.0f - static int battery_sub = -1; static int gps_sub = -1; static int home_sub = -1; @@ -146,7 +144,7 @@ build_gps_response(uint8_t *buffer, size_t *size) /* Get latitude in degrees, minutes and seconds */ double lat = ((double)(gps.lat))*1e-7d; - /* Prepend N or S specifier */ + /* Set the N or S specifier */ msg.latitude_ns = 0; if (lat < 0) { msg.latitude_ns = 1; @@ -168,7 +166,7 @@ build_gps_response(uint8_t *buffer, size_t *size) /* Get longitude in degrees, minutes and seconds */ double lon = ((double)(gps.lon))*1e-7d; - /* Prepend E or W specifier */ + /* Set the E or W specifier */ msg.longitude_ew = 0; if (lon < 0) { msg.longitude_ew = 1; @@ -185,7 +183,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3 + ALT_OFFSET ); + uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; -- cgit v1.2.3 From 45fe45fefa7cff5c9799037ee024671b4493ab85 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 4 Jun 2013 13:32:57 +0200 Subject: Better error handling for too large arguments --- src/drivers/px4io/px4io.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0a31831f0..0934e614b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -702,10 +702,12 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { uint16_t regs[_max_actuators]; - unsigned max = (len < _max_actuators) ? len : _max_actuators; + if (len > _max_actuators) + /* fail with error */ + return E2BIG; /* copy values to registers in IO */ - return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, max); + return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); } int @@ -1763,7 +1765,10 @@ px4io_main(int argc, char *argv[]) } } - g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + int ret = g_dev->set_failsafe_values(failsafe, sizeof(failsafe) / sizeof(failsafe[0])); + + if (ret != OK) + errx(ret, "failed setting failsafe values"); } else { errx(1, "not loaded"); } -- cgit v1.2.3 From 032f7d0b0e0bfb0cb5b77ebb8553cf3dc7ddda9b Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 23:24:30 +0200 Subject: Fix syncing issue with receiver on startup. --- src/drivers/hott_telemetry/hott_telemetry_main.c | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) (limited to 'src/drivers') diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 4318244f8..1d2bdd92e 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -133,15 +133,14 @@ recv_req_id(int uart, uint8_t *id) if (poll(fds, 1, timeout_ms) > 0) { /* Get the mode: binary or text */ read(uart, &mode, sizeof(mode)); - /* Read the device ID being polled */ - read(uart, id, sizeof(*id)); /* if we have a binary mode request */ if (mode != BINARY_MODE_REQUEST_ID) { - warnx("Non binary request ID detected: %d", mode); return ERROR; } + /* Read the device ID being polled */ + read(uart, id, sizeof(*id)); } else { warnx("UART timeout on TX/RX port"); return ERROR; @@ -216,9 +215,15 @@ hott_telemetry_thread_main(int argc, char *argv[]) uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; + bool connected = true; while (!thread_should_exit) { if (recv_req_id(uart, &id) == OK) { + if (!connected) { + connected = true; + warnx("OK"); + } + switch (id) { case EAM_SENSOR_ID: build_eam_response(buffer, &size); @@ -234,7 +239,8 @@ hott_telemetry_thread_main(int argc, char *argv[]) send_data(uart, buffer, size); } else { - warnx("NOK"); + connected = false; + warnx("syncing"); } } -- cgit v1.2.3