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

Merge pull request #3278 from erikd/driver_framework 

Driver framework
This commit is contained in:
Lorenz Meier 2015-11-27 08:15:51 +01:00
parent 1c6a7fb360 67d61488da
commit 2341d56927
3 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -1183,8 +1183,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
tstatus.heartbeat_time = tstatus.timestamp; tstatus.heartbeat_time = tstatus.timestamp;
if (_telemetry_status_pub == nullptr) { if (_telemetry_status_pub == nullptr) {
int multi_instance; _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH);
_telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH);
} else { } else {
orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus);
@ -1837,8 +1836,10 @@ MavlinkReceiver::receive_thread(void *arg)
} }
} }
/* count received bytes */ /* count received bytes (nread will be -1 on read error) */
_mavlink->count_rxbytes(nread); if (nread > 0) {
_mavlink->count_rxbytes(nread);
}
} }
} }

15
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -216,7 +216,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
fclose(f); fclose(f);
} }
#else #else
#define write_debug_log(...) #define write_debug_log(...)
#endif #endif
/**************************************************************************** /****************************************************************************
@ -380,6 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_advert_t vehicle_global_position_pub = NULL; orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params; struct position_estimator_inav_params params;
memset(&params, 0, sizeof(params));
struct position_estimator_inav_param_handles pos_inav_param_handles; struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */ /* initialize parameter handles */
inav_parameters_init(&pos_inav_param_handles); inav_parameters_init(&pos_inav_param_handles);
@ -595,7 +596,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f; flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f;
//moving average //moving average
if (n_flow >= 100) { if (n_flow >= 100) {
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
@ -617,7 +618,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1); att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
n_flow++; n_flow++;
} }
/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/ /*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]); yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
@ -841,7 +842,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) { } else if (t > ref_init_start + ref_init_delay) {
ref_inited = true; ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */ /* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f; x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s; x_est[1] = gps.vel_n_m_s;
@ -981,7 +982,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */ /* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy); bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
@ -1147,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p, acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v); corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est)); memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision)); memset(corr_vision, 0, sizeof(corr_vision));
@ -1274,7 +1275,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
buf_ptr = 0; buf_ptr = 0;
} }
/* publish local position */ /* publish local position */
local_pos.xy_valid = can_estimate_xy; local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy;

1
src/modules/sensors/sensors.cpp
View File

@ -542,6 +542,7 @@ Sensors::Sensors() :
memset(&_rc, 0, sizeof(_rc)); memset(&_rc, 0, sizeof(_rc));
memset(&_diff_pres, 0, sizeof(_diff_pres)); memset(&_diff_pres, 0, sizeof(_diff_pres));
memset(&_parameters, 0, sizeof(_parameters));
memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
/* basic r/c parameters */ /* basic r/c parameters */