From 00a2a0370eedf84f68dcda5995f4e34495aaf887 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 12:43:36 +0200 Subject: accelerometer_calibration fix --- src/modules/commander/accelerometer_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 82df7c37d..7a7fa6f4e 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -298,7 +298,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* set accel error threshold to 5m/s^2 */ float accel_err_thr = 5.0f; /* still time required in us */ - int64_t still_time = 2000000; + hrt_abstime still_time = 2000000; struct pollfd fds[1]; fds[0].fd = sub_sensor_combined; fds[0].events = POLLIN; @@ -342,7 +342,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_timeout = t + timeout; } else { /* still since t_still */ - if ((int64_t) t - (int64_t) t_still > still_time) { + if (t > t_still + still_time) { /* vehicle is still, exit from the loop to detection of its orientation */ break; } -- cgit v1.2.3 From bf9282c9882882a5fc4adf680a6ecc5e655bb0e8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:10 +0200 Subject: position_estimator_inav: requre EPH < 5m to set GPS reference --- src/modules/position_estimator_inav/position_estimator_inav_main.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 af6a704a2..bc6e0b020 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -429,7 +429,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (fds[6].revents & POLLIN) { orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps); - if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) { + if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 5.0f) { /* initialize reference position if needed */ if (!ref_xy_inited) { if (ref_xy_init_start == 0) { -- cgit v1.2.3 From baa2cab69dd7ba4021bad294cb4e3c49d12a0f9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:52:55 +0200 Subject: commander: do AUTO_MISSION after takeoff --- src/modules/commander/commander.cpp | 54 ++++++++++++++++++------------------- 1 file changed, 26 insertions(+), 28 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 39d74e37a..d90008633 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1592,43 +1592,41 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v case MAIN_STATE_AUTO: if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { if (current_status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - /* check if takeoff completed */ - if (local_pos->z < -5.0f && !status.condition_landed) { - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } else { + /* don't switch to other states until takeoff not completed */ + if (local_pos->z > -5.0f || status.condition_landed) { res = TRANSITION_NOT_CHANGED; + break; } + } + /* check again, state can be changed */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); + // TRANSITION_DENIED is not possible here + break; } else { - if (current_status->condition_landed) { - /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); - // TRANSITION_DENIED is not possible here - break; + /* if not landed: */ + if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { + /* act depending on switches when manual control enabled */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); - } else { - /* if not landed: */ - if (current_status->rc_signal_found_once && !current_status->rc_signal_lost) { - /* act depending on switches when manual control enabled */ - if (current_status->return_switch == RETURN_SWITCH_RETURN) { - /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { - if (current_status->mission_switch == MISSION_SWITCH_MISSION) { - /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); - - } else { - /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); - } + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } - - } else { - /* always switch to loiter in air when no RC control */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } + + } else { + /* switch to mission in air when no RC control */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } } } else { -- cgit v1.2.3 From 7326f8a4215fe736aedd2e2cdb2ab51d06e20f80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 13:53:30 +0200 Subject: multirotor_pos_control: fixes, set local_position_sp.yaw --- .../multirotor_pos_control.c | 25 ++++++++++++++++------ 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c index e65792c76..385892f04 100644 --- a/src/modules/multirotor_pos_control/multirotor_pos_control.c +++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c @@ -405,30 +405,38 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } } - /* publish local position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + local_pos_sp.yaw = att_sp.yaw_body; /* local position setpoint is valid and can be used for loiter after position controlled mode */ local_pos_sp_valid = control_mode.flag_control_position_enabled; + reset_auto_pos = true; + /* force reprojection of global setpoint after manual mode */ global_pos_sp_reproject = true; } else { /* non-manual mode, use global setpoint */ - if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (control_mode.auto_state == NAVIGATION_STATE_AUTO_READY) { + reset_auto_pos = true; + } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_TAKEOFF) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = -takeoff_alt_default; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; + mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, local_pos_sp.z); } } else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_LOITER) { if (reset_auto_pos) { local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; att_sp.yaw_body = att.yaw; reset_auto_pos = false; } @@ -462,6 +470,8 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) } else { local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude; } + local_pos_sp.yaw = global_pos_sp.yaw; + att_sp.yaw_body = global_pos_sp.yaw; mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y); @@ -472,15 +482,13 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) local_pos_sp.x = local_pos.x; local_pos_sp.y = local_pos.y; local_pos_sp.z = local_pos.z; + local_pos_sp.yaw = att.yaw; + local_pos_sp_valid = true; } mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y); } - - /* publish local position setpoint as projection of global position setpoint */ - orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); } - att_sp.yaw_body = global_pos_sp.yaw; reset_auto_pos = true; } @@ -493,6 +501,9 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[]) reset_sp_z = true; } + /* publish local position setpoint */ + orb_publish(ORB_ID(vehicle_local_position_setpoint), local_pos_sp_pub, &local_pos_sp); + /* run position & altitude controllers, calculate velocity setpoint */ if (control_mode.flag_control_altitude_enabled) { global_vel_sp.vz = pid_calculate(&z_pos_pid, local_pos_sp.z, local_pos.z, local_pos.vz - sp_move_rate[2], dt) + sp_move_rate[2]; -- cgit v1.2.3 From e88d63ef272124e8c0ee9574506d14866feadb8b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Aug 2013 15:03:58 +0200 Subject: Increased USB buffer size to cope with fast transfers --- nuttx-configs/px4fmu-v1/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index b6f14490e..3e02c204e 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -564,8 +564,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512 CONFIG_CDCACM_NWRREQS=4 CONFIG_CDCACM_NRDREQS=4 CONFIG_CDCACM_BULKIN_REQLEN=96 -CONFIG_CDCACM_RXBUFSIZE=256 -CONFIG_CDCACM_TXBUFSIZE=256 +CONFIG_CDCACM_RXBUFSIZE=512 +CONFIG_CDCACM_TXBUFSIZE=512 CONFIG_CDCACM_VENDORID=0x26ac CONFIG_CDCACM_PRODUCTID=0x0010 CONFIG_CDCACM_VENDORSTR="3D Robotics" -- cgit v1.2.3 From b29d13347a10156bf1690b67938e2850d4e1e4c5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 26 Aug 2013 22:08:56 +0200 Subject: position_estimator_inav: reset reference altitude on arming. --- .../position_estimator_inav_main.c | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) 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 bc6e0b020..ef13ade88 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -75,8 +75,11 @@ static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s -static hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s + +static const hrt_abstime gps_timeout = 1000000; // GPS timeout = 1s +static const hrt_abstime flow_timeout = 1000000; // optical flow timeout = 1s +static const uint32_t updates_counter_len = 1000000; +static const uint32_t pub_interval = 4000; // limit publish rate to 250 Hz __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -172,6 +175,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float alt_avg = 0.0f; bool landed = true; hrt_abstime landed_time = 0; + bool flag_armed = false; uint32_t accel_counter = 0; uint32_t baro_counter = 0; @@ -275,10 +279,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) uint16_t flow_updates = 0; hrt_abstime updates_counter_start = hrt_absolute_time(); - uint32_t updates_counter_len = 1000000; - hrt_abstime pub_last = hrt_absolute_time(); - uint32_t pub_interval = 4000; // limit publish rate to 250 Hz /* acceleration in NED frame */ float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G }; @@ -407,7 +408,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } else { // new ground level baro_alt0 += sonar_corr; - warnx("new home: alt = %.3f", baro_alt0); mavlink_log_info(mavlink_fd, "[inav] new home: alt = %.3f", baro_alt0); local_pos.ref_alt = baro_alt0; local_pos.ref_timestamp = hrt_absolute_time(); @@ -486,6 +486,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; t_prev = t; + /* reset ground level on arm */ + if (armed.armed && !flag_armed) { + baro_alt0 -= z_est[0]; + z_est[0] = 0.0f; + local_pos.ref_alt = baro_alt0; + local_pos.ref_timestamp = hrt_absolute_time(); + mavlink_log_info(mavlink_fd, "[inav] new home on arm: alt = %.3f", baro_alt0); + } + /* accel bias correction, now only for Z * not strictly correct, but stable and works */ accel_bias[2] += (accel_NED[2] + CONSTANTS_ONE_G) * params.w_acc_bias * dt; @@ -629,6 +638,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_global_position), vehicle_global_position_pub, &global_pos); } + flag_armed = armed.armed; } warnx("exiting."); -- cgit v1.2.3