dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Updated C files for attitude estimator

This commit is contained in:
Lorenz Meier 2012-09-21 14:42:57 +02:00
parent eaa431e5ce
commit 8a11f76994
31 changed files with 52 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
View File

@ -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;
}

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_initialize.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_terminate.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/attitudeKalmanfilter_types.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/cross.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/diag.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/diag.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/eye.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/eye.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/find.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/find.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/mrdivide.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/norm.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetInf.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtGetNaN.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_defines.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.c
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rt_nonfinite.h
View File

@ -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
*
*/

2
apps/attitude_estimator_ekf/codegen/rtwtypes.h
View File

@ -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
*
*/

6
apps/commander/commander.c
View File

@ -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, &current_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, &current_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, &current_status, mavlink_fd);
}

12
apps/mavlink/mavlink.c
View File

@ -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 */

4
apps/sdlog/sdlog.c
View File

@ -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;