diff options
-rw-r--r-- | ROMFS/scripts/rc.standalone | 2 | ||||
-rw-r--r-- | apps/drivers/ms5611/ms5611.cpp | 3 | ||||
-rw-r--r-- | apps/fixedwing_control/Makefile | 2 | ||||
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.c | 203 | ||||
-rw-r--r-- | apps/fixedwing_control/fixedwing_control.h | 66 | ||||
-rw-r--r-- | apps/fixedwing_control/pid.c | 14 | ||||
-rw-r--r-- | apps/mavlink/mavlink.c | 25 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/Makefile | 5 | ||||
-rw-r--r-- | apps/px4/attitude_estimator_bm/attitude_estimator_bm.c | 39 | ||||
-rw-r--r-- | apps/px4/px4io/driver/px4io.cpp | 5 | ||||
-rw-r--r-- | apps/px4io/px4io.c | 24 | ||||
-rw-r--r-- | apps/sensors/sensors.c | 46 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/include/nsh_romfsimg.h | 138 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_hmc5833l.c | 2 | ||||
-rw-r--r-- | nuttx/configs/px4fmu/src/drv_ms5611.c | 11 |
15 files changed, 279 insertions, 306 deletions
diff --git a/ROMFS/scripts/rc.standalone b/ROMFS/scripts/rc.standalone index 72153ef18..7dfd98a16 100644 --- a/ROMFS/scripts/rc.standalone +++ b/ROMFS/scripts/rc.standalone @@ -8,7 +8,7 @@ echo "[init] doing standalone PX4FMU startup..." # # Start the ORB # -#uorb start +uorb start # # Start the sensors. diff --git a/apps/drivers/ms5611/ms5611.cpp b/apps/drivers/ms5611/ms5611.cpp index d3c75f755..1f75ceb4b 100644 --- a/apps/drivers/ms5611/ms5611.cpp +++ b/apps/drivers/ms5611/ms5611.cpp @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Driver for the MS5611 barometric pressure sensor connected via I2C + * @file ms5611.cpp + * Driver for the MS5611 barometric pressure sensor connected via I2C. */ #include <nuttx/config.h> diff --git a/apps/fixedwing_control/Makefile b/apps/fixedwing_control/Makefile index 985708ae5..02d4463dd 100644 --- a/apps/fixedwing_control/Makefile +++ b/apps/fixedwing_control/Makefile @@ -37,7 +37,7 @@ APPNAME = fixedwing_control PRIORITY = SCHED_PRIORITY_MAX - 1 -STACKSIZE = 12288 +STACKSIZE = 4096 CSRCS = fixedwing_control.c diff --git a/apps/fixedwing_control/fixedwing_control.c b/apps/fixedwing_control/fixedwing_control.c index cc834e957..9faa257ca 100644 --- a/apps/fixedwing_control/fixedwing_control.c +++ b/apps/fixedwing_control/fixedwing_control.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: @author Ivan Ovinnikov <oivan@ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -53,7 +53,17 @@ #include <drivers/drv_pwm_output.h> #include <nuttx/spi.h> #include "../mix_and_link/mix_and_link.h" -#include "fixedwing_control.h" +#include <uORB/uORB.h> +#include <uORB/topics/rc_channels.h> +#include <uORB/topics/vehicle_global_position.h> +#include <uORB/topics/vehicle_global_position_setpoint.h> +#include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> +#include <uORB/topics/fixedwing_control.h> + +#ifndef F_M_PI +#define F_M_PI ((float)M_PI) +#endif __EXPORT int fixedwing_control_main(int argc, char *argv[]); @@ -61,7 +71,6 @@ __EXPORT int fixedwing_control_main(int argc, char *argv[]); #define PID_SCALER 0.1f #define PID_DERIVMODE_CALC 0 #define HIL_MODE 32 -#define RAD2DEG ((1.0/180.0)*M_PI) #define AUTO -1000 #define MANUAL 3000 #define SERVO_MIN 1000 @@ -75,7 +84,6 @@ pthread_t servo_thread; * Servo channels function enumerator used for * the servo writing part */ - enum SERVO_CHANNELS_FUNCTION { AIL_1 = 0, @@ -127,9 +135,9 @@ typedef struct { /* Next waypoint*/ - float wp_x; - float wp_y; - float wp_z; + double wp_x; + double wp_y; + double wp_z; /* Setpoints */ @@ -189,14 +197,6 @@ static float calc_throttle_setpoint(void); static float calc_wp_distance(void); static void set_plane_mode(void); -/* - * The control, navigation and servo loop threads - */ - -static void *control_loop(void *arg); -static void *nav_loop(void *arg); -static void *servo_loop(void *arg); - /**************************************************************************** * Public Data ****************************************************************************/ @@ -210,8 +210,9 @@ float scaler = 1; //M_PI; ****************************************************************************/ /** - * - * Calculates the PID control output given an error. Incorporates PD scaling and low-pass filter for the derivative component. + * Calculates the PID control output given an error. + * + * Incorporates PD scaling and low-pass filter for the derivative component. * * @param error the input error * @param error_deriv the derivative of the input error @@ -241,7 +242,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float output += error * Kp; - if ((fabs(Kd) > 0) && (dt > 0)) { + if ((fabsf(Kd) > 0) && (dt > 0)) { if (PID_DERIVMODE_CALC) { derivative = (error - lerror) / delta_time; @@ -250,7 +251,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float * discrete low pass filter, cuts out the * high frequency noise that can drive the controller crazy */ - float RC = 1 / (2 * M_PI * fCut); + float RC = 1.0 / (2.0f * M_PI * fCut); derivative = lderiv + (delta_time / (RC + delta_time)) * (derivative - lderiv); @@ -272,7 +273,7 @@ static float pid(float error, float error_deriv, uint16_t dt, float scale, float output *= scale; /* Compute integral component if time has elapsed */ - if ((fabs(Ki) > 0) && (dt > 0)) { + if ((fabsf(Ki) > 0) && (dt > 0)) { integrator += (error * Ki) * scaler * delta_time; if (integrator < -imax) { @@ -331,8 +332,8 @@ static void get_parameters() static void calc_body_angular_rates(float roll, float pitch, float yaw, float rollspeed, float pitchspeed, float yawspeed) { plane_data.p = rollspeed - sinf(pitch) * yawspeed; - plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cos(pitch) * yawspeed; - plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cos(pitch) * yawspeed; + plane_data.q = cosf(roll) * pitchspeed + sinf(roll) * cosf(pitch) * yawspeed; + plane_data.r = -sinf(roll) * pitchspeed + cosf(roll) * cosf(pitch) * yawspeed; } /** @@ -390,13 +391,13 @@ static void calc_rotation_matrix(float roll, float pitch, float yaw, float x, fl * @return bearing Bearing error * */ - static float calc_bearing() { - float bearing = 90 + atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)) * RAD2DEG; + float bearing = F_M_PI/2.0f + (float)atan2(-(plane_data.wp_y - plane_data.lat), (plane_data.wp_x - plane_data.lon)); - if (bearing < 0) - bearing += 360; + if (bearing < 0.0f) { + bearing += 2*F_M_PI; + } return bearing; } @@ -430,7 +431,6 @@ static float calc_roll_ail() * * @return Pitch elevators control output (-1,1) */ - static float calc_pitch_elev() { float ret = pid((plane_data.pitch_setpoint - plane_data.pitch) / scaler, plane_data.pitchspeed, PID_DT, PID_SCALER, @@ -453,10 +453,9 @@ static float calc_pitch_elev() * @return Yaw rudder control output (-1,1) * */ - static float calc_yaw_rudder(float hdg) { - float ret = pid((plane_data.yaw / RAD2DEG - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER, + float ret = pid((plane_data.yaw - abs(hdg)) / scaler, plane_data.yawspeed, PID_DT, PID_SCALER, plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); if (ret < -1) @@ -481,11 +480,11 @@ static float calc_throttle() float ret = pid(plane_data.throttle_setpoint - calc_gnd_speed(), 0, PID_DT, PID_SCALER, plane_data.Kp_pos, plane_data.Ki_pos, plane_data.Kd_pos, plane_data.intmax_pos); - if (ret < 0.2) - return 0.2; + if (ret < 0.2f) + return 0.2f; - if (ret > 1) - return 1; + if (ret > 1.0f) + return 1.0f; return ret; } @@ -542,11 +541,11 @@ static float calc_roll_setpoint() } else { setpoint = calc_bearing() - plane_data.yaw; - if (setpoint < -35) - return -35; + if (setpoint < (-35.0f/180.0f)*F_M_PI) + return (-35.0f/180.0f)*F_M_PI; - if (setpoint > 35) - return 35; + if (setpoint > (35/180.0f)*F_M_PI) + return (-35.0f/180.0f)*F_M_PI; } return setpoint; @@ -564,19 +563,19 @@ static float calc_roll_setpoint() static float calc_pitch_setpoint() { - float setpoint = 0; + float setpoint = 0.0f; if (plane_data.mode == TAKEOFF) { - setpoint = 35; + setpoint = ((35/180.0f)*F_M_PI); } else { - setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()) * RAD2DEG; + setpoint = atanf((plane_data.wp_z - plane_data.alt) / calc_wp_distance()); - if (setpoint < -35) - return -35; + if (setpoint < (-35.0f/180.0f)*F_M_PI) + return (-35.0f/180.0f)*F_M_PI; - if (setpoint > 35) - return 35; + if (setpoint > (35/180.0f)*F_M_PI) + return (-35.0f/180.0f)*F_M_PI; } return setpoint; @@ -597,7 +596,7 @@ static float calc_throttle_setpoint() // if TAKEOFF full throttle if (plane_data.mode == TAKEOFF) { - setpoint = 60; + setpoint = 0.6f; } // if CRUISE - parameter value @@ -607,7 +606,7 @@ static float calc_throttle_setpoint() // if LAND no throttle if (plane_data.mode == LAND) { - setpoint = 0; + setpoint = 0.0f; } return setpoint; @@ -623,7 +622,7 @@ static float calc_throttle_setpoint() static void set_plane_mode() { - if (plane_data.alt < 10) { + if (plane_data.alt < 10.0f) { plane_data.mode = TAKEOFF; } else { @@ -647,27 +646,31 @@ static void set_plane_mode() int fixedwing_control_main(int argc, char *argv[]) { - /* print text */ - printf("Fixedwing control started\n"); - usleep(100000); - /* default values for arguments */ char *fixedwing_uart_name = "/dev/ttyS1"; - char *commandline_usage = "\tusage: fixedwing_control -d fixedwing-devicename\n"; + char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n"; /* read arguments */ - int i; + bool verbose = false; - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { - if (argc > i + 1) { - fixedwing_uart_name = argv[i + 1]; + for (int i = 1; i < argc; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { + if (argc > i + 1) { + fixedwing_uart_name = argv[i + 1]; - } else { - printf(commandline_usage); - return 0; + } else { + printf(commandline_usage, argv[0]); + return 0; + } + } + if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) { + verbose = true; } } + /* welcome user */ + printf("[fixedwing control] started\n"); + /* Set up to publish fixed wing control messages */ struct fixedwing_control_s control; int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control); @@ -679,6 +682,12 @@ int fixedwing_control_main(int argc, char *argv[]) int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); struct rc_channels_s rc; int rc_sub = orb_subscribe(ORB_ID(rc_channels)); + struct vehicle_global_position_setpoint_s global_setpoint; + int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + + /* Mainloop setup */ + unsigned int loopcounter = 0; + unsigned int failcounter = 0; /* Control constants */ control_outputs.mode = HIL_MODE; @@ -692,7 +701,7 @@ int fixedwing_control_main(int argc, char *argv[]) fd = open("/dev/pwm_servo", O_RDWR); if (fd < 0) { - printf("failed opening /dev/pwm_servo\n"); + printf("[fixedwing control] Failed opening /dev/pwm_servo\n"); } ioctl(fd, PWM_SERVO_ARM, 0); @@ -728,8 +737,7 @@ int fixedwing_control_main(int argc, char *argv[]) * Main control, navigation and servo routine */ - while(1) - { + while(1) { /* * DATA Handling * Fetch current flight data @@ -738,6 +746,7 @@ int fixedwing_control_main(int argc, char *argv[]) /* get position, attitude and rc inputs */ // XXX add error checking orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); + orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint); orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att); orb_copy(ORB_ID(rc_channels), rc_sub, &rc); @@ -746,12 +755,12 @@ int fixedwing_control_main(int argc, char *argv[]) */ /* position values*/ - plane_data.lat = global_pos.lat / 10000000; - plane_data.lon = global_pos.lon / 10000000; - plane_data.alt = global_pos.alt / 1000; - plane_data.vx = global_pos.vx / 100; - plane_data.vy = global_pos.vy / 100; - plane_data.vz = global_pos.vz / 100; + plane_data.lat = global_pos.lat / 10000000.0; + plane_data.lon = global_pos.lon / 10000000.0; + plane_data.alt = global_pos.alt / 1000.0f; + plane_data.vx = global_pos.vx / 100.0f; + plane_data.vy = global_pos.vy / 100.0f; + plane_data.vz = global_pos.vz / 100.0f; /* attitude values*/ plane_data.roll = att.roll; @@ -766,37 +775,35 @@ int fixedwing_control_main(int argc, char *argv[]) /* Attitude control part */ -//#define MUTE -#ifndef MUTE - /******************************** DEBUG OUTPUT ************************************************************/ - - printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, - (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, - (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, - (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); + if (verbose && loopcounter % 20 == 0) { + /******************************** DEBUG OUTPUT ************************************************************/ -// printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint); -// printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint); -// printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint()); + printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)plane_data.Kp_att, (int)plane_data.Ki_att, + (int)plane_data.Kd_att, (int)plane_data.intmax_att, (int)plane_data.Kp_pos, (int)plane_data.Ki_pos, + (int)plane_data.Kd_pos, (int)plane_data.intmax_pos, (int)plane_data.airspeed, + (int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z); -// printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); + // printf("PITCH SETPOINT: %i\n", (int)plane_data.pitch_setpoint); + // printf("ROLL SETPOINT: %i\n", (int)plane_data.roll_setpoint); + // printf("THROTTLE SETPOINT: %i\n", (int)calc_throttle_setpoint()); -// printf("Current Altitude: %i\n\n", (int)plane_data.alt); + // printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed())); - printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", - (int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw), - (int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed)); + // printf("Current Altitude: %i\n\n", (int)plane_data.alt); -// printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", -// (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); + printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ", + (int)(1000 * plane_data.roll), (int)(1000 * plane_data.pitch), (int)(1000 * plane_data.yaw), + (int)(100 * plane_data.rollspeed), (int)(100 * plane_data.pitchspeed), (int)(100 * plane_data.yawspeed)); - printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n", - (int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator), - (int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle)); + // printf("\nBody Rates: \n P: %i \n Q: %i \n R: %i \n", + // (int)(10000 * plane_data.p), (int)(10000 * plane_data.q), (int)(10000 * plane_data.r)); - /************************************************************************************************************/ + printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n", + (int)(10000 * control_outputs.roll_ailerons), (int)(10000 * control_outputs.pitch_elevator), + (int)(10000 * control_outputs.yaw_rudder), (int)(10000 * control_outputs.throttle)); -#endif + /************************************************************************************************************/ + } /* * Computation section @@ -809,11 +816,11 @@ int fixedwing_control_main(int argc, char *argv[]) set_plane_mode(); /* Calculate the P,Q,R body rates of the aircraft */ - calc_body_angular_rates(plane_data.roll / RAD2DEG, plane_data.pitch / RAD2DEG, plane_data.yaw / RAD2DEG, + calc_body_angular_rates(plane_data.roll, plane_data.pitch, plane_data.yaw, plane_data.rollspeed, plane_data.pitchspeed, plane_data.yawspeed); /* Calculate the body frame angles of the aircraft */ - //calc_bodyframe_angles(plane_data.roll/RAD2DEG,plane_data.pitch/RAD2DEG,plane_data.yaw/RAD2DEG); + //calc_bodyframe_angles(plane_data.roll,plane_data.pitch,plane_data.yaw); /* Calculate the output values */ control_outputs.roll_ailerons = calc_roll_ail(); @@ -869,9 +876,6 @@ int fixedwing_control_main(int argc, char *argv[]) //control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg); - // 10 Hz loop - usleep(100000); - } else { control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale; control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale; @@ -927,9 +931,14 @@ int fixedwing_control_main(int argc, char *argv[]) int result = write(fd, &data, sizeof(data)); if (result != sizeof(data)) { - printf("failed writing servo outputs\n"); + if (failcounter < 10 || failcounter % 20 == 0) { + printf("[fixedwing_control] failed writing servo outputs\n"); + } + failcounter++; } + loopcounter++; + /* 20Hz loop*/ usleep(50000); } diff --git a/apps/fixedwing_control/fixedwing_control.h b/apps/fixedwing_control/fixedwing_control.h deleted file mode 100644 index 6023e3967..000000000 --- a/apps/fixedwing_control/fixedwing_control.h +++ /dev/null @@ -1,66 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. - * Author: Ivan Ovinnikov <oivan@ethz.ch> - * - * 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 Definition of attitude controller - */ - -#ifndef FIXEDWING_CONTROL_H_ -#define FIXEDWING_CONTROL_H_ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - -#include <uORB/uORB.h> -#include <uORB/topics/rc_channels.h> -#include <uORB/topics/vehicle_global_position.h> -#include <uORB/topics/vehicle_attitude.h> -#include <uORB/topics/vehicle_status.h> -#include <uORB/topics/fixedwing_control.h> - -/**************************************************************************** - * Public Data - ****************************************************************************/ - -/**************************************************************************** - * Internal definitions - ****************************************************************************/ - - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -#endif /* FIXEDWING_CONTROL_H_ */ diff --git a/apps/fixedwing_control/pid.c b/apps/fixedwing_control/pid.c index 81b4deac1..652e4143f 100644 --- a/apps/fixedwing_control/pid.c +++ b/apps/fixedwing_control/pid.c @@ -1,8 +1,7 @@ /**************************************************************************** - * pid.c * - * Copyright (C) 2012 Ivan Ovinnikov. All rights reserved. - * Authors: Ivan Ovinnikov <oivan@ethz.ch> + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Ivan Ovinnikov <oivan@ethz.ch> * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -14,7 +13,7 @@ * notice, this list of conditions and the following disclaimer in * the documentation and/or other materials provided with the * distribution. - * 3. Neither the name NuttX nor the names of its contributors may be + * 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. * @@ -33,9 +32,10 @@ * ****************************************************************************/ -/**************************************************************************** - * Included Files - ****************************************************************************/ +/** + * @file pid.c + * Implementation of a fixed wing attitude and position controller. + */ #include "pid.h" #include "fixedwing_control.h" diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 466ea7565..e3c8e5fa1 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -213,7 +213,7 @@ extern void mavlink_missionlib_current_waypoint_changed(uint16_t index, float pa orb_publish(ORB_ID(vehicle_local_position_setpoint), local_position_setpoint_pub, &sp); } - printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x, param6_lon_y, param7_alt_z, param4); + //printf("[mavlink mp] new setpoint: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000); } @@ -375,7 +375,7 @@ static void *receiveloop(void *arg) static void *uorb_receiveloop(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "mavlink uORB async", getpid()); + prctl(PR_SET_NAME, "mavlink uORB", getpid()); /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ @@ -586,7 +586,10 @@ static void *uorb_receiveloop(void *arg) if (fds[6].revents & POLLIN) { /* copy fixed wing control into local buffer */ orb_copy(ORB_ID(fixedwing_control), fw_sub, &fw_control); - // XXX Output fixed wing control + /* send control output via MAVLink */ + mavlink_msg_roll_pitch_yaw_thrust_setpoint_send(MAVLINK_COMM_0, fw_control.timestamp / 1000, fw_control.attitude_control_output[0], + fw_control.attitude_control_output[1], fw_control.attitude_control_output[2], + fw_control.attitude_control_output[3]); } /* --- VEHICLE GLOBAL POSITION --- */ @@ -594,15 +597,15 @@ static void *uorb_receiveloop(void *arg) /* copy global position data into local buffer */ orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos); uint64_t timestamp = global_pos.timestamp; - int32_t lat = (int32_t)(global_pos.lat * 1e7); - int32_t lon = (int32_t)(global_pos.lon * 1e7); - int32_t alt = (int32_t)(global_pos.alt * 1e3); - int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1e3); - int16_t vx = (int16_t)(global_pos.vx * 1e2); - int16_t vy = (int16_t)(global_pos.vy * 1e2); - int16_t vz = (int16_t)(global_pos.vz * 1e2); + int32_t lat = global_pos.lat; + int32_t lon = global_pos.lon; + int32_t alt = (int32_t)(global_pos.alt*1000); + int32_t relative_alt = (int32_t)(global_pos.relative_alt * 1000.0f); + int16_t vx = (int16_t)(global_pos.vx * 100.0f); + int16_t vy = (int16_t)(global_pos.vy * 100.0f); + int16_t vz = (int16_t)(global_pos.vz * 100.0f); /* heading in degrees * 10, from 0 to 36.000) */ - uint16_t hdg = (global_pos.hdg / M_PI_F) * (180 * 10) + (180 * 10); + uint16_t hdg = (global_pos.hdg / M_PI_F) * (180.0f * 10.0f) + (180.0f * 10.0f); mavlink_msg_global_position_int_send(MAVLINK_COMM_0, timestamp / 1000, lat, lon, alt, relative_alt, vx, vy, vz, hdg); } diff --git a/apps/px4/attitude_estimator_bm/Makefile b/apps/px4/attitude_estimator_bm/Makefile index 358b062c0..cf971ae05 100644 --- a/apps/px4/attitude_estimator_bm/Makefile +++ b/apps/px4/attitude_estimator_bm/Makefile @@ -37,9 +37,6 @@ APPNAME = attitude_estimator_bm PRIORITY = SCHED_PRIORITY_MAX - 10 -STACKSIZE = 3000 - -# XXX this is *horribly* broken -INCLUDES = $(TOPDIR)/../mavlink/include/mavlink +STACKSIZE = 4096 include $(APPDIR)/mk/app.mk diff --git a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c index 3c6c8eec3..8551b5e1c 100644 --- a/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c +++ b/apps/px4/attitude_estimator_bm/attitude_estimator_bm.c @@ -33,7 +33,10 @@ * ****************************************************************************/ -/* @file Black Magic Attitude Estimator */ +/** + * @file attitude_estimator_bm.c + * Black Magic Attitude Estimator + */ @@ -49,6 +52,7 @@ #include <uORB/uORB.h> #include <uORB/topics/sensor_combined.h> #include <uORB/topics/vehicle_attitude.h> +#include <uORB/topics/vehicle_status.h> #include <math.h> #include <errno.h> @@ -135,14 +139,22 @@ int attitude_estimator_bm_main(int argc, char *argv[]) /* rate-limit raw data updates to 200Hz */ //orb_set_interval(sub_raw, 5); + bool hil_enabled = false; + bool publishing = false; + /* advertise attitude */ int pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + publishing = true; struct pollfd fds[] = { { .fd = sub_raw, .events = POLLIN }, }; - // int paramcounter = 100; + /* subscribe to system status */ + struct vehicle_status_s vstatus = {0}; + int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + + unsigned int loopcounter = 0; /* Main loop*/ while (true) { @@ -217,8 +229,29 @@ int attitude_estimator_bm_main(int argc, char *argv[]) att.R[0][2] = x_n_b.z; // XXX add remaining entries + if (loopcounter % 250 == 0) { + /* Check HIL state */ + orb_copy(ORB_ID(vehicle_status), vstatus_sub, &vstatus); + /* switching from non-HIL to HIL mode */ + if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { + hil_enabled = true; + publishing = false; + close(pub_att); + + /* switching from HIL to non-HIL mode */ + + } else if (!publishing && !hil_enabled) { + /* advertise the topic and make the initial publication */ + pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); + hil_enabled = false; + publishing = true; + } + } + // Broadcast - orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + if (publishing) orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); + + loopcounter++; } /* Should never reach here */ diff --git a/apps/px4/px4io/driver/px4io.cpp b/apps/px4/px4io/driver/px4io.cpp index c3f14e3a4..3a3ff6407 100644 --- a/apps/px4/px4io/driver/px4io.cpp +++ b/apps/px4/px4io/driver/px4io.cpp @@ -32,7 +32,8 @@ ****************************************************************************/ /** - * @file Driver for the PX4IO board. + * @file px4io.cpp + * Driver for the PX4IO board. * * PX4IO is connected via serial (or possibly some other interface at a later * point). @@ -555,6 +556,6 @@ px4io_main(int argc, char *argv[]) - printf("need a verb, only support 'start'\n"); + printf("need a verb, only support 'start' and 'update'\n"); return ERROR; } diff --git a/apps/px4io/px4io.c b/apps/px4io/px4io.c index 44b62f3f9..30a62fa65 100644 --- a/apps/px4io/px4io.c +++ b/apps/px4io/px4io.c @@ -32,14 +32,10 @@ ****************************************************************************/ /** - * @file Top-level logic for the PX4IO module. + * @file px4io.c + * Top-level logic for the PX4IO module. */ -/**************************************************************************** - * Included Files - ****************************************************************************/ - - #include <nuttx/config.h> #include <stdio.h> #include <stdbool.h> @@ -58,24 +54,8 @@ #include "px4io.h" -/**************************************************************************** - * Definitions - ****************************************************************************/ - -/**************************************************************************** - * Private Data - ****************************************************************************/ - -/**************************************************************************** - * Public Functions - ****************************************************************************/ - __EXPORT int user_start(int argc, char *argv[]); -/**************************************************************************** - * user_start - ****************************************************************************/ - struct sys_state_s system_state; int gpio_fd; diff --git a/apps/sensors/sensors.c b/apps/sensors/sensors.c index a88361e1a..3343b33f4 100644 --- a/apps/sensors/sensors.c +++ b/apps/sensors/sensors.c @@ -90,17 +90,14 @@ #define BAT_VOL_LOWPASS_2 0.01f #define VOLTAGE_BATTERY_IGNORE_THRESHOLD_VOLTS 3.5f -/*PPM Settings*/ +/* PPM Settings */ #define PPM_MIN 1000 #define PPM_MAX 2000 -/*Our internal resolution is 10000*/ +/* Internal resolution is 10000 */ #define PPM_SCALE 10000/((PPM_MAX-PPM_MIN)/2) #define PPM_MID (PPM_MIN+PPM_MAX)/2 -/**************************************************************************** - * Definitions - ****************************************************************************/ static pthread_cond_t sensors_read_ready; static pthread_mutex_t sensors_read_ready_mutex; @@ -281,7 +278,8 @@ int sensors_main(int argc, char *argv[]) bool baro_healthy = false; bool adc_healthy = false; - bool hil_enabled = false; + bool hil_enabled = false; /**< HIL is disabled by default */ + bool publishing = false; /**< the app is not publishing by default, only if HIL is disabled on first run */ int magcounter = 0; int barocounter = 0; @@ -318,16 +316,19 @@ int sensors_main(int argc, char *argv[]) int16_t acc_offset[3] = {0, 0, 0}; int16_t gyro_offset[3] = {0, 0, 0}; + #pragma pack(push,1) struct adc_msg4_s { - uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ - int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ - uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ - int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ - uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ - int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ - uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ - int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ - } __attribute__((__packed__));; + uint8_t am_channel1; /**< The 8-bit ADC Channel 1 */ + int32_t am_data1; /**< ADC convert result 1 (4 bytes) */ + uint8_t am_channel2; /**< The 8-bit ADC Channel 2 */ + int32_t am_data2; /**< ADC convert result 2 (4 bytes) */ + uint8_t am_channel3; /**< The 8-bit ADC Channel 3 */ + int32_t am_data3; /**< ADC convert result 3 (4 bytes) */ + uint8_t am_channel4; /**< The 8-bit ADC Channel 4 */ + int32_t am_data4; /**< ADC convert result 4 (4 bytes) */ + }; + #pragma pack(pop) + struct adc_msg4_s buf_adc; size_t adc_readsize = 1 * sizeof(struct adc_msg4_s); @@ -335,7 +336,7 @@ int sensors_main(int argc, char *argv[]) battery_voltage_conversion = global_data_parameter_storage->pm.param_values[PARAM_BATTERYVOLTAGE_CONVERSION]; if (-1.0f == battery_voltage_conversion) { - /**< default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ + /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ battery_voltage_conversion = 3.3f * 52.0f / 5.0f / 4095.0f; } @@ -383,13 +384,14 @@ int sensors_main(int argc, char *argv[]) /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); + publishing = true; /* advertise the rc topic */ struct rc_channels_s rc = {0}; int rc_pub = orb_advertise(ORB_ID(rc_channels), &rc); /* subscribe to system status */ - struct vehicle_status_s vstatus; + struct vehicle_status_s vstatus = {0}; int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -430,14 +432,16 @@ int sensors_main(int argc, char *argv[]) /* switching from non-HIL to HIL mode */ if ((vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && !hil_enabled) { hil_enabled = true; + publishing = false; close(sensor_pub); /* switching from HIL to non-HIL mode */ - } else if (!(vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED) && hil_enabled) { + } else if (!publishing && !hil_enabled) { /* advertise the topic and make the initial publication */ sensor_pub = orb_advertise(ORB_ID(sensor_combined), &raw); hil_enabled = false; + publishing = true; } @@ -556,7 +560,7 @@ int sensors_main(int argc, char *argv[]) if (acctime > 500) printf("ACC: %d us\n", acctime); /* MAGNETOMETER */ - if (magcounter == 4) { //(magcounter == 4) // 100 Hz + if (magcounter == 4) { /* 120 Hz */ uint64_t start_mag = hrt_absolute_time(); ret_magnetometer = read(fd_magnetometer, buf_magnetometer, sizeof(buf_magnetometer)); int errcode_mag = (int) * get_errno_ptr(); @@ -604,7 +608,7 @@ int sensors_main(int argc, char *argv[]) magcounter++; /* BAROMETER */ - if (barocounter == 5 && (fd_barometer > 0)) { //(barocounter == 4) // 100 Hz + if (barocounter == 5 && (fd_barometer > 0)) { /* 100 Hz */ uint64_t start_baro = hrt_absolute_time(); *get_errno_ptr() = 0; ret_barometer = read(fd_barometer, buf_barometer, sizeof(buf_barometer)); @@ -798,7 +802,7 @@ int sensors_main(int argc, char *argv[]) uint64_t total_time = hrt_absolute_time() - current_time; /* Inform other processes that new data is available to copy */ - if ((gyro_updated || acc_updated || magn_updated || baro_updated) && !hil_enabled) { + if ((gyro_updated || acc_updated || magn_updated || baro_updated) && publishing) { /* Values changed, publish */ orb_publish(ORB_ID(sensor_combined), sensor_pub, &raw); } diff --git a/nuttx/configs/px4fmu/include/nsh_romfsimg.h b/nuttx/configs/px4fmu/include/nsh_romfsimg.h index 799670c4d..9965277c7 100644 --- a/nuttx/configs/px4fmu/include/nsh_romfsimg.h +++ b/nuttx/configs/px4fmu/include/nsh_romfsimg.h @@ -234,7 +234,7 @@ unsigned char romfs_img[] = { 0x69, 0x73, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x46, 0x41, 0x49, 0x4c, 0x45, 0x44, 0x2e, 0x22, 0x0a, 0x23, 0x09, 0x72, 0x65, 0x62, 0x6f, 0x6f, 0x74, 0x0a, 0x23, 0x66, 0x69, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x0e, 0xf2, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcd, 0x52, 0xce, 0xe0, 0xfc, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x03, 0xcc, 0x52, 0xce, 0xe0, 0xfd, 0x72, 0x63, 0x2e, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x46, 0x6c, 0x69, 0x67, 0x68, 0x74, 0x20, 0x73, 0x74, 0x61, @@ -248,76 +248,76 @@ unsigned char romfs_img[] = { 0x34, 0x46, 0x4d, 0x55, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x2e, 0x2e, 0x2e, 0x22, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x4f, 0x52, 0x42, 0x0a, - 0x23, 0x0a, 0x23, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72, - 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, - 0x20, 0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, - 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, - 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73, - 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, - 0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e, - 0x6b, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e, - 0x6b, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74, - 0x79, 0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30, - 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, - 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, - 0x64, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, - 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, - 0x20, 0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, - 0x65, 0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, - 0x0a, 0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20, + 0x23, 0x0a, 0x75, 0x6f, 0x72, 0x62, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, + 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20, + 0x74, 0x68, 0x65, 0x20, 0x73, 0x65, 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x2e, + 0x0a, 0x23, 0x0a, 0x23, 0x73, 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, + 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x73, 0x65, + 0x6e, 0x73, 0x6f, 0x72, 0x73, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, + 0x74, 0x61, 0x72, 0x74, 0x20, 0x4d, 0x41, 0x56, 0x4c, 0x69, 0x6e, 0x6b, + 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x6d, 0x61, 0x76, 0x6c, 0x69, 0x6e, 0x6b, + 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, 0x2f, 0x74, 0x74, 0x79, + 0x53, 0x30, 0x20, 0x2d, 0x62, 0x20, 0x35, 0x37, 0x36, 0x30, 0x30, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, - 0x20, 0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64, - 0x65, 0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a, - 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, - 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27, - 0x3c, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74, - 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74, - 0x69, 0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, - 0x74, 0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f, - 0x73, 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, - 0x61, 0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, - 0x53, 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69, - 0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e, - 0x74, 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, - 0x20, 0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, - 0x74, 0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, - 0x69, 0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66, - 0x69, 0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f, - 0x20, 0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68, - 0x65, 0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f, - 0x61, 0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, - 0x67, 0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69, - 0x78, 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65, - 0x3f, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, - 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, - 0x20, 0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f, - 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72, - 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64, - 0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, - 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66, - 0x69, 0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f, - 0x72, 0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, - 0x20, 0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, - 0x58, 0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f, - 0x0a, 0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73, - 0x74, 0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, - 0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20, - 0x66, 0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23, + 0x20, 0x74, 0x68, 0x65, 0x20, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, + 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, + 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, + 0x62, 0x65, 0x20, 0x27, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, + 0x72, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, + 0x23, 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x65, 0x72, 0x20, 0x26, + 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, 0x72, 0x74, 0x20, + 0x74, 0x68, 0x65, 0x20, 0x61, 0x74, 0x74, 0x69, 0x74, 0x75, 0x64, 0x65, + 0x20, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, 0x6f, 0x72, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, - 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e, - 0x65, 0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61, - 0x63, 0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23, - 0x0a, 0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, - 0x76, 0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61, - 0x6c, 0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, - 0x61, 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20, - 0x74, 0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69, - 0x66, 0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73, - 0x68, 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, - 0x64, 0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, - 0x0a, 0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69, - 0x74, 0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64, - 0x6f, 0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, + 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, 0x27, 0x3c, + 0x63, 0x6f, 0x6d, 0x6d, 0x61, 0x6e, 0x64, 0x3e, 0x20, 0x73, 0x74, 0x61, + 0x72, 0x74, 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x61, 0x74, 0x74, 0x69, + 0x74, 0x75, 0x64, 0x65, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, 0x74, + 0x6f, 0x72, 0x5f, 0x62, 0x6d, 0x20, 0x26, 0x0a, 0x23, 0x70, 0x6f, 0x73, + 0x69, 0x74, 0x69, 0x6f, 0x6e, 0x5f, 0x65, 0x73, 0x74, 0x69, 0x6d, 0x61, + 0x74, 0x6f, 0x72, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, + 0x74, 0x61, 0x72, 0x74, 0x20, 0x74, 0x68, 0x65, 0x20, 0x66, 0x69, 0x78, + 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x63, 0x6f, 0x6e, 0x74, + 0x72, 0x6f, 0x6c, 0x6c, 0x65, 0x72, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x20, + 0x58, 0x58, 0x58, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x74, + 0x68, 0x69, 0x73, 0x20, 0x62, 0x65, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, + 0x6e, 0x67, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, + 0x67, 0x75, 0x72, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x20, 0x74, 0x6f, 0x20, + 0x64, 0x65, 0x63, 0x69, 0x64, 0x65, 0x0a, 0x23, 0x20, 0x77, 0x68, 0x65, + 0x74, 0x68, 0x65, 0x72, 0x20, 0x74, 0x68, 0x65, 0x20, 0x62, 0x6f, 0x61, + 0x72, 0x64, 0x20, 0x69, 0x73, 0x20, 0x63, 0x6f, 0x6e, 0x66, 0x69, 0x67, + 0x75, 0x72, 0x65, 0x64, 0x20, 0x66, 0x6f, 0x72, 0x20, 0x66, 0x69, 0x78, + 0x65, 0x64, 0x2d, 0x77, 0x69, 0x6e, 0x67, 0x20, 0x75, 0x73, 0x65, 0x3f, + 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, + 0x73, 0x20, 0x73, 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x62, 0x65, 0x20, + 0x27, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, 0x69, 0x6e, 0x67, 0x5f, 0x63, + 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, + 0x27, 0x2e, 0x0a, 0x23, 0x0a, 0x23, 0x66, 0x69, 0x78, 0x65, 0x64, 0x77, + 0x69, 0x6e, 0x67, 0x5f, 0x63, 0x6f, 0x6e, 0x74, 0x72, 0x6f, 0x6c, 0x20, + 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x43, 0x6f, 0x6e, 0x66, 0x69, + 0x67, 0x75, 0x72, 0x65, 0x20, 0x46, 0x4d, 0x55, 0x20, 0x66, 0x6f, 0x72, + 0x20, 0x73, 0x74, 0x61, 0x6e, 0x64, 0x61, 0x6c, 0x6f, 0x6e, 0x65, 0x20, + 0x6d, 0x6f, 0x64, 0x65, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x58, 0x58, 0x58, + 0x20, 0x61, 0x72, 0x67, 0x75, 0x6d, 0x65, 0x6e, 0x74, 0x73, 0x3f, 0x0a, + 0x23, 0x0a, 0x23, 0x70, 0x78, 0x34, 0x66, 0x6d, 0x75, 0x20, 0x73, 0x74, + 0x61, 0x72, 0x74, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, + 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x6f, 0x6b, 0x69, 0x6e, 0x67, 0x20, 0x66, + 0x6f, 0x72, 0x20, 0x61, 0x20, 0x47, 0x50, 0x53, 0x2e, 0x0a, 0x23, 0x0a, + 0x23, 0x20, 0x58, 0x58, 0x58, 0x20, 0x74, 0x68, 0x69, 0x73, 0x20, 0x73, + 0x68, 0x6f, 0x75, 0x6c, 0x64, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x6e, 0x65, + 0x65, 0x64, 0x20, 0x74, 0x6f, 0x20, 0x62, 0x65, 0x20, 0x62, 0x61, 0x63, + 0x6b, 0x67, 0x72, 0x6f, 0x75, 0x6e, 0x64, 0x65, 0x64, 0x0a, 0x23, 0x0a, + 0x23, 0x67, 0x70, 0x73, 0x20, 0x2d, 0x64, 0x20, 0x2f, 0x64, 0x65, 0x76, + 0x2f, 0x74, 0x74, 0x79, 0x53, 0x33, 0x20, 0x2d, 0x6d, 0x20, 0x61, 0x6c, + 0x6c, 0x20, 0x26, 0x0a, 0x0a, 0x23, 0x0a, 0x23, 0x20, 0x53, 0x74, 0x61, + 0x72, 0x74, 0x20, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x20, 0x74, + 0x6f, 0x20, 0x6d, 0x69, 0x63, 0x72, 0x6f, 0x53, 0x44, 0x20, 0x69, 0x66, + 0x20, 0x77, 0x65, 0x20, 0x63, 0x61, 0x6e, 0x0a, 0x23, 0x0a, 0x73, 0x68, + 0x20, 0x2f, 0x65, 0x74, 0x63, 0x2f, 0x69, 0x6e, 0x69, 0x74, 0x2e, 0x64, + 0x2f, 0x72, 0x63, 0x2e, 0x6c, 0x6f, 0x67, 0x67, 0x69, 0x6e, 0x67, 0x0a, + 0x0a, 0x65, 0x63, 0x68, 0x6f, 0x20, 0x22, 0x5b, 0x69, 0x6e, 0x69, 0x74, + 0x5d, 0x20, 0x73, 0x74, 0x61, 0x72, 0x74, 0x75, 0x70, 0x20, 0x64, 0x6f, + 0x6e, 0x65, 0x22, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0a, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x09, 0x76, 0x8d, 0x9c, 0xa3, 0x80, 0x72, 0x63, 0x53, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x23, 0x21, 0x6e, 0x73, 0x68, 0x0a, 0x23, 0x0a, diff --git a/nuttx/configs/px4fmu/src/drv_hmc5833l.c b/nuttx/configs/px4fmu/src/drv_hmc5833l.c index 0e6dbe2ac..2a6d04306 100644 --- a/nuttx/configs/px4fmu/src/drv_hmc5833l.c +++ b/nuttx/configs/px4fmu/src/drv_hmc5833l.c @@ -204,7 +204,7 @@ read_values(int16_t *data) int hmc_status = hmc5883l_read_reg(ADDR_STATUS); if (hmc_status < 0) { - if (ret == ETIMEDOUT) hmc5883l_reset(); + if (hmc_status == ETIMEDOUT) hmc5883l_reset(); ret = hmc_status; } else diff --git a/nuttx/configs/px4fmu/src/drv_ms5611.c b/nuttx/configs/px4fmu/src/drv_ms5611.c index 7fea65159..c7e91c5ea 100644 --- a/nuttx/configs/px4fmu/src/drv_ms5611.c +++ b/nuttx/configs/px4fmu/src/drv_ms5611.c @@ -123,6 +123,16 @@ static FAR struct { static int ms5611_read_prom(void); +int ms5611_reset() +{ + int ret; + printf("[ms5611 drv] Resettet I2C2 BUS\n"); + up_i2cuninitialize(ms5611_dev.i2c); + ms5611_dev.i2c = up_i2cinitialize(2); + I2C_SETFREQUENCY(ms5611_dev.i2c, 400000); + return ret; +} + static bool read_values(float *data) { @@ -279,6 +289,7 @@ read_values(float *data) else { errno = -ret; + if (errno == ETIMEDOUT || ret == ETIMEDOUT) ms5611_reset(); return ret; } } |