From fcebafba778ff981f1b485d7cb30fed5dce6295c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Aug 2014 09:24:37 +0200 Subject: Obey vision estimates in attitude estimator if available --- .../attitude_estimator_ekf_main.cpp | 52 +++++++++++++++------- 1 file changed, 36 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 35dc39ec6..6bbc26a65 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -62,6 +62,7 @@ #include #include #include +#include #include #include @@ -206,10 +207,6 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ - // print text - printf("Extended Kalman Filter Attitude Estimator initialized..\n\n"); - fflush(stdout); - int overloadcounter = 19; /* Initialize filter */ @@ -222,6 +219,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); + gps.eph = 100000; + gps.epv = 100000; struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); struct vehicle_attitude_s att; @@ -260,9 +259,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to control mode*/ + /* subscribe to control mode */ int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + /* subscribe to vision estimate */ + int vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -270,16 +272,12 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds thread_running = true; - /* advertise debug value */ - // struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; - // orb_advert_t pub_dbg = -1; - /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params; + struct attitude_estimator_ekf_params ekf_params {}; - struct attitude_estimator_ekf_param_handles ekf_param_handles; + struct attitude_estimator_ekf_param_handles ekf_param_handles {}; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -295,6 +293,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds math::Matrix<3, 3> R_decl; R_decl.identity(); + struct vision_position_estimate vision {}; + /* register the perf counter */ perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); @@ -315,8 +315,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); if (!control_mode.flag_system_hil_enabled) { - fprintf(stderr, - "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); + warnx("WARNING: Not getting sensors - sensor app running?"); } } else { @@ -451,9 +450,30 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds sensor_last_timestamp[2] = raw.magnetometer_timestamp; } - z_k[6] = raw.magnetometer_ga[0]; - z_k[7] = raw.magnetometer_ga[1]; - z_k[8] = raw.magnetometer_ga[2]; + bool vision_updated = false; + orb_check(vision_sub, &vision_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); + } + + if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000) && gps.eph > 3.0f) { + + math::Quaternion q(vision.q); + math::Matrix<3, 3> R = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + math::Vector<3> vn = R * v; + + z_k[6] = vn(0); + z_k[7] = vn(1); + z_k[8] = vn(2); + } else { + z_k[6] = raw.magnetometer_ga[0]; + z_k[7] = raw.magnetometer_ga[1]; + z_k[8] = raw.magnetometer_ga[2]; + } uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; -- cgit v1.2.3 From 368afa1c1d1cad13b4dfaa3f7215dd399300d817 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 27 Aug 2014 09:47:31 +0200 Subject: Start the laser on a port not offending the 2nd MAVLink instance --- src/drivers/sf0x/sf0x.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 80ecab2ee..5f7c0a84a 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -88,7 +88,9 @@ static const int ERROR = -1; #define SF0X_TAKE_RANGE_REG 'd' #define SF02F_MIN_DISTANCE 0.0f #define SF02F_MAX_DISTANCE 40.0f -#define SF0X_DEFAULT_PORT "/dev/ttyS2" + +// designated SERIAL4/5 on Pixhawk +#define SF0X_DEFAULT_PORT "/dev/ttyS6" class SF0X : public device::CDev { -- cgit v1.2.3 From 8eb5aeaae3154f3faa6ac493ddc3d6aefdbaecc0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 21:04:27 +0200 Subject: Use vision for heading reference whenever available, independent of GPS --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6bbc26a65..cd7c56b87 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -457,7 +457,7 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds orb_copy(ORB_ID(vision_position_estimate), vision_sub, &vision); } - if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000) && gps.eph > 3.0f) { + if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); math::Matrix<3, 3> R = q.to_dcm(); -- cgit v1.2.3 From 428b9612abd3ca892a7240d284916c50e2496cea Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 1 Sep 2014 21:04:44 +0200 Subject: INAV: Adjust vision default params --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 4 ++-- src/modules/position_estimator_inav/position_estimator_inav_params.c | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 81bbaa658..3f59eb597 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -235,8 +235,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; - float eph_vision = 0.5f; - float epv_vision = 0.5f; + float eph_vision = 0.2f; + float epv_vision = 0.2f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155..b7f6509d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); /** * XY axis weight for vision velocity -- cgit v1.2.3 From 821c06f7cc58b50afe80442ee2258bf99cbe1fd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Sep 2014 17:46:02 +0200 Subject: Support speed estimate --- .../position_estimator_inav_main.c | 37 +++++++++++++++++++--- 1 file changed, 33 insertions(+), 4 deletions(-) (limited to 'src') diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 3f59eb597..6160200f3 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -641,6 +641,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + static float last_vision_x = 0.0f; + static float last_vision_y = 0.0f; + static float last_vision_z = 0.0f; + /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; @@ -655,6 +659,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } vision_valid = true; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } @@ -664,10 +673,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; - /* calculate correction for velocity */ - corr_vision[0][1] = vision.vx - x_est[1]; - corr_vision[1][1] = vision.vy - y_est[1]; - corr_vision[2][1] = vision.vz - z_est[1]; + static hrt_abstime last_vision_time = 0; + + float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; + last_vision_time = vision.timestamp_boot; + + if (vision_dt > 0.000001f && vision_dt < 0.2f) { + vision.vx = (vision.x - last_vision_x) / vision_dt; + vision.vy = (vision.y - last_vision_y) / vision_dt; + vision.vz = (vision.z - last_vision_z) / vision_dt; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + + /* calculate correction for velocity */ + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; + } else { + /* assume zero motion */ + corr_vision[0][1] = 0.0f - x_est[1]; + corr_vision[1][1] = 0.0f - y_est[1]; + corr_vision[2][1] = 0.0f - z_est[1]; + } } } -- cgit v1.2.3 From 30d851284606476642c6cf8a54070bc08ab923c6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 19 Nov 2014 12:31:04 +0100 Subject: MC pos control offb: read altitude sp separately --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index d52138522..f2c650ddd 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -652,8 +652,6 @@ MulticopterPositionControl::control_offboard(float dt) /* control position */ _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; - _pos_sp(2) = _pos_sp_triplet.current.z; - } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ /* reset position setpoint to current position if needed */ @@ -670,7 +668,10 @@ MulticopterPositionControl::control_offboard(float dt) _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } - if (_control_mode.flag_control_altitude_enabled) { + if (_control_mode.flag_control_altitude_enabled && _pos_sp_triplet.current.position_valid) { + /* Control altitude */ + _pos_sp(2) = _pos_sp_triplet.current.z; + } else if (_control_mode.flag_control_climb_rate_enabled && _pos_sp_triplet.current.velocity_valid) { /* reset alt setpoint to current altitude if needed */ reset_alt_sp(); -- cgit v1.2.3 From d511e39ea7a3f1e0cb672b14598e18a7df18156a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 22 Dec 2014 17:09:43 -0500 Subject: turn on -Werror and fix resulting errors --- makefiles/toolchain_gnu-arm-eabi.mk | 1 + src/drivers/ardrone_interface/ardrone_interface.c | 2 +- src/drivers/boards/aerocore/aerocore_led.c | 3 ++- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 2 +- src/drivers/px4flow/module.mk | 2 ++ src/drivers/px4flow/px4flow.cpp | 2 +- src/drivers/roboclaw/roboclaw_main.cpp | 2 +- src/drivers/trone/trone.cpp | 2 +- src/examples/fixedwing_control/main.c | 2 +- src/examples/fixedwing_control/module.mk | 2 ++ .../flow_position_estimator/flow_position_estimator_main.c | 2 +- src/examples/flow_position_estimator/module.mk | 2 ++ src/examples/hwtest/hwtest.c | 12 +++++++----- src/examples/matlab_csv_serial/matlab_csv_serial.c | 2 +- src/examples/px4_daemon_app/px4_daemon_app.c | 9 ++++++--- src/lib/conversion/rotation.cpp | 5 +++++ src/lib/rc/st24.c | 2 +- .../attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 5 +++-- src/modules/attitude_estimator_ekf/module.mk | 5 +++++ .../attitude_estimator_so3/attitude_estimator_so3_main.cpp | 2 +- src/modules/attitude_estimator_so3/module.mk | 2 ++ src/modules/bottle_drop/bottle_drop.cpp | 7 +++++++ src/modules/commander/commander.cpp | 2 +- src/modules/commander/module.mk | 2 ++ src/modules/ekf_att_pos_estimator/module.mk | 2 +- src/modules/fixedwing_backside/fixedwing_backside_main.cpp | 2 +- src/modules/fw_pos_control_l1/module.mk | 2 ++ src/modules/mavlink/mavlink_ftp.cpp | 2 +- src/modules/mavlink/mavlink_ftp.h | 2 +- src/modules/mavlink/mavlink_main.cpp | 2 +- src/modules/mavlink/mavlink_messages.cpp | 3 +++ src/modules/navigator/module.mk | 2 ++ src/modules/position_estimator_inav/module.mk | 2 ++ .../position_estimator_inav/position_estimator_inav_main.c | 2 +- src/modules/sdlog2/module.mk | 2 ++ src/modules/sdlog2/sdlog2.c | 4 +++- src/modules/segway/segway_main.cpp | 2 +- src/modules/sensors/module.mk | 2 ++ src/modules/systemlib/module.mk | 2 ++ src/modules/systemlib/systemlib.c | 2 +- src/modules/systemlib/systemlib.h | 2 +- src/modules/vtol_att_control/module.mk | 2 ++ src/systemcmds/mixer/module.mk | 2 ++ src/systemcmds/mtd/module.mk | 2 ++ src/systemcmds/param/param.c | 2 +- src/systemcmds/tests/module.mk | 2 ++ 48 files changed, 95 insertions(+), 35 deletions(-) (limited to 'src') diff --git a/makefiles/toolchain_gnu-arm-eabi.mk b/makefiles/toolchain_gnu-arm-eabi.mk index 84e5cd08a..7119c723b 100644 --- a/makefiles/toolchain_gnu-arm-eabi.mk +++ b/makefiles/toolchain_gnu-arm-eabi.mk @@ -127,6 +127,7 @@ ARCHCXXFLAGS = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics # ARCHWARNINGS = -Wall \ -Wextra \ + -Werror \ -Wdouble-promotion \ -Wshadow \ -Wfloat-equal \ diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 9d2c1441d..7f1b21a95 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -121,7 +121,7 @@ int ardrone_interface_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 15, 1100, ardrone_interface_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/boards/aerocore/aerocore_led.c b/src/drivers/boards/aerocore/aerocore_led.c index e40d1730c..94a716029 100644 --- a/src/drivers/boards/aerocore/aerocore_led.c +++ b/src/drivers/boards/aerocore/aerocore_led.c @@ -45,6 +45,7 @@ #include "board_config.h" #include +#include /* * Ideally we'd be able to get these from up_internal.h, @@ -54,7 +55,7 @@ * CONFIG_ARCH_LEDS configuration switch. */ __BEGIN_DECLS -extern void led_init(); +extern void led_init(void); extern void led_on(int led); extern void led_off(int led); extern void led_toggle(int led); diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index bccdf1190..5035600ef 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -221,7 +221,7 @@ int frsky_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, frsky_telemetry_thread_main, - (const char **)argv); + (char * const *)argv); while (!thread_running) { usleep(200); diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index 8ab9d8d55..4b8e0c0b0 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -214,7 +214,7 @@ hott_sensors_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 1024, hott_sensors_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index edbb14172..17a24d104 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -240,7 +240,7 @@ hott_telemetry_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2048, hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk index 460bec7b9..ecd3e804a 100644 --- a/src/drivers/px4flow/module.mk +++ b/src/drivers/px4flow/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = px4flow SRCS = px4flow.cpp MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-attributes diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 09ec4bf96..62308fc65 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -106,7 +106,7 @@ struct i2c_frame { }; struct i2c_frame f; -typedef struct i2c_integral_frame { +struct i2c_integral_frame { uint16_t frame_count_since_last_readout; int16_t pixel_flow_x_integral; int16_t pixel_flow_y_integral; diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp index 44ed03254..83086fd7c 100644 --- a/src/drivers/roboclaw/roboclaw_main.cpp +++ b/src/drivers/roboclaw/roboclaw_main.cpp @@ -109,7 +109,7 @@ int roboclaw_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 2048, roboclaw_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } else if (!strcmp(argv[1], "test")) { diff --git a/src/drivers/trone/trone.cpp b/src/drivers/trone/trone.cpp index 83b5c987e..cf3546669 100644 --- a/src/drivers/trone/trone.cpp +++ b/src/drivers/trone/trone.cpp @@ -206,7 +206,7 @@ static const uint8_t crc_table[] = { 0xfa, 0xfd, 0xf4, 0xf3 }; -uint8_t crc8(uint8_t *p, uint8_t len){ +static uint8_t crc8(uint8_t *p, uint8_t len) { uint16_t i; uint16_t crc = 0x0; diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c index 6a5cbcd30..fcbb54c8e 100644 --- a/src/examples/fixedwing_control/main.c +++ b/src/examples/fixedwing_control/main.c @@ -423,7 +423,7 @@ int ex_fixedwing_control_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 20, 2048, fixedwing_control_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); thread_running = true; exit(0); } diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index a2a9eb113..3fab9c8d4 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -41,3 +41,5 @@ SRCS = main.c \ params.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 0b8c01f79..a89ccf933 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -114,7 +114,7 @@ int flow_position_estimator_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 4000, flow_position_estimator_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/flow_position_estimator/module.mk b/src/examples/flow_position_estimator/module.mk index 88c9ceb93..5c6e29f8f 100644 --- a/src/examples/flow_position_estimator/module.mk +++ b/src/examples/flow_position_estimator/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = flow_position_estimator SRCS = flow_position_estimator_main.c \ flow_position_estimator_params.c + +EXTRACFLAGS = -Wno-float-equal diff --git a/src/examples/hwtest/hwtest.c b/src/examples/hwtest/hwtest.c index d3b10f46e..95ff346bb 100644 --- a/src/examples/hwtest/hwtest.c +++ b/src/examples/hwtest/hwtest.c @@ -39,13 +39,15 @@ * @author Lorenz Meier */ -#include #include -#include +#include + #include -#include -#include +#include +#include #include +#include +#include __EXPORT int ex_hwtest_main(int argc, char *argv[]); @@ -53,7 +55,7 @@ int ex_hwtest_main(int argc, char *argv[]) { warnx("DO NOT FORGET TO STOP THE COMMANDER APP!"); warnx("(run to do so)"); - warnx("usage: http://px4.io/dev/examples/write_output"); + warnx("usage: http://px4.io/dev/examples/write_output"); struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index c66bebeec..a95f45d1a 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -107,7 +107,7 @@ int matlab_csv_serial_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 2000, matlab_csv_serial_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c index 3eaf14148..45d541137 100644 --- a/src/examples/px4_daemon_app/px4_daemon_app.c +++ b/src/examples/px4_daemon_app/px4_daemon_app.c @@ -38,10 +38,13 @@ * @author Example User */ +#include +#include +#include +#include + #include #include -#include -#include #include #include @@ -100,7 +103,7 @@ int px4_daemon_app_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2000, px4_daemon_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index e17715733..3d3e13355 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -181,6 +181,11 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) x = tmp; return; } + case ROTATION_ROLL_270_YAW_270: { + // unimplemented + ASSERT(0 == 1); + return; + } case ROTATION_PITCH_90: { tmp = z; z = -x; x = tmp; return; diff --git a/src/lib/rc/st24.c b/src/lib/rc/st24.c index e8a791b8f..5c53a1602 100644 --- a/src/lib/rc/st24.c +++ b/src/lib/rc/st24.c @@ -72,7 +72,7 @@ const char *decode_states[] = {"UNSYNCED", #define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f)) static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED; -static unsigned _rxlen; +static uint8_t _rxlen; static ReceiverFcPacket _rxpacket; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 6068ab082..a9205b6fd 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -134,7 +134,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 7200, attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -275,7 +275,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params = { 0 }; + struct attitude_estimator_ekf_params ekf_params; + memset(&ekf_params, 0, sizeof(ekf_params)); struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 749b0a91c..c4bf34229 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -42,3 +42,8 @@ SRCS = attitude_estimator_ekf_main.cpp \ codegen/AttitudeEKF.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-float-equal -Wno-error + +EXTRACXXFLAGS = -Wno-error + diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e49027e47..9414225ca 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -153,7 +153,7 @@ int attitude_estimator_so3_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 5, 14000, attitude_estimator_so3_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/attitude_estimator_so3/module.mk b/src/modules/attitude_estimator_so3/module.mk index f52715abb..7b2e206cc 100644 --- a/src/modules/attitude_estimator_so3/module.mk +++ b/src/modules/attitude_estimator_so3/module.mk @@ -8,3 +8,5 @@ SRCS = attitude_estimator_so3_main.cpp \ attitude_estimator_so3_params.c MODULE_STACKSIZE = 1200 + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp index e0bcbc6e9..4580b338d 100644 --- a/src/modules/bottle_drop/bottle_drop.cpp +++ b/src/modules/bottle_drop/bottle_drop.cpp @@ -523,6 +523,9 @@ BottleDrop::task_main() } switch (_drop_state) { + case DROP_STATE_INIT: + // do nothing + break; case DROP_STATE_TARGET_VALID: { @@ -689,6 +692,10 @@ BottleDrop::task_main() orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission); } break; + + case DROP_STATE_BAY_CLOSED: + // do nothing + break; } counter++; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 744323c01..325ab2a67 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -267,7 +267,7 @@ int commander_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 40, 3200, commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); while (!thread_running) { usleep(200); diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index 27ca5c182..caec1e869 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -51,3 +51,5 @@ SRCS = commander.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 36d854ddd..07e70ddb3 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,4 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ +EXTRACXXFLAGS = -Weffc++ -Wno-error diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index f4ea05088..71b387b1e 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -115,7 +115,7 @@ int fixedwing_backside_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, control_demo_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/fw_pos_control_l1/module.mk b/src/modules/fw_pos_control_l1/module.mk index 440bab2c5..98e5c0a1e 100644 --- a/src/modules/fw_pos_control_l1/module.mk +++ b/src/modules/fw_pos_control_l1/module.mk @@ -47,3 +47,5 @@ SRCS = fw_pos_control_l1_main.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-float-equal diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index f17497aa8..4ba595a87 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -801,7 +801,7 @@ MavlinkFTP::_return_request(Request *req) /// @brief Copy file (with limited space) int -MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, size_t length) { char buff[512]; int src_fd = -1, dst_fd = -1; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bef6775a9..9693a92a9 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -142,7 +142,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); - int _copy_file(const char *src_path, const char *dst_path, ssize_t length); + int _copy_file(const char *src_path, const char *dst_path, size_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b7..08a9ac76b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1638,7 +1638,7 @@ Mavlink::start(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT, 2800, (main_t)&Mavlink::start_helper, - (const char **)argv); + (char * const *)argv); // Ensure that this shell command // does not return before the instance diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 378e3427d..2203ea1e7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1764,6 +1764,9 @@ protected: case RC_INPUT_SOURCE_PX4IO_ST24: msg.rssi |= (3 << 4); break; + case RC_INPUT_SOURCE_UNKNOWN: + // do nothing + break; } if (rc.rc_lost) { diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c44d4c35e..0d7d6b9ef 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -62,3 +62,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-sign-compare diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 0658d3f09..5ebdb721c 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -41,3 +41,5 @@ SRCS = position_estimator_inav_main.c \ inertial_filter.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wno-error diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 0af01cba1..43829ede5 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -151,7 +151,7 @@ int position_estimator_inav_main(int argc, char *argv[]) position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL); + (argv) ? (char * const *) &argv[2] : (char * const *) NULL); exit(0); } diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d4a00af39..d7224b77a 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -45,3 +45,5 @@ SRCS = sdlog2.c \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 6df677338..0a8564da6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -304,7 +304,7 @@ int sdlog2_main(int argc, char *argv[]) SCHED_PRIORITY_DEFAULT - 30, 3000, sdlog2_thread_main, - (const char **)argv); + (char * const *)argv); exit(0); } @@ -1089,6 +1089,8 @@ int sdlog2_thread_main(int argc, char *argv[]) if (_extended_logging) { subs.sat_info_sub = orb_subscribe(ORB_ID(satellite_info)); + } else { + subs.sat_info_sub = 0; } /* close non-needed fd's */ diff --git a/src/modules/segway/segway_main.cpp b/src/modules/segway/segway_main.cpp index 061fbf9b9..ee492f85a 100644 --- a/src/modules/segway/segway_main.cpp +++ b/src/modules/segway/segway_main.cpp @@ -110,7 +110,7 @@ int segway_main(int argc, char *argv[]) SCHED_PRIORITY_MAX - 10, 5120, segway_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk index dfbba92d9..f37bc9327 100644 --- a/src/modules/sensors/module.mk +++ b/src/modules/sensors/module.mk @@ -44,3 +44,5 @@ SRCS = sensors.cpp \ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-type-limits diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index fe8b7e75a..f4dff2838 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -57,3 +57,5 @@ SRCS = err.c \ mcu_version.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-sign-compare diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c index 90d8dd77c..82183b0d7 100644 --- a/src/modules/systemlib/systemlib.c +++ b/src/modules/systemlib/systemlib.c @@ -87,7 +87,7 @@ static void kill_task(FAR struct tcb_s *tcb, FAR void *arg) kill(tcb->pid, SIGUSR1); } -int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[]) +int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, char * const argv[]) { int pid; diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 6e22a557e..2f24215a9 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -64,7 +64,7 @@ __EXPORT int task_spawn_cmd(const char *name, int scheduler, int stack_size, main_t entry, - const char *argv[]); + char * const argv[]); enum MULT_PORTS { MULT_0_US2_RXTX = 0, diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 0d5155e90..02403ef76 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -39,3 +39,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index cdbff75f0..40ffb8f12 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -41,3 +41,5 @@ SRCS = mixer.cpp MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os + +EXTRACXXFLAGS = -Wno-error diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index 1bc4f414e..c3baffc5d 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -6,3 +6,5 @@ MODULE_COMMAND = mtd SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os + +EXTRACFLAGS = -Wno-error \ No newline at end of file diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index e110335e7..80ee204e8 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -212,7 +212,7 @@ static void do_show(const char* search_string) { printf(" + = saved, * = unsaved\n"); - param_foreach(do_show_print, search_string, false); + param_foreach(do_show_print, (char *)search_string, false); exit(0); } diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 622a0faf3..1ea2a6620 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -34,3 +34,5 @@ SRCS = test_adc.c \ test_conv.cpp \ test_mount.c \ test_mtd.c + +EXTRACXXFLAGS = -Wno-error -- cgit v1.2.3 From d14fdf896b0081403908599a3164d09941d363c9 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 10:56:32 -0500 Subject: rotate_3f() implement missing ROTATION_ROLL_270_YAW_270 --- src/examples/fixedwing_control/module.mk | 2 +- src/lib/conversion/rotation.cpp | 4 ++-- src/modules/attitude_estimator_ekf/module.mk | 5 ++--- 3 files changed, 5 insertions(+), 6 deletions(-) (limited to 'src') diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 3fab9c8d4..8707776ae 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wno-frame-larger-than diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp index 3d3e13355..1fe36d395 100644 --- a/src/lib/conversion/rotation.cpp +++ b/src/lib/conversion/rotation.cpp @@ -182,8 +182,8 @@ rotate_3f(enum Rotation rot, float &x, float &y, float &z) return; } case ROTATION_ROLL_270_YAW_270: { - // unimplemented - ASSERT(0 == 1); + tmp = z; z = -y; y = tmp; + tmp = x; x = y; y = -tmp; return; } case ROTATION_PITCH_90: { diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index c4bf34229..96ba45242 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,7 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal -Wno-error - -EXTRACXXFLAGS = -Wno-error +EXTRACFLAGS = -Wno-float-equal +EXTRACXXFLAGS = -Wno-frame-larger-than -- cgit v1.2.3 From 5b600a815c13af56e879029196a24e544c110b97 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 23 Dec 2014 11:15:45 -0500 Subject: Replace use of -Wno-error and only ignore specific warnings --- src/examples/fixedwing_control/module.mk | 2 +- src/modules/attitude_estimator_ekf/module.mk | 4 ++-- src/modules/commander/module.mk | 3 ++- src/modules/ekf_att_pos_estimator/module.mk | 3 ++- src/modules/position_estimator_inav/module.mk | 3 ++- src/modules/sdlog2/module.mk | 3 ++- src/modules/vtol_att_control/module.mk | 3 ++- src/systemcmds/mixer/module.mk | 3 ++- src/systemcmds/mtd/module.mk | 3 ++- src/systemcmds/tests/module.mk | 3 ++- 10 files changed, 19 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/examples/fixedwing_control/module.mk b/src/examples/fixedwing_control/module.mk index 8707776ae..f6c882ead 100644 --- a/src/examples/fixedwing_control/module.mk +++ b/src/examples/fixedwing_control/module.mk @@ -42,4 +42,4 @@ SRCS = main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-frame-larger-than +EXTRACFLAGS = -Wframe-larger-than=1200 diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 96ba45242..3a3e1cc69 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -43,6 +43,6 @@ SRCS = attitude_estimator_ekf_main.cpp \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-float-equal +EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 -EXTRACXXFLAGS = -Wno-frame-larger-than +EXTRACXXFLAGS = -Wframe-larger-than=2200 diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index caec1e869..dac2ce59a 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -52,4 +52,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=1900 + diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 07e70ddb3..4311fb3f9 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,4 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wno-error +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000 + diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 5ebdb721c..45c876299 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -42,4 +42,5 @@ SRCS = position_estimator_inav_main.c \ MODULE_STACKSIZE = 1200 -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=3500 + diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk index d7224b77a..f1118000e 100644 --- a/src/modules/sdlog2/module.mk +++ b/src/modules/sdlog2/module.mk @@ -46,4 +46,5 @@ MODULE_STACKSIZE = 1200 MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error +EXTRACFLAGS = -Wframe-larger-than=1200 + diff --git a/src/modules/vtol_att_control/module.mk b/src/modules/vtol_att_control/module.mk index 02403ef76..0cf3072c8 100644 --- a/src/modules/vtol_att_control/module.mk +++ b/src/modules/vtol_att_control/module.mk @@ -40,4 +40,5 @@ MODULE_COMMAND = vtol_att_control SRCS = vtol_att_control_main.cpp \ vtol_att_control_params.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wno-write-strings + diff --git a/src/systemcmds/mixer/module.mk b/src/systemcmds/mixer/module.mk index 40ffb8f12..0fb899c67 100644 --- a/src/systemcmds/mixer/module.mk +++ b/src/systemcmds/mixer/module.mk @@ -42,4 +42,5 @@ MODULE_STACKSIZE = 4096 MAXOPTIMIZATION = -Os -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2048 + diff --git a/src/systemcmds/mtd/module.mk b/src/systemcmds/mtd/module.mk index c3baffc5d..bca1cdcc1 100644 --- a/src/systemcmds/mtd/module.mk +++ b/src/systemcmds/mtd/module.mk @@ -7,4 +7,5 @@ SRCS = mtd.c 24xxxx_mtd.c MAXOPTIMIZATION = -Os -EXTRACFLAGS = -Wno-error \ No newline at end of file +EXTRACFLAGS = -Wno-error + diff --git a/src/systemcmds/tests/module.mk b/src/systemcmds/tests/module.mk index 1ea2a6620..6eed3922c 100644 --- a/src/systemcmds/tests/module.mk +++ b/src/systemcmds/tests/module.mk @@ -35,4 +35,5 @@ SRCS = test_adc.c \ test_mount.c \ test_mtd.c -EXTRACXXFLAGS = -Wno-error +EXTRACXXFLAGS = -Wframe-larger-than=2500 + -- cgit v1.2.3 From 9292c8f405b0ed208443df0b1f9ebd497bb518ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 20 Dec 2014 10:27:02 -0700 Subject: add interrupt latency printout command and mean/variance to interval performance counter --- src/drivers/stm32/drv_hrt.c | 8 ++++-- src/modules/systemlib/perf_counter.c | 53 ++++++++++++++++++++++++++++-------- src/modules/systemlib/perf_counter.h | 9 +++++- src/systemcmds/perf/perf.c | 6 +++- 4 files changed, 60 insertions(+), 16 deletions(-) (limited to 'src') diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index 281f918d7..603c2eb9d 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -253,9 +253,11 @@ static uint16_t latency_baseline; static uint16_t latency_actual; /* latency histogram */ -#define LATENCY_BUCKET_COUNT 8 -static const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; -static uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; +#define LATENCY_BUCKET_COUNT 8 +__EXPORT const uint16_t latency_bucket_count = LATENCY_BUCKET_COUNT; +__EXPORT const uint16_t latency_buckets[LATENCY_BUCKET_COUNT] = { 1, 2, 5, 10, 20, 50, 100, 1000 }; +__EXPORT uint32_t latency_counters[LATENCY_BUCKET_COUNT + 1]; + /* timer-specific functions */ static void hrt_tim_init(void); diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c index d6d8284d2..f9e90652d 100644 --- a/src/modules/systemlib/perf_counter.c +++ b/src/modules/systemlib/perf_counter.c @@ -41,7 +41,7 @@ #include #include #include - +#include #include "perf_counter.h" /** @@ -84,7 +84,8 @@ struct perf_ctr_interval { uint64_t time_last; uint64_t time_least; uint64_t time_most; - + float mean; + float M2; }; /** @@ -109,6 +110,7 @@ perf_alloc(enum perf_counter_type type, const char *name) case PC_INTERVAL: ctr = (perf_counter_t)calloc(sizeof(struct perf_ctr_interval), 1); + break; default: @@ -156,15 +158,23 @@ perf_count(perf_counter_t handle) case 1: pci->time_least = now - pci->time_last; pci->time_most = now - pci->time_last; + pci->mean = pci->time_least / 1e6f; + pci->M2 = 0; break; default: { - hrt_abstime interval = now - pci->time_last; - if (interval < pci->time_least) - pci->time_least = interval; - if (interval > pci->time_most) - pci->time_most = interval; - break; - } + hrt_abstime interval = now - pci->time_last; + if (interval < pci->time_least) + pci->time_least = interval; + if (interval > pci->time_most) + pci->time_most = interval; + // maintain mean and variance of interval in seconds + // Knuth/Welford recursive mean and variance of update intervals (via Wikipedia) + float dt = interval / 1e6f; + float delta_intvl = dt - pci->mean; + pci->mean += delta_intvl / pci->event_count; + pci->M2 += delta_intvl * (dt - pci->mean); + break; + } } pci->time_last = now; pci->event_count++; @@ -313,13 +323,16 @@ perf_print_counter_fd(int fd, perf_counter_t handle) case PC_INTERVAL: { struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle; + float rms = sqrtf(pci->M2 / (pci->event_count-1)); - dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus\n", + dprintf(fd, "%s: %llu events, %lluus avg, min %lluus max %lluus %5.3f msec mean %5.3f msec rms\n", handle->name, pci->event_count, (pci->time_last - pci->time_first) / pci->event_count, pci->time_least, - pci->time_most); + pci->time_most, + (double)(1000 * pci->mean), + (double)(1000 * rms)); break; } @@ -365,6 +378,21 @@ perf_print_all(int fd) } } +extern const uint16_t latency_bucket_count; +extern uint32_t latency_counters[]; +extern const uint16_t latency_buckets[]; + +void +perf_print_latency(int fd) +{ + dprintf(fd, "bucket : events\n"); + for (int i = 0; i < latency_bucket_count; i++) { + printf(" %4i : %i\n", latency_buckets[i], latency_counters[i]); + } + // print the overflow bucket value + dprintf(fd, " >%4i : %i\n", latency_buckets[latency_bucket_count-1], latency_counters[latency_bucket_count]); +} + void perf_reset_all(void) { @@ -374,4 +402,7 @@ perf_reset_all(void) perf_reset(handle); handle = (perf_counter_t)sq_next(&handle->link); } + for (int i = 0; i <= latency_bucket_count; i++) { + latency_counters[i] = 0; + } } diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h index 668d9dfdf..d06606a5d 100644 --- a/src/modules/systemlib/perf_counter.h +++ b/src/modules/systemlib/perf_counter.h @@ -94,7 +94,7 @@ __EXPORT extern void perf_begin(perf_counter_t handle); * End a performance event. * * This call applies to counters that operate over ranges of time; PC_ELAPSED etc. - * If a call is made without a corresopnding perf_begin call, or if perf_cancel + * If a call is made without a corresponding perf_begin call, or if perf_cancel * has been called subsequently, no change is made to the counter. * * @param handle The handle returned from perf_alloc. @@ -142,6 +142,13 @@ __EXPORT extern void perf_print_counter_fd(int fd, perf_counter_t handle); */ __EXPORT extern void perf_print_all(int fd); +/** + * Print hrt latency counters. + * + * @param fd File descriptor to print to - e.g. 0 for stdout + */ +__EXPORT extern void perf_print_latency(int fd); + /** * Reset all of the performance counters. */ diff --git a/src/systemcmds/perf/perf.c b/src/systemcmds/perf/perf.c index b08a2e3b7..a788dfc11 100644 --- a/src/systemcmds/perf/perf.c +++ b/src/systemcmds/perf/perf.c @@ -68,8 +68,12 @@ int perf_main(int argc, char *argv[]) if (strcmp(argv[1], "reset") == 0) { perf_reset_all(); return 0; + } else if (strcmp(argv[1], "latency") == 0) { + perf_print_latency(0 /* stdout */); + fflush(stdout); + return 0; } - printf("Usage: perf \n"); + printf("Usage: perf [reset | latency]\n"); return -1; } -- cgit v1.2.3 From 5c51adf5f79266de2b483c2461babd4d673cfffb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 14:49:18 +0100 Subject: bl_update: Improve bootloader error reporting --- src/systemcmds/bl_update/bl_update.c | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/systemcmds/bl_update/bl_update.c b/src/systemcmds/bl_update/bl_update.c index 0569d89f5..ec9269d39 100644 --- a/src/systemcmds/bl_update/bl_update.c +++ b/src/systemcmds/bl_update/bl_update.c @@ -52,6 +52,8 @@ #include "systemlib/systemlib.h" #include "systemlib/err.h" +#define BL_FILE_SIZE_LIMIT 16384 + __EXPORT int bl_update_main(int argc, char *argv[]); static void setopt(void); @@ -72,12 +74,12 @@ bl_update_main(int argc, char *argv[]) struct stat s; - if (stat(argv[1], &s) < 0) + if (!stat(argv[1], &s)) err(1, "stat %s", argv[1]); /* sanity-check file size */ - if (s.st_size > 16384) - errx(1, "%s: file too large", argv[1]); + if (s.st_size > BL_FILE_SIZE_LIMIT) + errx(1, "%s: file too large (limit: %u, actual: %d)", argv[1], BL_FILE_SIZE_LIMIT, s.st_size); uint8_t *buf = malloc(s.st_size); -- cgit v1.2.3 From ed493918f70560067e213847b5546166398d494e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:36:10 +0100 Subject: Attitude estimator EKF: fix warnings --- src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ++-- src/modules/attitude_estimator_ekf/module.mk | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) (limited to 'src') diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index 1942fdc62..702894d60 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -462,11 +462,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds if (vision.timestamp_boot > 0 && (hrt_elapsed_time(&vision.timestamp_boot) < 500000)) { math::Quaternion q(vision.q); - math::Matrix<3, 3> R = q.to_dcm(); + math::Matrix<3, 3> Rvis = q.to_dcm(); math::Vector<3> v(1.0f, 0.0f, 0.4f); - math::Vector<3> vn = R * v; + math::Vector<3> vn = Rvis * v; z_k[6] = vn(0); z_k[7] = vn(1); diff --git a/src/modules/attitude_estimator_ekf/module.mk b/src/modules/attitude_estimator_ekf/module.mk index 3a3e1cc69..1158536e1 100644 --- a/src/modules/attitude_estimator_ekf/module.mk +++ b/src/modules/attitude_estimator_ekf/module.mk @@ -45,4 +45,4 @@ MODULE_STACKSIZE = 1200 EXTRACFLAGS = -Wno-float-equal -Wframe-larger-than=3600 -EXTRACXXFLAGS = -Wframe-larger-than=2200 +EXTRACXXFLAGS = -Wframe-larger-than=2400 -- cgit v1.2.3 From 2f6767480b5c88c16ec181b47d194bd9dabdc959 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 17:36:34 +0100 Subject: EKF att/pos estimator: Fix warnings (cross-checked stack size already) --- src/modules/ekf_att_pos_estimator/module.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk index 4311fb3f9..843dde978 100644 --- a/src/modules/ekf_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -42,5 +42,5 @@ SRCS = ekf_att_pos_estimator_main.cpp \ estimator_23states.cpp \ estimator_utilities.cpp -EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=3000 +EXTRACXXFLAGS = -Weffc++ -Wframe-larger-than=6100 -- cgit v1.2.3 From b097b13ba687412b97d2bcd005bc0f11be33de7b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 18:01:52 +0100 Subject: Companion computer interface: Add SYS_COMPANION parameter. Setting it to 921600 enables the companion interface. --- ROMFS/px4fmu_common/init.d/rcS | 5 ++++- src/modules/systemlib/system_params.c | 14 ++++++++++++++ 2 files changed, 18 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index b0e0c2674..26b729aad 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -434,7 +434,10 @@ then # if ver hwcmp PX4FMU_V2 then - mavlink start -d /dev/ttyS2 -b 230400 -m onboard + if param compare SYS_COMPANION 921600 + then + mavlink start -d /dev/ttyS2 -b 921600 -m onboard + fi fi # UAVCAN diff --git a/src/modules/systemlib/system_params.c b/src/modules/systemlib/system_params.c index 702e435ac..a0988035c 100644 --- a/src/modules/systemlib/system_params.c +++ b/src/modules/systemlib/system_params.c @@ -82,3 +82,17 @@ PARAM_DEFINE_INT32(SYS_USE_IO, 1); * @group System */ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2); + +/** +* Companion computer interface +* +* Configures the baud rate of the companion computer interface. +* Set to zero to disable, set to 921600 to enable. +* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for +* other baud rates. +* +* @min 0 +* @max 921600 +* @group System +*/ +PARAM_DEFINE_INT32(SYS_COMPANION, 0); -- cgit v1.2.3 From 3df44a27ec72bf29b7bff6d043384c164d743b0e Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Fri, 26 Dec 2014 22:55:27 +0530 Subject: Add streams to onboard mode --- src/modules/mavlink/mavlink_main.cpp | 4 ++++ 1 file changed, 4 insertions(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 29b7ec7b7..5d22dab6a 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1412,9 +1412,13 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("SYS_STATUS", 1.0f); configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); + configure_stream("LOCAL_POSITION_NED", 30.0f); configure_stream("CAMERA_CAPTURE", 2.0f); configure_stream("ATTITUDE_TARGET", 10.0f); configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("POSITION_TARGET_LOCAL_NED", 10.0f); + configure_stream("DISTANCE_SENSOR", 10.0f); + configure_stream("OPTICAL_FLOW_RAD", 10.0f); configure_stream("VFR_HUD", 10.0f); break; -- cgit v1.2.3 From da977bb39bf6157247cfd31a3da0f0f80b497bf0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 10 Oct 2014 00:04:39 +0200 Subject: Added HMC chip select support --- src/drivers/boards/px4fmu-v2/board_config.h | 2 ++ src/drivers/boards/px4fmu-v2/px4fmu_spi.c | 15 +++++++++++++++ 2 files changed, 17 insertions(+) (limited to 'src') diff --git a/src/drivers/boards/px4fmu-v2/board_config.h b/src/drivers/boards/px4fmu-v2/board_config.h index ee365e47c..2fd8bc724 100644 --- a/src/drivers/boards/px4fmu-v2/board_config.h +++ b/src/drivers/boards/px4fmu-v2/board_config.h @@ -105,6 +105,7 @@ __BEGIN_DECLS #define GPIO_SPI_CS_ACCEL_MAG (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN15) #define GPIO_SPI_CS_BARO (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN7) #define GPIO_SPI_CS_FRAM (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTD|GPIO_PIN10) +#define GPIO_SPI_CS_HMC (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN1) #define GPIO_SPI_CS_MPU (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_2MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN2) #define GPIO_SPI_CS_EXT0 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTE|GPIO_PIN4) #define GPIO_SPI_CS_EXT1 (GPIO_OUTPUT|GPIO_PUSHPULL|GPIO_SPEED_50MHz|GPIO_OUTPUT_SET|GPIO_PORTC|GPIO_PIN14) @@ -119,6 +120,7 @@ __BEGIN_DECLS #define PX4_SPIDEV_ACCEL_MAG 2 #define PX4_SPIDEV_BARO 3 #define PX4_SPIDEV_MPU 4 +#define PX4_SPIDEV_HMC 5 /* External bus */ #define PX4_SPIDEV_EXT0 1 diff --git a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c index 8c37d31a7..27f193513 100644 --- a/src/drivers/boards/px4fmu-v2/px4fmu_spi.c +++ b/src/drivers/boards/px4fmu-v2/px4fmu_spi.c @@ -73,6 +73,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_configgpio(GPIO_SPI_CS_GYRO); stm32_configgpio(GPIO_SPI_CS_ACCEL_MAG); stm32_configgpio(GPIO_SPI_CS_BARO); + stm32_configgpio(GPIO_SPI_CS_HMC); stm32_configgpio(GPIO_SPI_CS_MPU); /* De-activate all peripherals, @@ -82,6 +83,7 @@ __EXPORT void weak_function stm32_spiinitialize(void) stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); stm32_configgpio(GPIO_EXTI_GYRO_DRDY); @@ -117,6 +119,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, !selected); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -125,6 +128,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, !selected); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -133,6 +137,16 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, !selected); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); + stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); + break; + + case PX4_SPIDEV_HMC: + /* Making sure the other peripherals are not selected */ + stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); + stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); + stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, !selected); stm32_gpiowrite(GPIO_SPI_CS_MPU, 1); break; @@ -141,6 +155,7 @@ __EXPORT void stm32_spi1select(FAR struct spi_dev_s *dev, enum spi_dev_e devid, stm32_gpiowrite(GPIO_SPI_CS_GYRO, 1); stm32_gpiowrite(GPIO_SPI_CS_ACCEL_MAG, 1); stm32_gpiowrite(GPIO_SPI_CS_BARO, 1); + stm32_gpiowrite(GPIO_SPI_CS_HMC, 1); stm32_gpiowrite(GPIO_SPI_CS_MPU, !selected); break; -- cgit v1.2.3 From 0e84f60826a5f640233a3e265da2079f89e422c0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 11 Oct 2014 00:25:10 +0200 Subject: Add macro for multi topic support --- src/modules/uORB/uORB.h | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) (limited to 'src') diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 82ff46ad2..beb23f61d 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -67,6 +67,33 @@ typedef const struct orb_metadata *orb_id_t; */ #define ORB_ID(_name) &__orb_##_name +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + * @param _count The class ID of the topic + */ +#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) + +/** + * Generates a pointer to the uORB metadata structure for + * a given topic. + * + * The topic must have been declared previously in scope + * with ORB_DECLARE(). + * + * @param _name The name of the topic. + * @param _count The class ID of the topic + */ +#define ORB_ID_TRIPLE(_name, _count) \ + ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ + ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ + (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) + /** * Declare (prototype) the uORB metadata for a topic. * -- cgit v1.2.3 From e952324fbf08405fe555be7169ef0a53a12487f8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 26 Dec 2014 20:25:31 +0100 Subject: Rework HoTT into a proper library, which fixes parallel build breakage --- makefiles/config_px4fmu-v2_default.mk | 1 + src/drivers/hott/comms.h | 2 +- src/drivers/hott/hott_sensors/module.mk | 4 +-- src/drivers/hott/hott_telemetry/module.mk | 4 +-- src/drivers/hott/messages.h | 18 +++++++------- src/drivers/hott/module.mk | 41 +++++++++++++++++++++++++++++++ 6 files changed, 54 insertions(+), 16 deletions(-) create mode 100644 src/drivers/hott/module.mk (limited to 'src') diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 335055cb1..fa5dff93c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -32,6 +32,7 @@ MODULES += drivers/ll40ls # MODULES += drivers/trone MODULES += drivers/gps MODULES += drivers/hil +MODULES += drivers/hott MODULES += drivers/hott/hott_telemetry MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index f5608122f..0a586a8fd 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -41,6 +41,6 @@ #ifndef COMMS_H_ #define COMMS_H -int open_uart(const char *device); +__EXPORT int open_uart(const char *device); #endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index 47aea6caf..25a6f0ac0 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_sensors -SRCS = hott_sensors.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_sensors.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index cd7bdbc85..9de47b356 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -37,8 +37,6 @@ MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry.cpp \ - ../messages.cpp \ - ../comms.cpp +SRCS = hott_telemetry.cpp MAXOPTIMIZATION = -Os diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index 224f8fc56..a116a50dd 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -235,15 +235,15 @@ struct gps_module_msg { // The maximum size of a message. #define MAX_MESSAGE_BUFFER_SIZE 45 -void init_sub_messages(void); -void init_pub_messages(void); -void build_gam_request(uint8_t *buffer, size_t *size); -void publish_gam_message(const uint8_t *buffer); -void build_eam_response(uint8_t *buffer, size_t *size); -void build_gam_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); +__EXPORT void init_sub_messages(void); +__EXPORT void init_pub_messages(void); +__EXPORT void build_gam_request(uint8_t *buffer, size_t *size); +__EXPORT void publish_gam_message(const uint8_t *buffer); +__EXPORT void build_eam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gam_response(uint8_t *buffer, size_t *size); +__EXPORT void build_gps_response(uint8_t *buffer, size_t *size); +__EXPORT float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +__EXPORT void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); #endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott/module.mk b/src/drivers/hott/module.mk new file mode 100644 index 000000000..31a66d491 --- /dev/null +++ b/src/drivers/hott/module.mk @@ -0,0 +1,41 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# +# 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. +# +############################################################################ + +# +# Graupner HoTT Sensors messages. +# + +SRCS = messages.cpp \ + comms.cpp + +MAXOPTIMIZATION = -Os -- cgit v1.2.3 From 779f8f00554319facd5c2572c39dd3be4b9c5afd Mon Sep 17 00:00:00 2001 From: Stephan Brown Date: Fri, 26 Dec 2014 23:32:38 +0000 Subject: mavlink: Removed warnx that references a deleted pointer and causes a hardfault. --- src/modules/mavlink/mavlink_main.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 087ab3c99..f9a3681b3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -948,7 +948,6 @@ Mavlink::configure_stream(const char *stream_name, const float rate) /* delete stream */ LL_DELETE(_streams, stream); delete stream; - warnx("deleted stream %s", stream->get_name()); } return OK; -- cgit v1.2.3 From c29972424f6d7b99633c8497f0c25ab7cda4d2ca Mon Sep 17 00:00:00 2001 From: "M.H.Kabir" Date: Sat, 27 Dec 2014 17:18:57 +0530 Subject: fix timestamp --- src/modules/mavlink/mavlink_messages.cpp | 1 + 1 file changed, 1 insertion(+) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 2203ea1e7..0f25c969d 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1486,6 +1486,7 @@ protected: if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { mavlink_position_target_global_int_t msg{}; + msg.time_boot_ms = hrt_absolute_time()/1000; msg.coordinate_frame = MAV_FRAME_GLOBAL; msg.lat_int = pos_sp_triplet.current.lat * 1e7; msg.lon_int = pos_sp_triplet.current.lon * 1e7; -- cgit v1.2.3