diff options
Diffstat (limited to 'apps')
31 files changed, 52 insertions, 41 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index d7f448861..d04556e12 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -70,7 +70,7 @@ __EXPORT int attitude_estimator_ekf_main(int argc, char *argv[]); // #define PROJECTION_INITIALIZE_COUNTER_LIMIT 5000 // #define REPROJECTION_COUNTER_LIMIT 125 -static unsigned int loop_interval_alarm = 4500; // loop interval in microseconds +static unsigned int loop_interval_alarm = 6500; // loop interval in microseconds static float dt = 1; /* 0, 0, -9.81, 1, 1, 1, wo (gyro offset), w */ @@ -205,6 +205,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(sub_raw, 5); + /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -222,11 +225,11 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) }; int ret = poll(fds, 1, 1000); - /* check for a timeout */ - if (ret == 0) { - /* */ - - /* update successful, copy data on every 2nd run and execute filter */ + if (ret < 0) { + /* XXX this is seriously bad - should be an emergency */ + } else if (ret == 0) { + /* XXX this means no sensor data - should be critical or emergency */ + printf("[attitude estimator ekf] WARNING: Not getting sensor data - sensor app running?\n"); } else { orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); @@ -256,7 +259,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) if (time_elapsed > loop_interval_alarm) { //TODO: add warning, cpu overload here if (overloadcounter == 20) { - printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR BLACK MAGIC (%lu > %lu)\n", time_elapsed, loop_interval_alarm); + printf("CPU OVERLOAD DETECTED IN ATTITUDE ESTIMATOR EKF (%lu > %lu)\n", time_elapsed, loop_interval_alarm); overloadcounter = 0; } diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c index 321e6874d..bfbb3be01 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:42 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h index 8207aa5c5..56f02b4c8 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c index abf1519a6..b72256a09 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_initialize'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h index 20186f183..efb465b25 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_initialize'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c index 458ddb9eb..d0bf625f0 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_terminate'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h index a939eee20..1ad84575f 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter_terminate'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h index 599a16884..bd83cc168 100755 --- a/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h +++ b/apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:42 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/cross.c b/apps/attitude_estimator_ekf/codegen/cross.c index 49f655ece..c88869453 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.c +++ b/apps/attitude_estimator_ekf/codegen/cross.c @@ -3,7 +3,7 @@ *
* Code generation for function 'cross'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/cross.h b/apps/attitude_estimator_ekf/codegen/cross.h index 844d8138f..92e3a884d 100755 --- a/apps/attitude_estimator_ekf/codegen/cross.h +++ b/apps/attitude_estimator_ekf/codegen/cross.h @@ -3,7 +3,7 @@ *
* Code generation for function 'cross'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/diag.c b/apps/attitude_estimator_ekf/codegen/diag.c index e54d52a38..522f6e285 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.c +++ b/apps/attitude_estimator_ekf/codegen/diag.c @@ -3,7 +3,7 @@ *
* Code generation for function 'diag'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/diag.h b/apps/attitude_estimator_ekf/codegen/diag.h index a4a2a4c82..bb92fb242 100755 --- a/apps/attitude_estimator_ekf/codegen/diag.h +++ b/apps/attitude_estimator_ekf/codegen/diag.h @@ -3,7 +3,7 @@ *
* Code generation for function 'diag'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/eye.c b/apps/attitude_estimator_ekf/codegen/eye.c index aece401c2..e67071dce 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.c +++ b/apps/attitude_estimator_ekf/codegen/eye.c @@ -3,7 +3,7 @@ *
* Code generation for function 'eye'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/eye.h b/apps/attitude_estimator_ekf/codegen/eye.h index e715ae1c3..c07a1bc97 100755 --- a/apps/attitude_estimator_ekf/codegen/eye.h +++ b/apps/attitude_estimator_ekf/codegen/eye.h @@ -3,7 +3,7 @@ *
* Code generation for function 'eye'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/find.c b/apps/attitude_estimator_ekf/codegen/find.c index 532ba4397..8f05ef146 100755 --- a/apps/attitude_estimator_ekf/codegen/find.c +++ b/apps/attitude_estimator_ekf/codegen/find.c @@ -3,7 +3,7 @@ *
* Code generation for function 'find'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/find.h b/apps/attitude_estimator_ekf/codegen/find.h index ceb90b6cf..ca525c600 100755 --- a/apps/attitude_estimator_ekf/codegen/find.h +++ b/apps/attitude_estimator_ekf/codegen/find.h @@ -3,7 +3,7 @@ *
* Code generation for function 'find'
*
- * C source code generated on: Mon Sep 17 20:13:22 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.c b/apps/attitude_estimator_ekf/codegen/mrdivide.c index 2fcaf8cb6..d098162d5 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.c +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.c @@ -3,7 +3,7 @@ *
* Code generation for function 'mrdivide'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/mrdivide.h b/apps/attitude_estimator_ekf/codegen/mrdivide.h index a58faaa26..e807afcc8 100755 --- a/apps/attitude_estimator_ekf/codegen/mrdivide.h +++ b/apps/attitude_estimator_ekf/codegen/mrdivide.h @@ -3,7 +3,7 @@ *
* Code generation for function 'mrdivide'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/norm.c b/apps/attitude_estimator_ekf/codegen/norm.c index 8d8e4e110..97509a0c0 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.c +++ b/apps/attitude_estimator_ekf/codegen/norm.c @@ -3,7 +3,7 @@ *
* Code generation for function 'norm'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:43 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/norm.h b/apps/attitude_estimator_ekf/codegen/norm.h index 479503ae3..63294717f 100755 --- a/apps/attitude_estimator_ekf/codegen/norm.h +++ b/apps/attitude_estimator_ekf/codegen/norm.h @@ -3,7 +3,7 @@ *
* Code generation for function 'norm'
*
- * C source code generated on: Mon Sep 17 20:13:23 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.c b/apps/attitude_estimator_ekf/codegen/rtGetInf.c index 42b3af501..f1bb71c87 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetInf.h b/apps/attitude_estimator_ekf/codegen/rtGetInf.h index 20d8c7f05..6d32d51b5 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetInf.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetInf.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c index d29aea34b..50581f34d 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.c +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:45 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h index a14b6170b..12d8734f5 100755 --- a/apps/attitude_estimator_ekf/codegen/rtGetNaN.h +++ b/apps/attitude_estimator_ekf/codegen/rtGetNaN.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_defines.h b/apps/attitude_estimator_ekf/codegen/rt_defines.h index 4cad63ada..419edf01c 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_defines.h +++ b/apps/attitude_estimator_ekf/codegen/rt_defines.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c index bd04f52e6..42ff21d39 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.c @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h index 67b3ba205..60128156e 100755 --- a/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h +++ b/apps/attitude_estimator_ekf/codegen/rt_nonfinite.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/attitude_estimator_ekf/codegen/rtwtypes.h b/apps/attitude_estimator_ekf/codegen/rtwtypes.h index 6feb2e1a9..2709915e9 100755 --- a/apps/attitude_estimator_ekf/codegen/rtwtypes.h +++ b/apps/attitude_estimator_ekf/codegen/rtwtypes.h @@ -3,7 +3,7 @@ *
* Code generation for function 'attitudeKalmanfilter'
*
- * C source code generated on: Mon Sep 17 20:13:24 2012
+ * C source code generated on: Fri Sep 21 13:56:44 2012
*
*/
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 011a7be97..578bcd875 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1365,12 +1365,18 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = false; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 7eca3031d..f9c06099e 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1077,8 +1077,8 @@ static void *uorb_receiveloop(void *arg) if (fds[ifds++].revents & POLLIN) { /* copy local position data into local buffer */ orb_copy(ORB_ID(manual_control_setpoint), subs->man_control_sp_sub, &buf.man_control); - mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll, - buf.man_control.pitch, buf.man_control.yaw, buf.man_control.throttle, 0); + mavlink_msg_manual_control_send(MAVLINK_COMM_0, mavlink_system.sysid, buf.man_control.roll*1000, + buf.man_control.pitch*1000, buf.man_control.yaw*1000, buf.man_control.throttle*1000, 0); } /* --- ACTUATOR ARMED --- */ @@ -1427,10 +1427,10 @@ void handleMessage(mavlink_message_t *msg) struct manual_control_setpoint_s mc; static orb_advert_t mc_pub = 0; - mc.roll = man.x; - mc.pitch = man.y; - mc.yaw = man.r; - mc.roll = man.z; + mc.roll = man.x*1000; + mc.pitch = man.y*1000; + mc.yaw = man.r*1000; + mc.roll = man.z*1000; /* fake RC channels with manual control input from simulator */ diff --git a/apps/sdlog/sdlog.c b/apps/sdlog/sdlog.c index b99c6652c..f1c1f9a23 100644 --- a/apps/sdlog/sdlog.c +++ b/apps/sdlog/sdlog.c @@ -289,6 +289,8 @@ int sdlog_thread_main(int argc, char *argv[]) { /* subscribe to ORB for sensors raw */ subs.sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); fds[fdsc_count].fd = subs.sensor_sub; + /* rate-limit raw data updates to 200Hz */ + orb_set_interval(subs.sensor_sub, 5); fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -459,7 +461,7 @@ int sdlog_thread_main(int argc, char *argv[]) { sysvector_bytes += write(sysvector_file, (const char*)&sysvector, sizeof(sysvector)); - usleep(4900); + usleep(10000); } unsigned bytes = sensor_combined_bytes + actuator_outputs_bytes + blackbox_file_bytes + actuator_controls_bytes; |