diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..37d070718d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1183,8 +1183,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); @@ -1837,8 +1836,10 @@ MavlinkReceiver::receive_thread(void *arg) } } - /* count received bytes */ - _mavlink->count_rxbytes(nread); + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + } } } 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 56db8b4c0a..449b9c059f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -216,7 +216,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e fclose(f); } #else -#define write_debug_log(...) +#define write_debug_log(...) #endif /**************************************************************************** @@ -380,6 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_advert_t vehicle_global_position_pub = NULL; struct position_estimator_inav_params params; + memset(¶ms, 0, sizeof(params)); struct position_estimator_inav_param_handles pos_inav_param_handles; /* initialize parameter 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[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; - + //moving average if (n_flow >= 100) { 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); n_flow++; } - + /*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]); @@ -841,7 +842,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else if (t > ref_init_start + ref_init_delay) { ref_inited = true; - + /* set position estimate to (0, 0, 0), use GPS velocity for XY */ x_est[0] = 0.0f; 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) */ 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 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]))) { 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, - 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)); memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_vision, 0, sizeof(corr_vision)); @@ -1274,7 +1275,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } - + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fbbdc80ba..827f1be916 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -542,6 +542,7 @@ Sensors::Sensors() : memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */