Browse Source

INAV: use int for outputs

sbg
Lorenz Meier 10 years ago
parent
commit
a36088b9c2
  1. 4
      src/modules/mc_pos_control/mc_pos_control_main.cpp
  2. 11
      src/modules/position_estimator_inav/position_estimator_inav_main.c

4
src/modules/mc_pos_control/mc_pos_control_main.cpp

@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0); - _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1) _pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1); - _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1)); mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
} }
} }
@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
if (_reset_alt_sp) { if (_reset_alt_sp) {
_reset_alt_sp = false; _reset_alt_sp = false;
_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2); _pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2)); mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
} }
} }

11
src/modules/position_estimator_inav/position_estimator_inav_main.c

@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else { } else {
wait_baro = false; wait_baro = false;
baro_offset /= (float) baro_init_cnt; baro_offset /= (float) baro_init_cnt;
warnx("baro offs: %.2f", (double)baro_offset); warnx("baro offs: %d", (int)baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset); mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
local_pos.z_valid = true; local_pos.z_valid = true;
local_pos.v_z_valid = true; local_pos.v_z_valid = true;
} }
@ -520,7 +520,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
sonar_valid_time = t; sonar_valid_time = t;
sonar_valid = true; sonar_valid = true;
local_pos.surface_bottom_timestamp = t; local_pos.surface_bottom_timestamp = t;
mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset); mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
} }
} else { } else {
@ -721,8 +721,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* initialize projection */ /* initialize projection */
map_projection_init(&ref, lat, lon); map_projection_init(&ref, lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt); // XXX replace this print
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt); warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
} }
} }

Loading…
Cancel
Save