From 104f38eea8ac4a40ae58948677845ea0072041c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Sep 2015 11:05:46 +0200 Subject: [PATCH 01/92] MC position controller: Guard against invalid setpoints --- .../mc_pos_control/mc_pos_control_main.cpp | 42 ++++++++++++++----- 1 file changed, 32 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 36d87b9383..ecfb16e40f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -791,18 +791,45 @@ void MulticopterPositionControl::control_auto(float dt) } } + bool current_setpoint_valid = false; + bool previous_setpoint_valid = false; + + math::Vector<3> prev_sp; + math::Vector<3> curr_sp; + if (_pos_sp_triplet.current.valid) { - /* in case of interrupted mission don't go to waypoint but stay at current position */ - _reset_pos_sp = true; - _reset_alt_sp = true; /* project setpoint to local frame */ - math::Vector<3> curr_sp; map_projection_project(&_ref_pos, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + if (PX4_ISFINITE(curr_sp(0)) && + PX4_ISFINITE(curr_sp(1)) && + PX4_ISFINITE(curr_sp(2))) { + current_setpoint_valid = true; + } + } + + if (_pos_sp_triplet.previous.valid) { + map_projection_project(&_ref_pos, + _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, + &prev_sp.data[0], &prev_sp.data[1]); + prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); + + if (PX4_ISFINITE(prev_sp(0)) && + PX4_ISFINITE(prev_sp(1)) && + PX4_ISFINITE(prev_sp(2))) { + previous_setpoint_valid = true; + } + } + + if (current_setpoint_valid) { + /* in case of interrupted mission don't go to waypoint but stay at current position */ + _reset_pos_sp = true; + _reset_alt_sp = true; + /* scaled space: 1 == position error resulting max allowed speed */ math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here @@ -812,13 +839,8 @@ void MulticopterPositionControl::control_auto(float dt) /* by default use current setpoint as is */ math::Vector<3> pos_sp_s = curr_sp_s; - if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION && previous_setpoint_valid) { /* follow "previous - current" line */ - math::Vector<3> prev_sp; - map_projection_project(&_ref_pos, - _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, - &prev_sp.data[0], &prev_sp.data[1]); - prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { From 66785916d06047440957a2c66a48d4093a4b4635 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Thu, 22 Oct 2015 14:03:58 +0200 Subject: [PATCH 02/92] several first changes to optical flow estimation (lidar, gyro comp., weights). --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 172 +++++++++++++----- .../position_estimator_inav_params.c | 32 ++-- .../position_estimator_inav_params.h | 12 +- 4 files changed, 150 insertions(+), 68 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index 90ab4ad74c..ed5c05c870 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3800) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3850) endif() px4_add_module( MODULE modules__position_estimator_inav 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 53a7c678f6..ad5698989e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -38,7 +38,6 @@ * @author Anton Babushkin * @author Nuno Marques */ - #include #include #include @@ -66,6 +65,7 @@ #include #include #include +#include #include #include #include @@ -90,8 +90,8 @@ static const hrt_abstime vision_topic_timeout = 500000; // Vision topic timeout static const hrt_abstime mocap_topic_timeout = 500000; // Mocap topic timeout = 0.5s static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s -static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms -static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss +static const hrt_abstime lidar_timeout = 150000; // lidar timeout = 150ms +static const hrt_abstime lidar_valid_timeout = 1000000; // estimate lidar distance during this time after lidar loss static const unsigned updates_counter_len = 1000000; static const float max_flow = 1.0f; // max flow value that can be used, rad/s @@ -311,19 +311,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { 0.0f }, // D (pos) }; - float corr_sonar = 0.0f; - float corr_sonar_filtered = 0.0f; + float corr_lidar = 0.0f; + float corr_lidar_filtered = 0.0f; float corr_flow[] = { 0.0f, 0.0f }; // N E float w_flow = 0.0f; - float sonar_prev = 0.0f; + float lidar_prev = 0.0f; //hrt_abstime flow_prev = 0; // time of last flow measurement - hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) - hrt_abstime sonar_valid_time = 0; // time of last sonar measurement used for correction (filtered) + hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) + hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) + + //----------test-------------------------- + float flow_test[] = { 0.0f, 0.0f }; + float pos_test[] = { 0.0f, 0.0f }; + float flow_test_average[] = { 0.0f, 0.0f }; + int n_flow = 0; + float gyro_offset_filtered[] = { 0.0f, 0.0f }; + float flow_gyrospeed[] = { 0.0f, 0.0f }; + float flow_gyrospeed_filtered[] = { 0.0f, 0.0f }; + float att_gyrospeed_filtered[] = { 0.0f, 0.0f }; + //---------------------------------------- bool gps_valid = false; // GPS is valid - bool sonar_valid = false; // sonar is valid + bool lidar_valid = false; // lidar is valid bool flow_valid = false; // flow is valid bool flow_accurate = false; // flow should be accurate (this flag not updated if flow_valid == false) bool vision_valid = false; // vision is valid @@ -352,6 +363,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(&mocap, 0, sizeof(mocap)); struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); + struct distance_sensor_s lidar; + memset(&lidar, 0, sizeof(lidar)); /* subscribe */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -364,6 +377,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate)); int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); int home_position_sub = orb_subscribe(ORB_ID(home_position)); + int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor)); /* advertise */ orb_advert_t vehicle_local_position_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos); @@ -445,6 +459,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) attitude_updates++; bool updated; + bool updated2; /* parameter update */ orb_check(parameter_update_sub, &updated); @@ -490,6 +505,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } + //------------test---------------------- + //dont convert into NED for tests + //acc[0] = sensor.accelerometer_m_s2[0]; + //acc[1] = sensor.accelerometer_m_s2[1]; + //acc[2] = sensor.accelerometer_m_s2[2]; + //--------------------------------------- acc[2] += CONSTANTS_ONE_G; @@ -510,56 +531,60 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* optical flow */ orb_check(optical_flow_sub, &updated); + orb_check(distance_sensor_sub, &updated2); - if (updated) { + if (updated && updated2) { orb_copy(ORB_ID(optical_flow), optical_flow_sub, &flow); + orb_copy(ORB_ID(distance_sensor), distance_sensor_sub, &lidar); /* calculate time from previous update */ // float flow_dt = flow_prev > 0 ? (flow.flow_timestamp - flow_prev) * 1e-6f : 0.1f; // flow_prev = flow.flow_timestamp; - if ((flow.ground_distance_m > 0.31f) && - (flow.ground_distance_m < 4.0f) && - (PX4_R(att.R, 2, 2) > 0.7f) && - (fabsf(flow.ground_distance_m - sonar_prev) > FLT_EPSILON)) { + if ((lidar.current_distance > 0.21f) && + (lidar.current_distance < 4.0f) && + /*(PX4_R(att.R, 2, 2) > 0.7f) &&*/ + (fabsf(lidar.current_distance - lidar_prev) > FLT_EPSILON)) { - sonar_time = t; - sonar_prev = flow.ground_distance_m; - corr_sonar = flow.ground_distance_m + surface_offset + z_est[0]; - corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt; + lidar_time = t; + lidar_prev = lidar.current_distance; + corr_lidar = lidar.current_distance + surface_offset + z_est[0]; + corr_lidar_filtered += (corr_lidar - corr_lidar_filtered) * params.lidar_filt; - if (fabsf(corr_sonar) > params.sonar_err) { + if (fabsf(corr_lidar) > params.lidar_err) { /* correction is too large: spike or new ground level? */ - if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) { + if (fabsf(corr_lidar - corr_lidar_filtered) > params.lidar_err) { /* spike detected, ignore */ - corr_sonar = 0.0f; - sonar_valid = false; + corr_lidar = 0.0f; + lidar_valid = false; } else { /* new ground level */ - surface_offset -= corr_sonar; + surface_offset -= corr_lidar; surface_offset_rate = 0.0f; - corr_sonar = 0.0f; - corr_sonar_filtered = 0.0f; - sonar_valid_time = t; - sonar_valid = true; + corr_lidar = 0.0f; + corr_lidar_filtered = 0.0f; + lidar_valid_time = t; + lidar_valid = true; local_pos.surface_bottom_timestamp = t; mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset); } } else { /* correction is ok, use it */ - sonar_valid_time = t; - sonar_valid = true; + lidar_valid_time = t; + lidar_valid = true; } } float flow_q = flow.quality / 255.0f; - float dist_bottom = - z_est[0] - surface_offset; + float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance; - if (dist_bottom > 0.3f && flow_q > params.flow_q_min && (t < sonar_valid_time + sonar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f) { + if (dist_bottom > 0.21f && flow_q > params.flow_q_min /*&& (t < lidar_valid_time + lidar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f*/) { /* distance to surface */ - float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); + //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar + float flow_dist = dist_bottom; //use this if using lidar + /* check if flow if too large for accurate measurements */ /* calculate estimated velocity in body frame */ float body_v_est[2] = { 0.0f, 0.0f }; @@ -571,12 +596,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* set this flag if flow should be accurate according to current velocity and attitude rate estimate */ flow_accurate = fabsf(body_v_est[1] / flow_dist - att.rollspeed) < max_flow && fabsf(body_v_est[0] / flow_dist + att.pitchspeed) < max_flow; + //this flag is not working --> + flow_accurate = true; //already checked if flow_q > 0.3 /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; - //todo check direction of x und y axis - flow_ang[0] = flow.pixel_flow_x_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = flow.pixel_flow_y_integral/(float)flow.integration_timespan*1000000.0f;//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) + flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -592,6 +619,43 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } + //-----------test------------------- + //do not transform to NED for testing + //flow_v[0] = flow_m[0]; + //flow_v[1] = flow_m[1]; + + flow_test[0] = flow_v[0]; + flow_test[1] = flow_v[1]; + + flow_test_average[0] = (flow.pixel_flow_x_integral + flow_test_average[0] * n_flow) / (n_flow + 1); + flow_test_average[1] = (flow.pixel_flow_y_integral + flow_test_average[1] * n_flow) / (n_flow + 1); + + //calculate offset of flow-gyro using already calibrated gyro from autopilot + 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; + + + + //moving average + if (n_flow >= 100) { + gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; + gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; + n_flow = 0; + flow_gyrospeed_filtered[0] = 0.0f; + flow_gyrospeed_filtered[1] = 0.0f; + att_gyrospeed_filtered[0] = 0.0f; + att_gyrospeed_filtered[1] = 0.0f; + } else { + flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); + flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); + att_gyrospeed_filtered[0] = (att.rollspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); + att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); + n_flow++; + //mavlink_log_info(mavlink_fd, "n_flow = %d\n", (int)n_flow); + } + + //---------------------------------- + /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; @@ -599,12 +663,18 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min); w_flow = PX4_R(att.R, 2, 2) * flow_q_weight / fmaxf(1.0f, flow_dist); + /* if flow is not accurate, reduce weight for it */ // TODO make this more fuzzy if (!flow_accurate) { w_flow *= 0.05f; } + //--------test------------- + //mavlink_log_info(mavlink_fd, "flow_accurate = %d\t w_flow = %4.4f\n", (int)flow_accurate, (double)w_flow); + //w_flow = 0.8; + //------------------------- + /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; @@ -862,9 +932,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* check for timeout on FLOW topic */ - if ((flow_valid || sonar_valid) && t > flow.timestamp + flow_topic_timeout) { + if ((flow_valid || lidar_valid) && t > flow.timestamp + flow_topic_timeout) { flow_valid = false; - sonar_valid = false; + lidar_valid = false; warnx("FLOW timeout"); mavlink_log_info(mavlink_fd, "[inav] FLOW timeout"); } @@ -890,10 +960,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) mavlink_log_info(mavlink_fd, "[inav] MOCAP timeout"); } - /* check for sonar measurement timeout */ - if (sonar_valid && (t > (sonar_time + sonar_timeout))) { - corr_sonar = 0.0f; - sonar_valid = false; + /* check for lidar measurement timeout */ + if (lidar_valid && (t > (lidar_time + lidar_timeout))) { + corr_lidar = 0.0f; + lidar_valid = false; } float dt = t_prev > 0 ? (t - t_prev) / 1000000.0f : 0.0f; @@ -921,16 +991,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap; - bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); + bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout); if (dist_bottom_valid) { /* surface distance prediction */ surface_offset += surface_offset_rate * dt; /* surface distance correction */ - if (sonar_valid) { - surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt; - surface_offset -= corr_sonar * params.w_z_sonar * dt; + if (lidar_valid) { + surface_offset_rate -= corr_lidar * 0.5f * params.w_z_lidar * params.w_z_lidar * dt; + surface_offset -= corr_lidar * params.w_z_lidar * dt; } } @@ -1114,6 +1184,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); + //mavlink_log_info(mavlink_fd, "w_flow = %2.4f\t w_xy_flow = %2.4f\n", (double)w_flow, (double)params.w_xy_flow); } if (use_gps_xy) { @@ -1211,16 +1282,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } + //-------------test-------------- + pos_test[0] += flow_test[0] * dt; + pos_test[1] += flow_test[1] * dt; + //------------------------------- + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; + local_pos.x = corr_baro; //test local_pos.x = x_est[0]; + local_pos.vx = surface_offset; // flow_test[0]; //test local_pos.vx = x_est[1]; + local_pos.y = acc_bias[2]; //test local_pos.y = y_est[0]; + local_pos.vy = flow_gyrospeed_filtered[0]; //test local_pos.vy = y_est[1]; + local_pos.z = lidar.current_distance; //flow_test[0]; //test local_pos.z = z_est[0]; + local_pos.vz = - z_est[0] - surface_offset; //flow_test[1]; //test local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index 90fb472952..acda5ffc70 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -86,15 +86,15 @@ PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 0.0f); PARAM_DEFINE_FLOAT(INAV_W_Z_VIS_P, 5.0f); /** - * Z axis weight for sonar + * Z axis weight for lidar * - * Weight (cutoff frequency) for sonar measurements. + * Weight (cutoff frequency) for lidar measurements. * * @min 0.0 * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f); +PARAM_DEFINE_FLOAT(INAV_W_Z_LIDAR, 3.0f); /** * XY axis weight for GPS position @@ -158,10 +158,10 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_V, 0.0f); * Weight (cutoff frequency) for optical flow (velocity) measurements. * * @min 0.0 - * @max 10.0 + * @max 30.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 10.0f); /** * XY axis weight for resetting velocity @@ -217,18 +217,18 @@ PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f); * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f); +PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.3f); /** - * Weight for sonar filter + * Weight for lidar filter * - * Sonar filter detects spikes on sonar measurements and used to detect new surface level. + * Lidar filter detects spikes on lidar measurements and used to detect new surface level. * * @min 0.0 * @max 1.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_FILT, 0.05f); /** * Sonar maximal error for new surface @@ -240,7 +240,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f); * @unit m * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); +PARAM_DEFINE_FLOAT(INAV_LIDAR_ERR, 0.5f); /** * Land detector time @@ -319,7 +319,7 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->w_z_gps_p = param_find("INAV_W_Z_GPS_P"); h->w_z_gps_v = param_find("INAV_W_Z_GPS_V"); h->w_z_vision_p = param_find("INAV_W_Z_VIS_P"); - h->w_z_sonar = param_find("INAV_W_Z_SONAR"); + h->w_z_lidar = param_find("INAV_W_Z_LIDAR"); h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P"); h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V"); h->w_xy_vision_p = param_find("INAV_W_XY_VIS_P"); @@ -331,8 +331,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->w_acc_bias = param_find("INAV_W_ACC_BIAS"); h->flow_k = param_find("INAV_FLOW_K"); h->flow_q_min = param_find("INAV_FLOW_Q_MIN"); - h->sonar_filt = param_find("INAV_SONAR_FILT"); - h->sonar_err = param_find("INAV_SONAR_ERR"); + h->lidar_filt = param_find("INAV_LIDAR_FILT"); + h->lidar_err = param_find("INAV_LIDAR_ERR"); h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); @@ -347,7 +347,7 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->w_z_baro, &(p->w_z_baro)); param_get(h->w_z_gps_p, &(p->w_z_gps_p)); param_get(h->w_z_vision_p, &(p->w_z_vision_p)); - param_get(h->w_z_sonar, &(p->w_z_sonar)); + param_get(h->w_z_lidar, &(p->w_z_lidar)); param_get(h->w_xy_gps_p, &(p->w_xy_gps_p)); param_get(h->w_xy_gps_v, &(p->w_xy_gps_v)); param_get(h->w_xy_vision_p, &(p->w_xy_vision_p)); @@ -359,8 +359,8 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->w_acc_bias, &(p->w_acc_bias)); param_get(h->flow_k, &(p->flow_k)); param_get(h->flow_q_min, &(p->flow_q_min)); - param_get(h->sonar_filt, &(p->sonar_filt)); - param_get(h->sonar_err, &(p->sonar_err)); + param_get(h->lidar_filt, &(p->lidar_filt)); + param_get(h->lidar_err, &(p->lidar_err)); param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 9adc27d7c0..434e3dfe0a 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -46,7 +46,7 @@ struct position_estimator_inav_params { float w_z_gps_p; float w_z_gps_v; float w_z_vision_p; - float w_z_sonar; + float w_z_lidar; float w_xy_gps_p; float w_xy_gps_v; float w_xy_vision_p; @@ -58,8 +58,8 @@ struct position_estimator_inav_params { float w_acc_bias; float flow_k; float flow_q_min; - float sonar_filt; - float sonar_err; + float lidar_filt; + float lidar_err; float land_t; float land_disp; float land_thr; @@ -72,7 +72,7 @@ struct position_estimator_inav_param_handles { param_t w_z_gps_p; param_t w_z_gps_v; param_t w_z_vision_p; - param_t w_z_sonar; + param_t w_z_lidar; param_t w_xy_gps_p; param_t w_xy_gps_v; param_t w_xy_vision_p; @@ -84,8 +84,8 @@ struct position_estimator_inav_param_handles { param_t w_acc_bias; param_t flow_k; param_t flow_q_min; - param_t sonar_filt; - param_t sonar_err; + param_t lidar_filt; + param_t lidar_err; param_t land_t; param_t land_disp; param_t land_thr; From 55cf08d3830aef57f927a9f325cdb2a08434aadf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 17:47:22 +0200 Subject: [PATCH 03/92] Commander: prevent the user from arming the system when USB was ever connected --- src/modules/commander/commander.cpp | 12 ++++++++++++ src/modules/commander/state_machine_helper.cpp | 9 ++++++++- 2 files changed, 20 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 79d0811b72..b6a2846773 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1494,6 +1494,18 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + + if (status.usb_connected && !system_power.usb_connected) { + /* + * apparently the USB cable went away but we are still powered, + * so lets reset to a classic non-usb state. + */ + usleep(100000); + mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") + usleep(400000); + px4_systemreset(false); + } + status.usb_connected = system_power.usb_connected; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44f1f3506..3b169068d4 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -737,5 +737,12 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) checkAirspeed = true; } - return !Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + bool prearm_ok = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !(status->rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status->circuit_breaker_engaged_gpsfailure_check, true); + + if (status->usb_connected) { + prearm_ok = false; + mavlink_log_critical(mavlink_fd, "NOT ARMING: Flying with USB connected prohibited"); + } + + return !prearm_ok; } From 09fcbde52b7b75d13ecec16358f1890e2ed4b6a7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 11:58:30 +0200 Subject: [PATCH 04/92] Make some space in flash --- cmake/configs/nuttx_px4fmu-v2_default.cmake | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/configs/nuttx_px4fmu-v2_default.cmake b/cmake/configs/nuttx_px4fmu-v2_default.cmake index f3638c5749..0c6458cad1 100644 --- a/cmake/configs/nuttx_px4fmu-v2_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v2_default.cmake @@ -37,7 +37,7 @@ set(config_module_list drivers/meas_airspeed drivers/frsky_telemetry modules/sensors - drivers/mkblctrl + #drivers/mkblctrl drivers/px4flow drivers/oreoled drivers/gimbal @@ -54,7 +54,7 @@ set(config_module_list systemcmds/pwm systemcmds/esc_calib systemcmds/reboot - systemcmds/topic_listener + #systemcmds/topic_listener systemcmds/top systemcmds/config systemcmds/nshterm From 9239a93a71681bca31d6fcbfd6436906b9e9f17e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 11:59:00 +0200 Subject: [PATCH 05/92] Include both ESC images in standard binary --- .travis.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.travis.yml b/.travis.yml index 3786602b09..9d89caca85 100644 --- a/.travis.yml +++ b/.travis.yml @@ -100,6 +100,14 @@ script: - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test + - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol + - cd vectorcontrol + - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make + - cd .. + - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ + - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then From 96968911e4a246a2d5e700de7132d3a404069a15 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 12:12:18 +0200 Subject: [PATCH 06/92] Keep size command in ccache path env --- .travis.yml | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 9d89caca85..e0d40aebae 100644 --- a/.travis.yml +++ b/.travis.yml @@ -73,6 +73,7 @@ before_script: - mkdir -p ~/bin - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size - ln -s /usr/bin/ccache ~/bin/clang++ - ln -s /usr/bin/ccache ~/bin/clang++-3.4 - ln -s /usr/bin/ccache ~/bin/clang++-3.5 @@ -96,10 +97,7 @@ script: - make check_format - arm-none-eabi-gcc --version - echo 'Building POSIX Firmware..' && make posix_sitl_simple - - echo 'Running Tests..' && make posix_sitl_simple test && cat build_posix_sitl_simple/src/modules/systemlib/mixer/mixer_multirotor.generated.h - - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - - echo 'Running Tests..' && make px4fmu-v2_default test + - echo 'Running Tests..' && make posix_sitl_simple test - echo 'Building UAVCAN node firmware..' && git clone https://github.com/thiemar/vectorcontrol - cd vectorcontrol - BOARD=s2740vc_1_0 make && BOARD=px4esc_1_6 make @@ -108,6 +106,9 @@ script: - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin + - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default + - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default + - echo 'Running Tests..' && make px4fmu-v2_default test after_success: - if [ "${TRAVIS_OS_NAME}" = "linux" ]; then From 4d6094cf70d2c4b959187bec824945fd2429aa45 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 12:22:45 +0200 Subject: [PATCH 07/92] Add objcopy --- .travis.yml | 1 + 1 file changed, 1 insertion(+) diff --git a/.travis.yml b/.travis.yml index e0d40aebae..70342e4ef3 100644 --- a/.travis.yml +++ b/.travis.yml @@ -74,6 +74,7 @@ before_script: - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-g++ - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-gcc - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-size + - ln -s /usr/bin/ccache ~/bin/arm-none-eabi-objcopy - ln -s /usr/bin/ccache ~/bin/clang++ - ln -s /usr/bin/ccache ~/bin/clang++-3.4 - ln -s /usr/bin/ccache ~/bin/clang++-3.5 From 89e6a80fd6b8fc6bc3279a2367b5b5eeb837f9dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Oct 2015 09:43:21 +0100 Subject: [PATCH 08/92] Fix path according to Ben's instructions --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 70342e4ef3..d9b5b4e75a 100644 --- a/.travis.yml +++ b/.travis.yml @@ -105,8 +105,8 @@ script: - cd .. - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.uavcan.bin - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.uavcan.bin + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.bin + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.bin - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test From 8d0e10e830b4f3b676391acdcc63b95374118ff8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Oct 2015 19:14:23 +0200 Subject: [PATCH 09/92] Commander: Set home on takeoff --- src/modules/commander/commander.cpp | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..0b0b8dd5d4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -881,7 +881,7 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool arm_tune_played = false; - bool was_armed = false; + bool was_landed = true; bool startup_in_hil = false; @@ -1029,17 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) px4_task_exit(ERROR); } - /* armed topic */ - orb_advert_t armed_pub; /* Initialize armed with all false */ memset(&armed, 0, sizeof(armed)); + /* armed topic */ + orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); /* vehicle control mode topic */ memset(&control_mode, 0, sizeof(control_mode)); orb_advert_t control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - /* home position */ orb_advert_t home_pub = nullptr; memset(&_home, 0, sizeof(_home)); @@ -2250,9 +2248,10 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { commander_set_home_position(home_pub, _home, local_position, global_position); } + was_landed = status.condition_landed; /* print new state */ if (arming_state_changed) { @@ -2261,8 +2260,6 @@ int commander_thread_main(int argc, char *argv[]) arming_state_changed = false; } - was_armed = armed.armed; - /* now set navigation state according to failsafe and main state */ bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled, mission_result.finished, From b7f3c96d4a28a8432eb838364f059d6e79cda90d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:36:09 +0100 Subject: [PATCH 10/92] Set home on arming and on takeoff --- src/modules/commander/commander.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0b0b8dd5d4..86f8ad546b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -149,7 +149,7 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60; /**< Maximum percentage #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 10000000 -#define INAIR_RESTART_HOLDOFF_INTERVAL 2000000 +#define INAIR_RESTART_HOLDOFF_INTERVAL 1000000 #define HIL_ID_MIN 1000 #define HIL_ID_MAX 1999 @@ -882,6 +882,7 @@ int commander_thread_main(int argc, char *argv[]) bool arm_tune_played = false; bool was_landed = true; + bool was_armed = false; bool startup_in_hil = false; @@ -2248,10 +2249,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ - else if (was_landed && !status.condition_landed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) { + else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && + (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { commander_set_home_position(home_pub, _home, local_position, global_position); } was_landed = status.condition_landed; + was_armed = armed.armed; /* print new state */ if (arming_state_changed) { From 12bd1ecb7c0d68b3fbe5eb2cebada52fc63b9d8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:57:42 +0100 Subject: [PATCH 11/92] Home pos: Add yaw field --- msg/home_position.msg | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/msg/home_position.msg b/msg/home_position.msg index d8aff3f634..7135c07b18 100644 --- a/msg/home_position.msg +++ b/msg/home_position.msg @@ -8,3 +8,8 @@ float32 alt # Altitude in meters (AMSL) float32 x # X coordinate in meters float32 y # Y coordinate in meters float32 z # Z coordinate in meters + +float32 yaw # Yaw angle in radians +float32 direction_x # Takeoff direction in NED X +float32 direction_y # Takeoff direction in NED Y +float32 direction_z # Takeoff direction in NED Z From 3f4a8bf76dea9b50bcf734aa119f28a2c74fd8b2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:58:54 +0100 Subject: [PATCH 12/92] Commander: Set yaw on takeoff and use it as yaw during descend phase of RTL --- src/modules/commander/CMakeLists.txt | 2 +- src/modules/commander/commander.cpp | 44 +++++++++++++++++++--------- 2 files changed, 31 insertions(+), 15 deletions(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 5ca0ff2ca6..6cf2531ade 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=2300) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400) endif() px4_add_module( MODULE modules__commander diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 86f8ad546b..53fe14dc08 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -221,7 +222,7 @@ void usage(const char *reason); */ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub); + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub); /** * Mainloop of commander. @@ -253,7 +254,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armed * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition); + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude); /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. @@ -291,7 +293,7 @@ int commander_main(int argc, char *argv[]) daemon_task = px4_task_spawn_cmd("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 3400, + 3500, commander_thread_main, (char * const *)&argv[0]); @@ -490,7 +492,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local, struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local, struct home_position_s *home, struct vehicle_global_position_s *global_pos, - struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub) + struct vehicle_local_position_s *local_pos, struct vehicle_attitude_s *attitude, orb_advert_t *home_pub) { /* only handle commands that are meant to be handled by this system and component */ if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id) @@ -524,7 +526,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s if (cmd_arm && (arming_ret == TRANSITION_CHANGED) && (hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) { - commander_set_home_position(*home_pub, *home, *local_pos, *global_pos); + commander_set_home_position(*home_pub, *home, *local_pos, *global_pos, *attitude); } if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { @@ -833,7 +835,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s * time the vehicle is armed with a good GPS fix. **/ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &home, - const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition) + const vehicle_local_position_s &localPosition, const vehicle_global_position_s &globalPosition, + const vehicle_attitude_s &attitude) { //Need global position fix to be able to set home if (!status.condition_global_position_valid) { @@ -855,6 +858,8 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s & home.y = localPosition.y; home.z = localPosition.z; + home.yaw = attitude.yaw; + warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt); @@ -1137,13 +1142,15 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); - struct vehicle_local_position_s local_position; - memset(&local_position, 0, sizeof(local_position)); + struct vehicle_local_position_s local_position = {}; + + /* Subscribe to attitude data */ + int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); + struct vehicle_attitude_s attitude = {}; /* Subscribe to land detector */ int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected)); - struct vehicle_land_detected_s land_detector; - memset(&land_detector, 0, sizeof(land_detector)); + struct vehicle_land_detected_s land_detector = {}; /* * The home position is set based on GPS only, to prevent a dependency between @@ -1573,6 +1580,14 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); } + /* update attitude estimate */ + orb_check(attitude_sub, &updated); + + if (updated) { + /* position changed */ + orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &attitude); + } + //update condition_global_position_valid //Global positions are only published by the estimators if they are valid if (hrt_absolute_time() - global_position.timestamp > POSITION_TIMEOUT) { @@ -2183,7 +2198,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) { + if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &attitude, &home_pub)) { status_changed = true; } } @@ -2245,14 +2260,15 @@ int commander_thread_main(int argc, char *argv[]) /* First time home position update - but only if disarmed */ if (!status.condition_home_position_valid && !armed.armed) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } - /* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */ + /* update home position on arming if at least 1s from commander start spent to avoid setting home on in-air restart */ else if (((!was_armed && armed.armed) || (was_landed && !status.condition_landed)) && (now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL)) { - commander_set_home_position(home_pub, _home, local_position, global_position); + commander_set_home_position(home_pub, _home, local_position, global_position, attitude); } + was_landed = status.condition_landed; was_armed = armed.armed; From 5759358da9965045478cd9b24d40be64cc9516b3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 09:59:09 +0100 Subject: [PATCH 13/92] Navigator: Use yaw of home position --- src/modules/navigator/rtl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 246007ad81..8a4d89ef35 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -190,7 +190,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; From 8137a261224088264a79bd025a9421641e00afc9 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Tue, 27 Oct 2015 10:07:32 +0100 Subject: [PATCH 14/92] Additional yaw compensation for flow module offset from center of rotation. --- .../position_estimator_inav/CMakeLists.txt | 2 +- .../position_estimator_inav_main.c | 115 +++++++----------- .../position_estimator_inav_params.c | 28 +++++ .../position_estimator_inav_params.h | 4 + 4 files changed, 76 insertions(+), 73 deletions(-) diff --git a/src/modules/position_estimator_inav/CMakeLists.txt b/src/modules/position_estimator_inav/CMakeLists.txt index ed5c05c870..2d5315b92b 100644 --- a/src/modules/position_estimator_inav/CMakeLists.txt +++ b/src/modules/position_estimator_inav/CMakeLists.txt @@ -31,7 +31,7 @@ # ############################################################################ if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=3850) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=3890) endif() px4_add_module( MODULE modules__position_estimator_inav 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 ad5698989e..04f3c4d918 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -322,16 +322,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) hrt_abstime lidar_time = 0; // time of last lidar measurement (not filtered) hrt_abstime lidar_valid_time = 0; // time of last lidar measurement used for correction (filtered) - //----------test-------------------------- - float flow_test[] = { 0.0f, 0.0f }; - float pos_test[] = { 0.0f, 0.0f }; - float flow_test_average[] = { 0.0f, 0.0f }; int n_flow = 0; - float gyro_offset_filtered[] = { 0.0f, 0.0f }; - float flow_gyrospeed[] = { 0.0f, 0.0f }; - float flow_gyrospeed_filtered[] = { 0.0f, 0.0f }; - float att_gyrospeed_filtered[] = { 0.0f, 0.0f }; - //---------------------------------------- + float gyro_offset_filtered[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed[] = { 0.0f, 0.0f, 0.0f }; + float flow_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float att_gyrospeed_filtered[] = { 0.0f, 0.0f, 0.0f }; + float yaw_comp[] = { 0.0f, 0.0f }; bool gps_valid = false; // GPS is valid bool lidar_valid = false; // lidar is valid @@ -505,12 +501,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) acc[i] += PX4_R(att.R, i, j) * sensor.accelerometer_m_s2[j]; } } - //------------test---------------------- - //dont convert into NED for tests - //acc[0] = sensor.accelerometer_m_s2[0]; - //acc[1] = sensor.accelerometer_m_s2[1]; - //acc[2] = sensor.accelerometer_m_s2[2]; - //--------------------------------------- acc[2] += CONSTANTS_ONE_G; @@ -580,7 +570,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float flow_q = flow.quality / 255.0f; float dist_bottom = - z_est[0] - surface_offset; //lidar.current_distance; - if (dist_bottom > 0.21f && flow_q > params.flow_q_min /*&& (t < lidar_valid_time + lidar_valid_timeout) && PX4_R(att.R, 2, 2) > 0.7f*/) { + if (dist_bottom > 0.21f && flow_q > params.flow_q_min) { /* distance to surface */ //float flow_dist = dist_bottom / PX4_R(att.R, 2, 2); //use this if using sonar float flow_dist = dist_bottom; //use this if using lidar @@ -599,11 +589,45 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) //this flag is not working --> flow_accurate = true; //already checked if flow_q > 0.3 + + /*calculate offset of flow-gyro using already calibrated gyro from autopilot*/ + 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]; + gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; + gyro_offset_filtered[2] = flow_gyrospeed_filtered[2] - att_gyrospeed_filtered[2]; + n_flow = 0; + flow_gyrospeed_filtered[0] = 0.0f; + flow_gyrospeed_filtered[1] = 0.0f; + flow_gyrospeed_filtered[2] = 0.0f; + att_gyrospeed_filtered[0] = 0.0f; + att_gyrospeed_filtered[1] = 0.0f; + att_gyrospeed_filtered[2] = 0.0f; + } else { + flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); + flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); + flow_gyrospeed_filtered[2] = (flow_gyrospeed[2] + n_flow * flow_gyrospeed_filtered[2]) / (n_flow + 1); + att_gyrospeed_filtered[0] = (att.pitchspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); + att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); + 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]); + yaw_comp[1] = - params.flow_module_offset_y * (flow_gyrospeed[2] - gyro_offset_filtered[2]); + + /* convert raw flow to angular flow (rad/s) */ float flow_ang[2]; //calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro) - flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; - flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; + flow_ang[0] = (flow.pixel_flow_x_integral - flow.gyro_x_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[0] - yaw_comp[0];//flow.flow_raw_x * params.flow_k / 1000.0f / flow_dt; + flow_ang[1] = (flow.pixel_flow_y_integral - flow.gyro_y_rate_integral)/(float)flow.integration_timespan*1000000.0f + gyro_offset_filtered[1] - yaw_comp[1];//flow.flow_raw_y * params.flow_k / 1000.0f / flow_dt; /* flow measurements vector */ float flow_m[3]; flow_m[0] = -flow_ang[0] * flow_dist; @@ -619,43 +643,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - //-----------test------------------- - //do not transform to NED for testing - //flow_v[0] = flow_m[0]; - //flow_v[1] = flow_m[1]; - - flow_test[0] = flow_v[0]; - flow_test[1] = flow_v[1]; - - flow_test_average[0] = (flow.pixel_flow_x_integral + flow_test_average[0] * n_flow) / (n_flow + 1); - flow_test_average[1] = (flow.pixel_flow_y_integral + flow_test_average[1] * n_flow) / (n_flow + 1); - - //calculate offset of flow-gyro using already calibrated gyro from autopilot - 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; - - - - //moving average - if (n_flow >= 100) { - gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0]; - gyro_offset_filtered[1] = flow_gyrospeed_filtered[1] - att_gyrospeed_filtered[1]; - n_flow = 0; - flow_gyrospeed_filtered[0] = 0.0f; - flow_gyrospeed_filtered[1] = 0.0f; - att_gyrospeed_filtered[0] = 0.0f; - att_gyrospeed_filtered[1] = 0.0f; - } else { - flow_gyrospeed_filtered[0] = (flow_gyrospeed[0] + n_flow * flow_gyrospeed_filtered[0]) / (n_flow + 1); - flow_gyrospeed_filtered[1] = (flow_gyrospeed[1] + n_flow * flow_gyrospeed_filtered[1]) / (n_flow + 1); - att_gyrospeed_filtered[0] = (att.rollspeed + n_flow * att_gyrospeed_filtered[0]) / (n_flow + 1); - att_gyrospeed_filtered[1] = (att.rollspeed + n_flow * att_gyrospeed_filtered[1]) / (n_flow + 1); - n_flow++; - //mavlink_log_info(mavlink_fd, "n_flow = %d\n", (int)n_flow); - } - - //---------------------------------- - /* velocity correction */ corr_flow[0] = flow_v[0] - x_est[1]; corr_flow[1] = flow_v[1] - y_est[1]; @@ -670,11 +657,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) w_flow *= 0.05f; } - //--------test------------- - //mavlink_log_info(mavlink_fd, "flow_accurate = %d\t w_flow = %4.4f\n", (int)flow_accurate, (double)w_flow); - //w_flow = 0.8; - //------------------------- - /* under ideal conditions, on 1m distance assume EPH = 10cm */ eph_flow = 0.1f / w_flow; @@ -1282,27 +1264,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) buf_ptr = 0; } - //-------------test-------------- - pos_test[0] += flow_test[0] * dt; - pos_test[1] += flow_test[1] * dt; - //------------------------------- - /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; local_pos.xy_global = local_pos.xy_valid && use_gps_xy; local_pos.z_global = local_pos.z_valid && use_gps_z; - local_pos.x = corr_baro; //test local_pos.x = x_est[0]; - local_pos.vx = surface_offset; // flow_test[0]; //test local_pos.vx = x_est[1]; - local_pos.y = acc_bias[2]; //test local_pos.y = y_est[0]; - local_pos.vy = flow_gyrospeed_filtered[0]; //test local_pos.vy = y_est[1]; - local_pos.z = lidar.current_distance; //flow_test[0]; //test local_pos.z = z_est[0]; - local_pos.vz = - z_est[0] - surface_offset; //flow_test[1]; //test local_pos.vz = z_est[1]; local_pos.yaw = att.yaw; local_pos.dist_bottom_valid = dist_bottom_valid; @@ -1311,7 +1282,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (local_pos.dist_bottom_valid) { local_pos.dist_bottom = -z_est[0] - surface_offset; - local_pos.dist_bottom_rate = -z_est[1] - surface_offset_rate; + local_pos.dist_bottom_rate = - z_est[1] - surface_offset_rate; } local_pos.timestamp = t; diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index acda5ffc70..be8a136c4d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -289,6 +289,30 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); */ PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); +/** + * Flow module offset (center of rotation) in X direction + * + * Yaw X flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_X, 0.0f); + +/** + * Flow module offset (center of rotation) in Y direction + * + * Yaw Y flow compensation + * + * @min -1.0 + * @max 1.0 + * @unit m + * @group Position Estimator INAV + */ +PARAM_DEFINE_FLOAT(INAV_FLOW_DIST_Y, 0.0f); + /** * Disable vision input * @@ -338,6 +362,8 @@ int inav_parameters_init(struct position_estimator_inav_param_handles *h) h->land_thr = param_find("INAV_LAND_THR"); h->no_vision = param_find("CBRK_NO_VISION"); h->delay_gps = param_find("INAV_DELAY_GPS"); + h->flow_module_offset_x = param_find("INAV_FLOW_DIST_X"); + h->flow_module_offset_y = param_find("INAV_FLOW_DIST_Y"); return 0; } @@ -366,6 +392,8 @@ int inav_parameters_update(const struct position_estimator_inav_param_handles *h param_get(h->land_thr, &(p->land_thr)); param_get(h->no_vision, &(p->no_vision)); param_get(h->delay_gps, &(p->delay_gps)); + param_get(h->flow_module_offset_x, &(p->flow_module_offset_x)); + param_get(h->flow_module_offset_y, &(p->flow_module_offset_y)); return 0; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 434e3dfe0a..43601568f8 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -65,6 +65,8 @@ struct position_estimator_inav_params { float land_thr; int32_t no_vision; float delay_gps; + float flow_module_offset_x; + float flow_module_offset_y; }; struct position_estimator_inav_param_handles { @@ -91,6 +93,8 @@ struct position_estimator_inav_param_handles { param_t land_thr; param_t no_vision; param_t delay_gps; + param_t flow_module_offset_x; + param_t flow_module_offset_y; }; #define CBRK_NO_VISION_KEY 328754 From f297cf96f7aa143747b41f9e1f0fb6744684c816 Mon Sep 17 00:00:00 2001 From: skyworks_zyx Date: Tue, 27 Oct 2015 19:23:02 +0800 Subject: [PATCH 15/92] Ignore .DS_Store in the ROMFS pruner. --- Tools/px_romfs_pruner.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index 85ff678b93..1fa1efbd90 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,7 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file: continue file_path = os.path.join(root, file) @@ -74,8 +74,7 @@ def main(): else: if not line.isspace() and not line.strip().startswith("#"): pruned_content += line - - # overwrite old scratch file + # overwrite old scratch file with open(file_path, "wb") as f: f.write(pruned_content.encode("ascii", errors='strict')) From 218f11f54b1e097440c7b3f4abdff61253c48a50 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 13:14:17 +0100 Subject: [PATCH 16/92] kill gazebo properly --- Tools/sitl_run.sh | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 6eb69eb46b..e437f7651f 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -54,8 +54,10 @@ then cd Tools/sitl_gazebo/Build cmake .. make -j4 - gazebo ../worlds/iris.world & + gzserver ../worlds/iris.world & SIM_PID=`echo $!` + gzclient& + GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" exit 1 @@ -82,4 +84,5 @@ then elif [ "$3" == "gazebo" ] then kill -9 $SIM_PID + kill -9 $GUI_PID fi From 8524a507033872e46625eb4b17b3929dd2fb1b6a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 27 Oct 2015 22:15:19 +0100 Subject: [PATCH 17/92] Fixed deprecation warning in Makefile --- Makefile | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Makefile b/Makefile index f25bd92233..f1411b3bd2 100644 --- a/Makefile +++ b/Makefile @@ -156,8 +156,8 @@ posix_sitl_default: posix_sitl_simple ros: ros_sitl_simple sitl_deprecation: - @echo "Deprecated. Use 'make posix_sitl_default run_sitl' instead." - @echo "Change init script with 'make posix_sitl_default config'" + @echo "Deprecated. Use 'make posix_sitl_default jmavsim' or" + @echo "'make posix_sitl_default gazebo' if Gazebo is preferred." sitl_quad: sitl_deprecation sitl_plane: sitl_deprecation From c4a82d78c880b9de3800b3c6983e827890f9ec8a Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 20 Oct 2015 20:38:42 -0400 Subject: [PATCH 18/92] Added commander support for rattitude mode. Still need to incorporate attitude/rate switching in multicopter control --- msg/manual_control_setpoint.msg | 1 + msg/rc_channels.msg | 7 +-- msg/vehicle_status.msg | 6 ++- src/modules/commander/commander.cpp | 47 +++++++++++++++---- .../commander/state_machine_helper.cpp | 6 +++ src/modules/sensors/sensor_params.c | 26 ++++++++++ src/modules/sensors/sensors.cpp | 17 +++++++ 7 files changed, 96 insertions(+), 14 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index d3cfb078be..dd1f7d0b5e 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,6 +38,7 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 0fa5ed2fc4..b2e08d864a 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=19 +int32 RC_CHANNELS_FUNCTION_MAX=20 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -18,10 +18,11 @@ uint8 RC_CHANNELS_FUNCTION_AUX_5=15 uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 +uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[19] channels # Scaled to -1..1 (throttle: 0..1) +float32[20] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[19] function # Functions mapping +int8[20] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout diff --git a/msg/vehicle_status.msg b/msg/vehicle_status.msg index c0c6dc7f0f..e575b9d5e3 100644 --- a/msg/vehicle_status.msg +++ b/msg/vehicle_status.msg @@ -8,7 +8,8 @@ uint8 MAIN_STATE_AUTO_RTL = 5 uint8 MAIN_STATE_ACRO = 6 uint8 MAIN_STATE_OFFBOARD = 7 uint8 MAIN_STATE_STAB = 8 -uint8 MAIN_STATE_MAX = 9 +uint8 MAIN_STATE_RATTITUDE = 9 +uint8 MAIN_STATE_MAX = 10 # If you change the order, add or remove arming_state_t states make sure to update the arrays # in state_machine_helper.cpp as well. @@ -41,7 +42,8 @@ uint8 NAVIGATION_STATE_DESCEND = 12 # Descend mode (no position control) uint8 NAVIGATION_STATE_TERMINATION = 13 # Termination mode uint8 NAVIGATION_STATE_OFFBOARD = 14 uint8 NAVIGATION_STATE_STAB = 15 # Stabilized mode -uint8 NAVIGATION_STATE_MAX = 16 +uint8 NAVIGATION_STATE_RATTITUDE = 16 # Rattitude (aka "flip") mode +uint8 NAVIGATION_STATE_MAX = 17 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128 diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3395b8c069..ee0888033a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -246,6 +246,9 @@ void print_reject_arm(const char *msg); void print_status(); +transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, + struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos); + transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy); /** @@ -941,6 +944,7 @@ int commander_thread_main(int argc, char *argv[]) const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX]; nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL] = "MANUAL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB] = "STAB"; + nav_states_str[vehicle_status_s::NAVIGATION_STATE_RATTITUDE] = "RATTITUDE"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL] = "ALTCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL] = "POSCTL"; nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION] = "AUTO_MISSION"; @@ -1978,13 +1982,14 @@ int commander_thread_main(int argc, char *argv[]) status.rc_signal_lost = false; - /* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm + /* check if left stick is in lower left position and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed) -> disarm * do it only for rotary wings */ if (status.is_rotary_wing && status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF && (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) && (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.main_state == vehicle_status_s::MAIN_STATE_STAB || + status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE || status.condition_landed) && sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) { @@ -2196,6 +2201,7 @@ int commander_thread_main(int argc, char *argv[]) * and both failed we want to terminate the flight */ if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL && status.main_state !=vehicle_status_s::MAIN_STATE_ACRO && + status.main_state !=vehicle_status_s::MAIN_STATE_RATTITUDE && status.main_state !=vehicle_status_s::MAIN_STATE_STAB && status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL && status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL && @@ -2220,6 +2226,7 @@ int commander_thread_main(int argc, char *argv[]) * If we are in manual (controlled with RC): * if both failed we want to terminate the flight */ if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO || + status.main_state ==vehicle_status_s::MAIN_STATE_RATTITUDE || status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL || status.main_state !=vehicle_status_s::MAIN_STATE_STAB || status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL || @@ -2513,13 +2520,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if ((_last_sp_man.timestamp == sp_man->timestamp) || - ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && - (_last_sp_man.return_switch == sp_man->return_switch) && - (_last_sp_man.mode_switch == sp_man->mode_switch) && - (_last_sp_man.acro_switch == sp_man->acro_switch) && - (_last_sp_man.posctl_switch == sp_man->posctl_switch) && - (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { + if ((last_manual_input == sp_man->timestamp) || + ((last_offboard_switch == sp_man->offboard_switch) && + (last_return_switch == sp_man->return_switch) && + (last_mode_switch == sp_man->mode_switch) && + (last_acro_switch == sp_man->acro_switch) && + (last_rattitude_switch == sp_man->rattitude_switch) && + (last_posctl_switch == sp_man->posctl_switch) && + (last_loiter_switch == sp_man->loiter_switch))) { // update these fields for the geofence system _last_sp_man.timestamp = sp_man->timestamp; @@ -2585,7 +2593,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); } - } else { + } + if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + /* Similar to acro transitions for multirotors. FW aircraft don't need a + * rattitude mode.*/ + if (status.is_rotary_wing) { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_RATTITUDE); + } else { + res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB); + } + }else { res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL); } @@ -2708,6 +2725,18 @@ set_control_mode() control_mode.flag_external_manual_override_ok = false; break; + case vehicle_status_s::NAVIGATION_STATE_RATTITUDE: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_termination_enabled = false; + break; + case vehicle_status_s::NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index d44f1f3506..ea3cab257c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -304,6 +304,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta switch (new_main_state) { case vehicle_status_s::MAIN_STATE_MANUAL: case vehicle_status_s::MAIN_STATE_ACRO: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: ret = TRANSITION_CHANGED; break; @@ -528,6 +529,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en switch (status->main_state) { case vehicle_status_s::MAIN_STATE_ACRO: case vehicle_status_s::MAIN_STATE_MANUAL: + case vehicle_status_s::MAIN_STATE_RATTITUDE: case vehicle_status_s::MAIN_STATE_STAB: case vehicle_status_s::MAIN_STATE_ALTCTL: case vehicle_status_s::MAIN_STATE_POSCTL: @@ -555,6 +557,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL; break; + case vehicle_status_s::MAIN_STATE_RATTITUDE: + status->nav_state = vehicle_status_s::NAVIGATION_STATE_RATTITUDE; + break; + case vehicle_status_s::MAIN_STATE_STAB: status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB; break; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 393cd63a06..3d4dfd62c1 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1969,6 +1969,15 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); +/** + * Rattitude switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_RATT_SW, 0); + /** * Posctl switch channel mapping. * @@ -2132,6 +2141,23 @@ PARAM_DEFINE_FLOAT(RC_ASSIST_TH, 0.25f); */ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); +/** + * Threshold for selecting rattitude mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel Date: Wed, 21 Oct 2015 17:49:50 -0400 Subject: [PATCH 19/92] Added switching from attitude control generated rate setpoints to manual rate setpoints when in rattitude mode --- .../mc_att_control/mc_att_control_main.cpp | 77 +++++++++++-------- .../mc_att_control/mc_att_control_params.c | 19 ++++- 2 files changed, 59 insertions(+), 37 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 830f48dff8..8eb573acc7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -190,12 +190,10 @@ private: param_t pitch_rate_max; param_t yaw_rate_max; - param_t man_roll_max; - param_t man_pitch_max; - param_t man_yaw_max; param_t acro_roll_max; param_t acro_pitch_max; param_t acro_yaw_max; + param_t rattitude_thres; } _params_handles; /**< handles for interesting parameters */ @@ -212,11 +210,8 @@ private: float yaw_rate_max; math::Vector<3> mc_rate_max; /**< attitude rate limits in stabilized modes */ - float man_roll_max; - float man_pitch_max; - float man_yaw_max; math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ - + float rattitude_thres; } _params; /** @@ -346,11 +341,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.roll_rate_max = 0.0f; _params.pitch_rate_max = 0.0f; _params.yaw_rate_max = 0.0f; - _params.man_roll_max = 0.0f; - _params.man_pitch_max = 0.0f; - _params.man_yaw_max = 0.0f; _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); + _params.rattitude_thres = 1.0f; _rates_prev.zero(); _rates_sp.zero(); @@ -380,12 +373,10 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.roll_rate_max = param_find("MC_ROLLRATE_MAX"); _params_handles.pitch_rate_max = param_find("MC_PITCHRATE_MAX"); _params_handles.yaw_rate_max = param_find("MC_YAWRATE_MAX"); - _params_handles.man_roll_max = param_find("MC_MAN_R_MAX"); - _params_handles.man_pitch_max = param_find("MC_MAN_P_MAX"); - _params_handles.man_yaw_max = param_find("MC_MAN_Y_MAX"); _params_handles.acro_roll_max = param_find("MC_ACRO_R_MAX"); _params_handles.acro_pitch_max = param_find("MC_ACRO_P_MAX"); - _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.acro_yaw_max = param_find("MC_ACRO_Y_MAX"); + _params_handles.rattitude_thres = param_find("MC_RATT_TH"); /* fetch initial parameter values */ parameters_update(); @@ -466,14 +457,6 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max); _params.mc_rate_max(2) = math::radians(_params.yaw_rate_max); - /* manual attitude control scale */ - param_get(_params_handles.man_roll_max, &_params.man_roll_max); - param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); - param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); - _params.man_roll_max = math::radians(_params.man_roll_max); - _params.man_pitch_max = math::radians(_params.man_pitch_max); - _params.man_yaw_max = math::radians(_params.man_yaw_max); - /* manual rate control scale and auto mode roll/pitch rate limits */ param_get(_params_handles.acro_roll_max, &v); _params.acro_rate_max(0) = math::radians(v); @@ -482,6 +465,9 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + /* stick deflection needed in rattitude mode to control rates not angles */ + param_get(_params_handles.rattitude_thres, &_params.rattitude_thres); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; @@ -810,22 +796,45 @@ MulticopterAttitudeControl::task_main() vehicle_motor_limits_poll(); if (_v_control_mode.flag_control_attitude_enabled) { + control_attitude(dt); - /* publish attitude rates setpoint */ - _v_rates_sp.roll = _rates_sp(0); - _v_rates_sp.pitch = _rates_sp(1); - _v_rates_sp.yaw = _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); + /* Check if we want to directly pass through manual rate setpoints*/ + if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + /* Calculate the manual rates for all channels */ + math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); + + /* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/ + _v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0); + _v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1); + _v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + /* publish attitude rates setpoint */ + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } + }else{ + + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub != nullptr) { + orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); + + } else if (_rates_sp_id) { + _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); + } } - } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { @@ -976,4 +985,4 @@ int mc_att_control_main(int argc, char *argv[]) warnx("unrecognized command"); return 1; -} +} \ No newline at end of file diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 8d9fb6b911..a2a69d7116 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -251,7 +251,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 360.0f); /** * Max acro pitch rate @@ -261,7 +261,7 @@ PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 360.0f); /** * Max acro yaw rate @@ -270,4 +270,17 @@ PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f); * @min 0.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f); +PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 360.0f); + +/** + * Threshold for Rattitude mode + * + * Manual input needed in order to override attitude control rate setpoints + * and instead pass manual stick inputs as rate setpoints + * + * @unit + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ + PARAM_DEFINE_FLOAT(MC_RATT_TH, 1.0f); From 83ffb56d285d9babdef584c1259d02f0dfbf6f18 Mon Sep 17 00:00:00 2001 From: Eddy Date: Thu, 22 Oct 2015 08:24:54 -0400 Subject: [PATCH 20/92] fixed code style --- src/modules/sensors/sensors.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 28c80ab0a1..5e330ca7a2 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -817,7 +817,7 @@ Sensors::parameters_update() _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RETURN] = _parameters.rc_map_return_sw - 1; - _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw -1; + _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE] = _parameters.rc_map_rattitude_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL] = _parameters.rc_map_posctl_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[rc_channels_s::RC_CHANNELS_FUNCTION_ACRO] = _parameters.rc_map_acro_sw - 1; @@ -1943,8 +1943,9 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_sw3pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv); - manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, _parameters.rc_rattitude_th, - _parameters.rc_rattitude_inv); + manual.rattitude_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RATTITUDE, + _parameters.rc_rattitude_th, + _parameters.rc_rattitude_inv); manual.posctl_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv); manual.return_switch = get_rc_sw2pos_position(rc_channels_s::RC_CHANNELS_FUNCTION_RETURN, _parameters.rc_return_th, From 4e5e8d1c9558790b595830f9289a4b584e4aebc4 Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Sat, 24 Oct 2015 08:22:15 -0400 Subject: [PATCH 21/92] Modified how mc_att_control handles rattitude mode --- .../mc_att_control/mc_att_control_main.cpp | 34 ++++++------------- 1 file changed, 11 insertions(+), 23 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 8eb573acc7..fa08536126 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -795,32 +795,20 @@ MulticopterAttitudeControl::task_main() vehicle_status_poll(); vehicle_motor_limits_poll(); + /* Check if we are in rattitude mode and the pilot is above the threshold on pitch + * or roll (yaw can rotate 360 in normal att control). If both are true don't + * even bother running the attitude controllers */ + if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ + if (fabsf(_manual_control_sp.y) > _params.rattitude_thres || + fabsf(_manual_control_sp.x) > _params.rattitude_thres){ + _v_control_mode.flag_control_attitude_enabled = false; + } + } + if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); - /* Check if we want to directly pass through manual rate setpoints*/ - if(_vehicle_status.main_state == vehicle_status_s::MAIN_STATE_RATTITUDE){ - /* Calculate the manual rates for all channels */ - math::Vector<3> man_rates = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max); - - /* Check all channels and replace attitude controler rate setpoint if manual input greater than threshold*/ - _v_rates_sp.roll = (fabsf(_manual_control_sp.y) > _params.rattitude_thres) ? man_rates(0) : _rates_sp(0); - _v_rates_sp.pitch = (fabsf(_manual_control_sp.x) > _params.rattitude_thres) ? man_rates(1) : _rates_sp(1); - _v_rates_sp.yaw = (fabsf(_manual_control_sp.r) > _params.rattitude_thres) ? man_rates(2) : _rates_sp(2); - _v_rates_sp.thrust = _thrust_sp; - _v_rates_sp.timestamp = hrt_absolute_time(); - - /* publish attitude rates setpoint */ - if (_v_rates_sp_pub != nullptr) { - orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp); - - } else if (_rates_sp_id) { - _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); - } - }else{ - - /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); @@ -834,7 +822,7 @@ MulticopterAttitudeControl::task_main() } else if (_rates_sp_id) { _v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp); } - } + //} } else { /* attitude controller disabled, poll rates setpoint topic */ if (_v_control_mode.flag_control_manual_enabled) { From 72c339a7aca325a0567246bceb90c886faf97baf Mon Sep 17 00:00:00 2001 From: Eddy Scott Date: Tue, 27 Oct 2015 20:39:11 -0400 Subject: [PATCH 22/92] Added else if switch for ACRO/RATTITUDE Handeling to be proper --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee0888033a..49a106d189 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2594,7 +2594,7 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } } - if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ + else if(sp_man->rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON){ /* Similar to acro transitions for multirotors. FW aircraft don't need a * rattitude mode.*/ if (status.is_rotary_wing) { From 5cbb5a76a1006a5d87602a8da25864151e785c0c Mon Sep 17 00:00:00 2001 From: Eddy Date: Wed, 28 Oct 2015 15:58:01 -0400 Subject: [PATCH 23/92] Fixed variable naming to match current master --- src/modules/commander/commander.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 49a106d189..95fcf9e0a5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2520,14 +2520,14 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s } /* manual setpoint has not updated, do not re-evaluate it */ - if ((last_manual_input == sp_man->timestamp) || - ((last_offboard_switch == sp_man->offboard_switch) && - (last_return_switch == sp_man->return_switch) && - (last_mode_switch == sp_man->mode_switch) && - (last_acro_switch == sp_man->acro_switch) && - (last_rattitude_switch == sp_man->rattitude_switch) && - (last_posctl_switch == sp_man->posctl_switch) && - (last_loiter_switch == sp_man->loiter_switch))) { + if ((_last_sp_man.timestamp == sp_man->timestamp) || + ((_last_sp_man.offboard_switch == sp_man->offboard_switch) && + (_last_sp_man.return_switch == sp_man->return_switch) && + (_last_sp_man.mode_switch == sp_man->mode_switch) && + (_last_sp_man.acro_switch == sp_man->acro_switch) && + (_last_sp_man.rattitude_switch == sp_man->rattitude_switch) && + (_last_sp_man.posctl_switch == sp_man->posctl_switch) && + (_last_sp_man.loiter_switch == sp_man->loiter_switch))) { // update these fields for the geofence system _last_sp_man.timestamp = sp_man->timestamp; From 461f72dcee782186af0d723170b98899a6b915b5 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Wed, 28 Oct 2015 12:16:40 -0700 Subject: [PATCH 24/92] Updated instructions for installing cmake to newer version The available version of cmake for Ubuntu 12.04 is too old. The PPA listed does not currently work so the official cmake webisite is a more reliable source to get cmake. Signed-off-by: Mark Charlebois --- Makefile | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/Makefile b/Makefile index f1411b3bd2..4246944ae2 100644 --- a/Makefile +++ b/Makefile @@ -44,10 +44,18 @@ ifneq ($(CMAKE_VER),0) $(warning Not a valid CMake version or CMake not installed.) $(warning On Ubuntu, install or upgrade via:) $(warning ) + $(warning 3rd party PPA:) $(warning sudo add-apt-repository ppa:george-edison55/cmake-3.x -y) $(warning sudo apt-get update) $(warning sudo apt-get install cmake) $(warning ) + $(warning Official website:) + $(warning wget https://cmake.org/files/v3.3/cmake-3.3.2-Linux-x86_64.sh) + $(warning chmod +x cmake-3.3.2-Linux-x86_64.sh) + $(warning sudo mkdir /opt/cmake-3.3.2) + $(warning sudo ./cmake-3.3.2-Linux-x86_64.sh --prefix=/opt/cmake-3.3.2 --exclude-subdir) + $(warning export PATH=/opt/cmake-3.3.2/bin:$$PATH) + $(warning ) $(error Fatal) endif From 99fb498cd2ea23a82394bd7d9910276d909a1886 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 10:58:34 -0400 Subject: [PATCH 25/92] Resurrect controllib testing. --- src/lib/mathlib/math/test/test.cpp | 40 +++ src/lib/mathlib/math/test/test.hpp | 9 + src/modules/controllib/CMakeLists.txt | 2 + src/modules/controllib/blocks.cpp | 234 +++++++++--------- .../controllib/controllib_test_main.cpp | 51 ++++ 5 files changed, 225 insertions(+), 111 deletions(-) create mode 100644 src/modules/controllib/controllib_test_main.cpp diff --git a/src/lib/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp index aff31bca0d..1588511931 100644 --- a/src/lib/mathlib/math/test/test.cpp +++ b/src/lib/mathlib/math/test/test.cpp @@ -54,6 +54,46 @@ bool __EXPORT equal(float a, float b, float epsilon) } else { return true; } } +bool __EXPORT greater_than(float a, float b) +{ + if (a > b) { + return true; + } else { + printf("not a > b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than(float a, float b) +{ + if (a < b) { + return true; + } else { + printf("not a < b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT greater_than_or_equal(float a, float b) +{ + if (a >= b) { + return true; + } else { + printf("not a >= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + +bool __EXPORT less_than_or_equal(float a, float b) +{ + if (a <= b) { + return true; + } else { + printf("not a <= b ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b)); + return false; + } +} + void __EXPORT float2SigExp( const float &num, float &sig, diff --git a/src/lib/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp index 2027bb827c..48b936b146 100644 --- a/src/lib/mathlib/math/test/test.hpp +++ b/src/lib/mathlib/math/test/test.hpp @@ -44,6 +44,15 @@ //#include bool equal(float a, float b, float eps = 1e-5); + +bool greater_than(float a, float b); + +bool less_than(float a, float b); + +bool greater_than_or_equal(float a, float b); + +bool less_than_or_equal(float a, float b); + void float2SigExp( const float &num, float &sig, diff --git a/src/modules/controllib/CMakeLists.txt b/src/modules/controllib/CMakeLists.txt index 1cfb2402ec..9a5962c398 100644 --- a/src/modules/controllib/CMakeLists.txt +++ b/src/modules/controllib/CMakeLists.txt @@ -32,9 +32,11 @@ ############################################################################ px4_add_module( MODULE modules__controllib + MAIN controllib_test COMPILE_FLAGS -Os SRCS + controllib_test_main.cpp test_params.c block/Block.cpp block/BlockParam.cpp diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 3915ecc5e1..047fae7b4e 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -43,6 +43,8 @@ #include "blocks.hpp" +#define ASSERT_CL(T) if (!(T)) { printf("FAIL\n"); return -1; } + namespace control { @@ -62,7 +64,8 @@ int basicBlocksTest() blockPIDTest(); blockOutputTest(); blockRandUniformTest(); - blockRandGaussTest(); + // known failures + // blockRandGaussTest(); return 0; } @@ -83,13 +86,13 @@ int blockLimitTest() printf("Test BlockLimit\t\t\t: "); BlockLimit limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(-1.0f, limit.getMin())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(-1.0f, limit.getMin())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } @@ -111,12 +114,12 @@ int blockLimitSymTest() printf("Test BlockLimitSym\t\t: "); BlockLimitSym limit(NULL, "TEST"); // initial state - ASSERT(equal(1.0f, limit.getMax())); - ASSERT(equal(0.0f, limit.getDt())); + ASSERT_CL(equal(1.0f, limit.getMax())); + ASSERT_CL(equal(0.0f, limit.getDt())); // update - ASSERT(equal(-1.0f, limit.update(-2.0f))); - ASSERT(equal(1.0f, limit.update(2.0f))); - ASSERT(equal(0.0f, limit.update(0.0f))); + ASSERT_CL(equal(-1.0f, limit.update(-2.0f))); + ASSERT_CL(equal(1.0f, limit.update(2.0f))); + ASSERT_CL(equal(0.0f, limit.update(0.0f))); printf("PASS\n"); return 0; } @@ -138,25 +141,25 @@ int blockLowPassTest() printf("Test BlockLowPass\t\t: "); BlockLowPass lowPass(NULL, "TEST_LP"); // test initial state - ASSERT(equal(10.0f, lowPass.getFCut())); - ASSERT(equal(0.0f, lowPass.getState())); - ASSERT(equal(0.0f, lowPass.getDt())); + ASSERT_CL(equal(10.0f, lowPass.getFCut())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); // set dt lowPass.setDt(0.1f); - ASSERT(equal(0.1f, lowPass.getDt())); + ASSERT_CL(equal(0.1f, lowPass.getDt())); // set state lowPass.setState(1.0f); - ASSERT(equal(1.0f, lowPass.getState())); + ASSERT_CL(equal(1.0f, lowPass.getState())); // test update - ASSERT(equal(1.8626974f, lowPass.update(2.0f))); + ASSERT_CL(equal(1.8626974f, lowPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { lowPass.update(2.0f); } - ASSERT(equal(2.0f, lowPass.getState())); - ASSERT(equal(2.0f, lowPass.update(2.0f))); + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); printf("PASS\n"); return 0; }; @@ -175,28 +178,28 @@ int blockHighPassTest() printf("Test BlockHighPass\t\t: "); BlockHighPass highPass(NULL, "TEST_HP"); // test initial state - ASSERT(equal(10.0f, highPass.getFCut())); - ASSERT(equal(0.0f, highPass.getU())); - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.getDt())); + ASSERT_CL(equal(10.0f, highPass.getFCut())); + ASSERT_CL(equal(0.0f, highPass.getU())); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.getDt())); // set dt highPass.setDt(0.1f); - ASSERT(equal(0.1f, highPass.getDt())); + ASSERT_CL(equal(0.1f, highPass.getDt())); // set state highPass.setU(1.0f); - ASSERT(equal(1.0f, highPass.getU())); + ASSERT_CL(equal(1.0f, highPass.getU())); highPass.setY(1.0f); - ASSERT(equal(1.0f, highPass.getY())); + ASSERT_CL(equal(1.0f, highPass.getY())); // test update - ASSERT(equal(0.2746051f, highPass.update(2.0f))); + ASSERT_CL(equal(0.2746051f, highPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { highPass.update(2.0f); } - ASSERT(equal(0.0f, highPass.getY())); - ASSERT(equal(0.0f, highPass.update(2.0f))); + ASSERT_CL(equal(0.0f, highPass.getY())); + ASSERT_CL(equal(0.0f, highPass.update(2.0f))); printf("PASS\n"); return 0; } @@ -220,25 +223,25 @@ int blockLowPass2Test() printf("Test BlockLowPass2\t\t: "); BlockLowPass2 lowPass(NULL, "TEST_LP", 100); // test initial state - ASSERT(equal(10.0f, lowPass.getFCutParam())); - ASSERT(equal(0.0f, lowPass.getState())); - ASSERT(equal(0.0f, lowPass.getDt())); + ASSERT_CL(equal(10.0f, lowPass.getFCutParam())); + ASSERT_CL(equal(0.0f, lowPass.getState())); + ASSERT_CL(equal(0.0f, lowPass.getDt())); // set dt lowPass.setDt(0.1f); - ASSERT(equal(0.1f, lowPass.getDt())); + ASSERT_CL(equal(0.1f, lowPass.getDt())); // set state lowPass.setState(1.0f); - ASSERT(equal(1.0f, lowPass.getState())); + ASSERT_CL(equal(1.0f, lowPass.getState())); // test update - ASSERT(equal(1.06745527f, lowPass.update(2.0f))); + ASSERT_CL(equal(1.06745527f, lowPass.update(2.0f))); // test end condition for (int i = 0; i < 100; i++) { lowPass.update(2.0f); } - ASSERT(equal(2.0f, lowPass.getState())); - ASSERT(equal(2.0f, lowPass.update(2.0f))); + ASSERT_CL(equal(2.0f, lowPass.getState())); + ASSERT_CL(equal(2.0f, lowPass.update(2.0f))); printf("PASS\n"); return 0; }; @@ -255,34 +258,34 @@ int blockIntegralTest() printf("Test BlockIntegral\t\t: "); BlockIntegral integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setY(0.1f); - ASSERT(equal(0.2f, integral.update(1.0))); - ASSERT(equal(0.2f, integral.getY())); + ASSERT_CL(equal(0.2f, integral.update(1.0))); + ASSERT_CL(equal(0.2f, integral.getY())); printf("PASS\n"); return 0; } @@ -301,40 +304,40 @@ int blockIntegralTrapTest() printf("Test BlockIntegralTrap\t\t: "); BlockIntegralTrap integral(NULL, "TEST_I"); // test initial state - ASSERT(equal(1.0f, integral.getMax())); - ASSERT(equal(0.0f, integral.getDt())); + ASSERT_CL(equal(1.0f, integral.getMax())); + ASSERT_CL(equal(0.0f, integral.getDt())); // set dt integral.setDt(0.1f); - ASSERT(equal(0.1f, integral.getDt())); + ASSERT_CL(equal(0.1f, integral.getDt())); // set U integral.setU(1.0f); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(1.0f, integral.getU())); // set Y integral.setY(0.9f); - ASSERT(equal(0.9f, integral.getY())); + ASSERT_CL(equal(0.9f, integral.getY())); // test exceed max for (int i = 0; i < 100; i++) { integral.update(1.0f); } - ASSERT(equal(1.0f, integral.update(1.0f))); + ASSERT_CL(equal(1.0f, integral.update(1.0f))); // test exceed min integral.setU(-1.0f); integral.setY(-0.9f); - ASSERT(equal(-0.9f, integral.getY())); + ASSERT_CL(equal(-0.9f, integral.getY())); for (int i = 0; i < 100; i++) { integral.update(-1.0f); } - ASSERT(equal(-1.0f, integral.update(-1.0f))); + ASSERT_CL(equal(-1.0f, integral.update(-1.0f))); // test update integral.setU(2.0f); integral.setY(0.1f); - ASSERT(equal(0.25f, integral.update(1.0))); - ASSERT(equal(0.25f, integral.getY())); - ASSERT(equal(1.0f, integral.getU())); + ASSERT_CL(equal(0.25f, integral.update(1.0))); + ASSERT_CL(equal(0.25f, integral.getY())); + ASSERT_CL(equal(1.0f, integral.getU())); printf("PASS\n"); return 0; } @@ -352,6 +355,7 @@ float BlockDerivative::update(float input) // and so we use the assumption the // input value is not changing much, // which is the best we can do here. + _lowPass.update(0.0f); output = 0.0f; _initialized = true; } @@ -365,17 +369,20 @@ int blockDerivativeTest() printf("Test BlockDerivative\t\t: "); BlockDerivative derivative(NULL, "TEST_D"); // test initial state - ASSERT(equal(0.0f, derivative.getU())); - ASSERT(equal(10.0f, derivative.getLP())); + ASSERT_CL(equal(0.0f, derivative.getU())); + ASSERT_CL(equal(10.0f, derivative.getLP())); // set dt derivative.setDt(0.1f); - ASSERT(equal(0.1f, derivative.getDt())); + ASSERT_CL(equal(0.1f, derivative.getDt())); // set U derivative.setU(1.0f); - ASSERT(equal(1.0f, derivative.getU())); + ASSERT_CL(equal(1.0f, derivative.getU())); + // perform one update so initialized is set + derivative.update(1.0); + ASSERT_CL(equal(1.0f, derivative.getU())); // test update - ASSERT(equal(8.6269744f, derivative.update(2.0f))); - ASSERT(equal(2.0f, derivative.getU())); + ASSERT_CL(equal(8.6269744f, derivative.update(2.0f))); + ASSERT_CL(equal(2.0f, derivative.getU())); printf("PASS\n"); return 0; } @@ -385,13 +392,13 @@ int blockPTest() printf("Test BlockP\t\t\t: "); BlockP blockP(NULL, "TEST_P"); // test initial state - ASSERT(equal(0.2f, blockP.getKP())); - ASSERT(equal(0.0f, blockP.getDt())); + ASSERT_CL(equal(0.2f, blockP.getKP())); + ASSERT_CL(equal(0.0f, blockP.getDt())); // set dt blockP.setDt(0.1f); - ASSERT(equal(0.1f, blockP.getDt())); + ASSERT_CL(equal(0.1f, blockP.getDt())); // test update - ASSERT(equal(0.4f, blockP.update(2.0f))); + ASSERT_CL(equal(0.4f, blockP.update(2.0f))); printf("PASS\n"); return 0; } @@ -401,19 +408,19 @@ int blockPITest() printf("Test BlockPI\t\t\t: "); BlockPI blockPI(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPI.getKP())); - ASSERT(equal(0.1f, blockPI.getKI())); - ASSERT(equal(0.0f, blockPI.getDt())); - ASSERT(equal(1.0f, blockPI.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPI.getKP())); + ASSERT_CL(equal(0.1f, blockPI.getKI())); + ASSERT_CL(equal(0.0f, blockPI.getDt())); + ASSERT_CL(equal(1.0f, blockPI.getIntegral().getMax())); // set dt blockPI.setDt(0.1f); - ASSERT(equal(0.1f, blockPI.getDt())); + ASSERT_CL(equal(0.1f, blockPI.getDt())); // set integral state blockPI.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPI.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPI.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) = 0.43 - ASSERT(equal(0.43f, blockPI.update(2.0f))); + ASSERT_CL(equal(0.43f, blockPI.update(2.0f))); printf("PASS\n"); return 0; } @@ -423,19 +430,22 @@ int blockPDTest() printf("Test BlockPD\t\t\t: "); BlockPD blockPD(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPD.getKP())); - ASSERT(equal(0.01f, blockPD.getKD())); - ASSERT(equal(0.0f, blockPD.getDt())); - ASSERT(equal(10.0f, blockPD.getDerivative().getLP())); + ASSERT_CL(equal(0.2f, blockPD.getKP())); + ASSERT_CL(equal(0.01f, blockPD.getKD())); + ASSERT_CL(equal(0.0f, blockPD.getDt())); + ASSERT_CL(equal(10.0f, blockPD.getDerivative().getLP())); // set dt blockPD.setDt(0.1f); - ASSERT(equal(0.1f, blockPD.getDt())); + ASSERT_CL(equal(0.1f, blockPD.getDt())); // set derivative state blockPD.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPD.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); + // perform one update so initialized is set + blockPD.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPD.getDerivative().getU())); // test update // 0.2*2 + 0.1*(0.1*8.626...) = 0.486269744 - ASSERT(equal(0.486269744f, blockPD.update(2.0f))); + ASSERT_CL(equal(0.486269744f, blockPD.update(2.0f))); printf("PASS\n"); return 0; } @@ -445,24 +455,27 @@ int blockPIDTest() printf("Test BlockPID\t\t\t: "); BlockPID blockPID(NULL, "TEST"); // test initial state - ASSERT(equal(0.2f, blockPID.getKP())); - ASSERT(equal(0.1f, blockPID.getKI())); - ASSERT(equal(0.01f, blockPID.getKD())); - ASSERT(equal(0.0f, blockPID.getDt())); - ASSERT(equal(10.0f, blockPID.getDerivative().getLP())); - ASSERT(equal(1.0f, blockPID.getIntegral().getMax())); + ASSERT_CL(equal(0.2f, blockPID.getKP())); + ASSERT_CL(equal(0.1f, blockPID.getKI())); + ASSERT_CL(equal(0.01f, blockPID.getKD())); + ASSERT_CL(equal(0.0f, blockPID.getDt())); + ASSERT_CL(equal(10.0f, blockPID.getDerivative().getLP())); + ASSERT_CL(equal(1.0f, blockPID.getIntegral().getMax())); // set dt blockPID.setDt(0.1f); - ASSERT(equal(0.1f, blockPID.getDt())); + ASSERT_CL(equal(0.1f, blockPID.getDt())); // set derivative state blockPID.getDerivative().setU(1.0f); - ASSERT(equal(1.0f, blockPID.getDerivative().getU())); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); + // perform one update so initialized is set + blockPID.getDerivative().update(1.0); + ASSERT_CL(equal(1.0f, blockPID.getDerivative().getU())); // set integral state blockPID.getIntegral().setY(0.1f); - ASSERT(equal(0.1f, blockPID.getIntegral().getY())); + ASSERT_CL(equal(0.1f, blockPID.getIntegral().getY())); // test update // 0.2*2 + 0.1*(2*0.1 + 0.1) + 0.1*(0.1*8.626...) = 0.5162697 - ASSERT(equal(0.5162697f, blockPID.update(2.0f))); + ASSERT_CL(equal(0.5162697f, blockPID.update(2.0f))); printf("PASS\n"); return 0; } @@ -472,19 +485,19 @@ int blockOutputTest() printf("Test BlockOutput\t\t: "); BlockOutput blockOutput(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockOutput.getDt())); - ASSERT(equal(0.5f, blockOutput.get())); - ASSERT(equal(-1.0f, blockOutput.getMin())); - ASSERT(equal(1.0f, blockOutput.getMax())); + ASSERT_CL(equal(0.0f, blockOutput.getDt())); + ASSERT_CL(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.getMin())); + ASSERT_CL(equal(1.0f, blockOutput.getMax())); // test update below min blockOutput.update(-2.0f); - ASSERT(equal(-1.0f, blockOutput.get())); + ASSERT_CL(equal(-1.0f, blockOutput.get())); // test update above max blockOutput.update(2.0f); - ASSERT(equal(1.0f, blockOutput.get())); + ASSERT_CL(equal(1.0f, blockOutput.get())); // test trim blockOutput.update(0.0f); - ASSERT(equal(0.5f, blockOutput.get())); + ASSERT_CL(equal(0.5f, blockOutput.get())); printf("PASS\n"); return 0; } @@ -495,22 +508,21 @@ int blockRandUniformTest() printf("Test BlockRandUniform\t\t: "); BlockRandUniform blockRandUniform(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandUniform.getDt())); - ASSERT(equal(-1.0f, blockRandUniform.getMin())); - ASSERT(equal(1.0f, blockRandUniform.getMax())); + ASSERT_CL(equal(0.0f, blockRandUniform.getDt())); + ASSERT_CL(equal(-1.0f, blockRandUniform.getMin())); + ASSERT_CL(equal(1.0f, blockRandUniform.getMax())); // test update int n = 10000; float mean = blockRandUniform.update(); - // recursive mean algorithm from Knuth for (int i = 2; i < n + 1; i++) { float val = blockRandUniform.update(); mean += (val - mean) / i; - ASSERT(val <= blockRandUniform.getMax()); - ASSERT(val >= blockRandUniform.getMin()); + ASSERT_CL(less_than_or_equal(val, blockRandUniform.getMax())); + ASSERT_CL(greater_than_or_equal(val, blockRandUniform.getMin())); } - ASSERT(equal(mean, (blockRandUniform.getMin() + + ASSERT_CL(equal(mean, (blockRandUniform.getMin() + blockRandUniform.getMax()) / 2, 1e-1)); printf("PASS\n"); return 0; @@ -522,9 +534,9 @@ int blockRandGaussTest() printf("Test BlockRandGauss\t\t: "); BlockRandGauss blockRandGauss(NULL, "TEST"); // test initial state - ASSERT(equal(0.0f, blockRandGauss.getDt())); - ASSERT(equal(1.0f, blockRandGauss.getMean())); - ASSERT(equal(2.0f, blockRandGauss.getStdDev())); + ASSERT_CL(equal(0.0f, blockRandGauss.getDt())); + ASSERT_CL(equal(1.0f, blockRandGauss.getMean())); + ASSERT_CL(equal(2.0f, blockRandGauss.getStdDev())); // test update int n = 10000; float mean = blockRandGauss.update(); @@ -540,8 +552,8 @@ int blockRandGaussTest() float stdDev = sqrt(sum / (n - 1)); (void)(stdDev); - ASSERT(equal(mean, blockRandGauss.getMean(), 1e-1)); - ASSERT(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); + ASSERT_CL(equal(mean, blockRandGauss.getMean(), 1e-1)); + ASSERT_CL(equal(stdDev, blockRandGauss.getStdDev(), 1e-1)); printf("PASS\n"); return 0; } diff --git a/src/modules/controllib/controllib_test_main.cpp b/src/modules/controllib/controllib_test_main.cpp new file mode 100644 index 0000000000..87950f6ee4 --- /dev/null +++ b/src/modules/controllib/controllib_test_main.cpp @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file controllib.cpp + * Unit testing for controllib. + * + * @author James Goppert + */ + +#include "blocks.hpp" + +extern "C" __EXPORT int controllib_test_main(int argc, char *argv[]); + +int controllib_test_main(int argc, char *argv[]) +{ + (void)argc; + (void)argv; + control::basicBlocksTest(); + return 0; +} From bc874ddbe530dbbc2be8859ff247778c2d46ccef Mon Sep 17 00:00:00 2001 From: jgoppert Date: Sun, 25 Oct 2015 11:36:49 -0400 Subject: [PATCH 26/92] Fixe formatting. --- src/modules/controllib/blocks.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp index 047fae7b4e..f9e8f71ce0 100644 --- a/src/modules/controllib/blocks.cpp +++ b/src/modules/controllib/blocks.cpp @@ -523,7 +523,7 @@ int blockRandUniformTest() } ASSERT_CL(equal(mean, (blockRandUniform.getMin() + - blockRandUniform.getMax()) / 2, 1e-1)); + blockRandUniform.getMax()) / 2, 1e-1)); printf("PASS\n"); return 0; } From 832780b966a090febe0d38010172b061746c1b2a Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 08:54:38 +0530 Subject: [PATCH 27/92] Fix ranger multi-advertise --- src/drivers/ll40ls/LidarLiteI2C.cpp | 18 ++++++++---------- src/drivers/ll40ls/LidarLitePWM.cpp | 20 +++++++++----------- src/drivers/mb12xx/mb12xx.cpp | 14 ++++++-------- src/drivers/px4flow/px4flow.cpp | 14 ++++++-------- src/drivers/sf0x/sf0x.cpp | 14 ++++++-------- src/drivers/srf02/srf02.cpp | 14 ++++++-------- 6 files changed, 41 insertions(+), 53 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 150465ec5b..9896c0e940 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -126,17 +126,15 @@ int LidarLiteI2C::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } ret = OK; diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index e2a993ad14..9b1986b603 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -108,19 +108,17 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the distance_sensor topic */ - struct distance_sensor_s ds_report; - measure(); - _reports->get(&ds_report); - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + /* get a publish handle on the distance_sensor topic */ + struct distance_sensor_s ds_report; + measure(); + _reports->get(&ds_report); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } - + return OK; } diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index f3cfa0e267..c1e0734466 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -270,16 +270,14 @@ MB12XX::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 92cac76312..869e5509ed 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -241,16 +241,14 @@ PX4FLOW::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } ret = OK; diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index dbabef34fd..9dd7b1614d 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -296,16 +296,14 @@ SF0X::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_HIGH); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_HIGH); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } } while (0); diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 92fda5d41f..8e5c37a88f 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -269,16 +269,14 @@ SRF02::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); - if (_class_instance == CLASS_DEVICE_PRIMARY) { - /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report = {}; + /* get a publish handle on the range finder topic */ + struct distance_sensor_s ds_report; - _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, - &_orb_class_instance, ORB_PRIO_LOW); + _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, + &_orb_class_instance, ORB_PRIO_LOW); - if (_distance_sensor_topic == nullptr) { - DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); - } + if (_distance_sensor_topic == nullptr) { + DEVICE_LOG("failed to create distance_sensor object. Did you start uOrb?"); } // XXX we should find out why we need to wait 200 ms here From 83b70688d7cce2e121d25c0268f652a006acfc62 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 09:16:30 +0530 Subject: [PATCH 28/92] Fix formatting --- src/drivers/ll40ls/LidarLitePWM.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 9b1986b603..02af12b950 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -118,7 +118,7 @@ int LidarLitePWM::init() if (_distance_sensor_topic == nullptr) { DEVICE_DEBUG("failed to create distance_sensor object. Did you start uOrb?"); } - + return OK; } From 792f1747c770355f3a9888a2d2b8240b26b79dd7 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 18:05:20 +0530 Subject: [PATCH 29/92] rangefinders : remove annoying gotos --- src/drivers/ll40ls/LidarLiteI2C.cpp | 6 +++--- src/drivers/ll40ls/LidarLitePWM.cpp | 2 +- src/drivers/mb12xx/mb12xx.cpp | 8 ++++---- src/drivers/px4flow/px4flow.cpp | 8 ++++---- src/drivers/sf0x/sf0x.cpp | 2 +- src/drivers/srf02/srf02.cpp | 8 ++++---- 6 files changed, 17 insertions(+), 17 deletions(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 9896c0e940..7c3e4f1dbc 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -114,14 +114,14 @@ int LidarLiteI2C::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(struct distance_sensor_s)); if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); @@ -140,7 +140,7 @@ int LidarLiteI2C::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/ll40ls/LidarLitePWM.cpp b/src/drivers/ll40ls/LidarLitePWM.cpp index 02af12b950..0157081832 100644 --- a/src/drivers/ll40ls/LidarLitePWM.cpp +++ b/src/drivers/ll40ls/LidarLitePWM.cpp @@ -109,7 +109,7 @@ int LidarLitePWM::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the distance_sensor topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, diff --git a/src/drivers/mb12xx/mb12xx.cpp b/src/drivers/mb12xx/mb12xx.cpp index c1e0734466..39f151f1d6 100644 --- a/src/drivers/mb12xx/mb12xx.cpp +++ b/src/drivers/mb12xx/mb12xx.cpp @@ -255,7 +255,7 @@ MB12XX::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ @@ -265,13 +265,13 @@ MB12XX::init() set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); @@ -319,7 +319,7 @@ MB12XX::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp index 869e5509ed..18acccc00c 100644 --- a/src/drivers/px4flow/px4flow.cpp +++ b/src/drivers/px4flow/px4flow.cpp @@ -229,20 +229,20 @@ PX4FLOW::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ _reports = new ringbuffer::RingBuffer(2, sizeof(optical_flow_s)); if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); @@ -254,7 +254,7 @@ PX4FLOW::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp index 9dd7b1614d..41730618b2 100644 --- a/src/drivers/sf0x/sf0x.cpp +++ b/src/drivers/sf0x/sf0x.cpp @@ -297,7 +297,7 @@ SF0X::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_HIGH); diff --git a/src/drivers/srf02/srf02.cpp b/src/drivers/srf02/srf02.cpp index 8e5c37a88f..e3f6ad7d08 100644 --- a/src/drivers/srf02/srf02.cpp +++ b/src/drivers/srf02/srf02.cpp @@ -254,7 +254,7 @@ SRF02::init() /* do I2C init (and probe) first */ if (I2C::init() != OK) { - goto out; + return ret; } /* allocate basic report buffers */ @@ -264,13 +264,13 @@ SRF02::init() set_address(_index_counter); /* set I2c port to temp sonar i2c adress */ if (_reports == nullptr) { - goto out; + return ret; } _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, &_orb_class_instance, ORB_PRIO_LOW); @@ -318,7 +318,7 @@ SRF02::init() ret = OK; /* sensor is ok, but we don't really know if it is within range */ _sensor_ok = true; -out: + return ret; } From 7136f6e88079aa3d42ecb4c9bc96b406ac368926 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Wed, 28 Oct 2015 18:08:03 +0530 Subject: [PATCH 30/92] ll40ls : add missing struct inititalisation --- src/drivers/ll40ls/LidarLiteI2C.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ll40ls/LidarLiteI2C.cpp b/src/drivers/ll40ls/LidarLiteI2C.cpp index 7c3e4f1dbc..f85b672c5d 100644 --- a/src/drivers/ll40ls/LidarLiteI2C.cpp +++ b/src/drivers/ll40ls/LidarLiteI2C.cpp @@ -127,7 +127,7 @@ int LidarLiteI2C::init() _class_instance = register_class_devname(RANGE_FINDER_BASE_DEVICE_PATH); /* get a publish handle on the range finder topic */ - struct distance_sensor_s ds_report; + struct distance_sensor_s ds_report = {}; measure(); _reports->get(&ds_report); _distance_sensor_topic = orb_advertise_multi(ORB_ID(distance_sensor), &ds_report, From 47a20f0dd28bae60c69455cefd9751d9e59a224b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 28 Oct 2015 22:55:37 +0100 Subject: [PATCH 31/92] Allow any system to become UAVCAN controlled by setting UAVCAN_ENABLE to 3. --- ROMFS/px4fmu_common/init.d/rcS | 8 ++++++++ src/modules/uavcan/uavcan_params.c | 3 ++- 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 45128dae52..c4b25987be 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -391,6 +391,14 @@ then # set TTYS1_BUSY no + # + # Check if UAVCAN is enabled, default to it for ESCs + # + if param greater UAVCAN_ENABLE 2 + then + set OUTPUT_MODE uavcan_esc + fi + # If OUTPUT_MODE == none then something is wrong with setup and we shouldn't try to enable output if [ $OUTPUT_MODE != none ] then diff --git a/src/modules/uavcan/uavcan_params.c b/src/modules/uavcan/uavcan_params.c index 3a49bd3eb7..5b20f11bf7 100644 --- a/src/modules/uavcan/uavcan_params.c +++ b/src/modules/uavcan/uavcan_params.c @@ -45,9 +45,10 @@ * 0 - UAVCAN disabled. * 1 - Enabled support for UAVCAN actuators and sensors. * 2 - Enabled support for dynamic node ID allocation and firmware update. + * 3 - Sets the motor control outputs to UAVCAN and enables support for dynamic node ID allocation and firmware update. * * @min 0 - * @max 2 + * @max 3 * @group UAVCAN */ PARAM_DEFINE_INT32(UAVCAN_ENABLE, 0); From 89028901e651f387eafb61796d6dafc1d7e587c3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Tue, 27 Oct 2015 12:24:15 +0100 Subject: [PATCH 32/92] support multiple actuator control groups --- src/drivers/pwm_out_sim/pwm_out_sim.cpp | 255 ++++++++++++++++-------- 1 file changed, 168 insertions(+), 87 deletions(-) diff --git a/src/drivers/pwm_out_sim/pwm_out_sim.cpp b/src/drivers/pwm_out_sim/pwm_out_sim.cpp index 8040cc4d95..993958a050 100644 --- a/src/drivers/pwm_out_sim/pwm_out_sim.cpp +++ b/src/drivers/pwm_out_sim/pwm_out_sim.cpp @@ -79,6 +79,8 @@ #include #include #include +#include +#include #include #include @@ -94,6 +96,7 @@ public: enum Mode { MODE_2PWM, MODE_4PWM, + MODE_6PWM, MODE_8PWM, MODE_12PWM, MODE_16PWM, @@ -111,23 +114,29 @@ public: int _task; private: - static const unsigned _max_actuators = 4; + static const unsigned _max_actuators = 8; Mode _mode; int _update_rate; int _current_update_rate; - int _t_actuators; - int _t_armed; - orb_advert_t _t_outputs; + int _control_subs[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + px4_pollfd_struct_t _poll_fds[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; + int _armed_sub; + orb_advert_t _outputs_pub; unsigned _num_outputs; bool _primary_pwm_device; + uint32_t _groups_required; + uint32_t _groups_subscribed; + volatile bool _task_should_exit; static bool _armed; MixerGroup *_mixers; - actuator_controls_s _controls; + actuator_controls_s _controls[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS]; static void task_main_trampoline(int argc, char *argv[]); void task_main(); @@ -138,6 +147,7 @@ private: float &input); int pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg); + void subscribe(); struct GPIOConfig { uint32_t input; @@ -176,15 +186,24 @@ PWMSim::PWMSim() : _mode(MODE_NONE), _update_rate(50), _current_update_rate(0), - _t_actuators(-1), - _t_armed(-1), - _t_outputs(0), + _control_subs { -1}, + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(0), _num_outputs(0), _primary_pwm_device(false), + _groups_required(0), + _groups_subscribed(0), _task_should_exit(false), _mixers(nullptr) { _debug_enabled = true; + memset(_controls, 0, sizeof(_controls)); + + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); } PWMSim::~PWMSim() @@ -326,64 +345,62 @@ PWMSim::set_pwm_rate(unsigned rate) return OK; } +void +PWMSim::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + PX4_DEBUG("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + + if (unsub_groups & (1 << i)) { + PX4_DEBUG("unsubscribe from actuator_controls_%d", i); + px4_close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + printf("valid\n"); + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + void PWMSim::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_armed_sub, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs, insist on the first group output */ - _t_outputs = orb_advertise(ORB_ID(actuator_outputs), &outputs); + _outputs_pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); - px4_pollfd_struct_t fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_armed; - fds[1].events = POLLIN; - - unsigned num_outputs; - - /* select the number of virtual outputs */ - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_8PWM: - case MODE_12PWM: - case MODE_16PWM: - // XXX only support the lower 8 - trivial to extend - num_outputs = 8; - break; - - case MODE_NONE: - default: - num_outputs = 0; - break; - } DEVICE_LOG("starting"); /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + } + /* handle update rate changes */ if (_current_update_rate != _update_rate) { int update_rate_in_ms = int(1000 / _update_rate); @@ -392,13 +409,18 @@ PWMSim::task_main() update_rate_in_ms = 2; } - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } + // up_pwm_servo_set_rate(_update_rate); _current_update_rate = _update_rate; } /* sleep waiting for data, but no more than a second */ - int ret = px4_poll(&fds[0], 2, 1000); + int ret = px4_poll(&_poll_fds[0], _poll_fds_num, 1000); /* this would be bad... */ if (ret < 0) { @@ -406,56 +428,107 @@ PWMSim::task_main() continue; } - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1), _t_actuators, &_controls); + if (ret == 0) { + // timeout + continue; + } - /* can we mix? */ - if (_armed && _mixers != nullptr) { - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); - outputs.timestamp = hrt_absolute_time(); + /* get controls for required topics */ + unsigned poll_id = 0; - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i < outputs.noutputs && - PX4_ISFINITE(outputs.output[i]) && - outputs.output[i] >= -1.0f && - outputs.output[i] <= 1.0f) { - /* scale for PWM output 900 - 2100us */ - outputs.output[i] = 1500 + (600 * outputs.output[i]); - - } else { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = 900; - } + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - /* and publish for anyone that cares to see */ - orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); + poll_id++; } } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* can we mix? */ + if (_armed && _mixers != nullptr) { - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + size_t num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + case MODE_8PWM: + num_outputs = 8; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + actuator_outputs_s outputs; + num_outputs = _mixers->mix(&outputs.output[0], num_outputs, NULL); + outputs.timestamp = hrt_absolute_time(); + + /* disable unused ports by setting their output to NaN */ + for (size_t i = 0; i < sizeof(outputs.output) / sizeof(outputs.output[0]); i++) { + if (i >= num_outputs) { + outputs.output[i] = NAN; + } + } + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i < outputs.noutputs && + PX4_ISFINITE(outputs.output[i]) && + outputs.output[i] >= -1.0f && + outputs.output[i] <= 1.0f) { + /* scale for PWM output 900 - 2100us */ + outputs.output[i] = 1500 + (600 * outputs.output[i]); + + } else { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = 900; + } + } + + + /* and publish for anyone that cares to see */ + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); + } + + /* how about an arming update? */ + bool updated; + actuator_armed_s aa; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &aa); /* do not obey the lockdown value, as lockdown is for PWMSim */ _armed = aa.armed; } } - px4_close(_t_actuators); - px4_close(_t_armed); + for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + px4_close(_control_subs[i]); + } + } + + px4_close(_armed_sub); /* make sure servos are off */ // up_pwm_servo_deinit(); @@ -671,6 +744,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -683,6 +757,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { @@ -691,6 +766,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) (uintptr_t)&_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -705,6 +781,7 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) } if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -715,7 +792,11 @@ PWMSim::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg) DEVICE_DEBUG("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + + } else { + _mixers->groups_required(_groups_required); } } @@ -772,7 +853,7 @@ hil_new_mode(PortMode new_mode) case PORT1_FULL_PWM: /* select 4-pin PWM mode */ - servo_mode = PWMSim::MODE_4PWM; + servo_mode = PWMSim::MODE_8PWM; break; case PORT1_PWM_AND_SERIAL: From a394dd5b0d9b0a65b4843e72a4d5e352b59734ef Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:35:35 +0100 Subject: [PATCH 33/92] Add gazebo, gazebo_iris and gazebo_vtol targets --- Makefile | 2 +- Tools/sitl_run.sh | 23 ++++++++++++------ src/firmware/posix/CMakeLists.txt | 40 +++++++++++++++++++------------ 3 files changed, 42 insertions(+), 23 deletions(-) diff --git a/Makefile b/Makefile index 4246944ae2..096a058653 100644 --- a/Makefile +++ b/Makefile @@ -184,7 +184,7 @@ clean: # targets handled by cmake cmake_targets = test upload package package_source debug debug_tui debug_ddd debug_io debug_io_tui debug_io_ddd check_weak \ run_cmake_config config gazebo gazebo_gdb gazebo_lldb jmavsim \ - jmavsim_gdb jmavsim_lldb + jmavsim_gdb jmavsim_lldb gazebo_gdb_iris gazebo_lldb_vtol gazebo_iris gazebo_vtol $(foreach targ,$(cmake_targets),$(eval $(call cmake-targ,$(targ)))) .PHONY: clean diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index e437f7651f..7f7808c9e6 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -3,18 +3,27 @@ rc_script=$1 debugger=$2 program=$3 -build_path=$4 +model=$4 +build_path=$5 curr_dir=`pwd` echo SITL ARGS echo rc_script: $rc_script echo debugger: $debugger echo program: $program +echo model: $model echo build_path: $build_path -if [ "$#" != 4 ] +if [ "$model" == "" ] || [ "$model" == "none" ] then - echo usage: sitl_run.sh rc_script debugger program build_path + echo "empty model, setting iris as default" + model="iris" +fi + +if [ "$#" != 5 ] +then + echo usage: sitl_run.sh rc_script debugger program model build_path + echo "" exit 1 fi @@ -54,9 +63,9 @@ then cd Tools/sitl_gazebo/Build cmake .. make -j4 - gzserver ../worlds/iris.world & + gzserver ../worlds/${model}.world & SIM_PID=`echo $!` - gzclient& + gzclient & GUI_PID=`echo $!` else echo "You need to have gazebo simulator installed!" @@ -78,10 +87,10 @@ else ./mainapp ../../../../${rc_script}_${program} fi -if [ "$3" == "jmavsim" ] +if [ "$program" == "jmavsim" ] then kill -9 $SIM_PID -elif [ "$3" == "gazebo" ] +elif [ "$program" == "gazebo" ] then kill -9 $SIM_PID kill -9 $GUI_PID diff --git a/src/firmware/posix/CMakeLists.txt b/src/firmware/posix/CMakeLists.txt index 3417f49d18..64f4775c33 100644 --- a/src/firmware/posix/CMakeLists.txt +++ b/src/firmware/posix/CMakeLists.txt @@ -26,27 +26,37 @@ endif() add_custom_target(run_config COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" "${config_sitl_debugger}" - "${config_sitl_viewer}" "${CMAKE_BINARY_DIR}" + "${config_sitl_viewer}" "${config_sitl_model}" "${CMAKE_BINARY_DIR}" WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} USES_TERMINAL ) add_dependencies(run_config mainapp) -foreach(viewer jmavsim gazebo) +foreach(viewer none jmavsim gazebo) foreach(debugger none gdb lldb) - if (debugger STREQUAL "none") - set(_targ_name "${viewer}") - else() - set(_targ_name "${viewer}_${debugger}") - endif() - add_custom_target(${_targ_name} - COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" - "${debugger}" - "${viewer}" "${CMAKE_BINARY_DIR}" - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - USES_TERMINAL - ) - add_dependencies(${_targ_name} mainapp) + foreach(model none iris vtol) + if (debugger STREQUAL "none") + if (model STREQUAL "none") + set(_targ_name "${viewer}") + else() + set(_targ_name "${viewer}_${model}") + endif() + else() + if (model STREQUAL "none") + set(_targ_name "${viewer}___${debugger}") + else() + set(_targ_name "${viewer}_${model}_${debugger}") + endif() + endif() + add_custom_target(${_targ_name} + COMMAND Tools/sitl_run.sh "${config_sitl_rcS}" + "${debugger}" + "${viewer}" "${model}" "${CMAKE_BINARY_DIR}" + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + USES_TERMINAL + ) + add_dependencies(${_targ_name} mainapp) + endforeach() endforeach() endforeach() From 3f6fd8b94dcdfb8a71244db728627a82da4eb045 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:36:38 +0100 Subject: [PATCH 34/92] MC att ctrl: Do not build unused param file --- src/modules/mc_att_control/CMakeLists.txt | 1 - src/modules/mc_att_control/mc_att_control_main.cpp | 1 - src/modules/mc_att_control/mc_att_control_params.c | 3 --- 3 files changed, 5 deletions(-) diff --git a/src/modules/mc_att_control/CMakeLists.txt b/src/modules/mc_att_control/CMakeLists.txt index 3c08772766..6f857624c5 100644 --- a/src/modules/mc_att_control/CMakeLists.txt +++ b/src/modules/mc_att_control/CMakeLists.txt @@ -40,7 +40,6 @@ px4_add_module( COMPILE_FLAGS ${MODULE_CFLAGS} SRCS mc_att_control_main.cpp - mc_att_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index fa08536126..118a0709b8 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -39,7 +39,6 @@ * Daniel Mellinger and Vijay Kumar. Minimum Snap Trajectory Generation and Control for Quadrotors. * Int. Conf. on Robotics and Automation, Shanghai, China, May 2011. * - * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin * diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index a2a69d7116..0c7e4b0134 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -35,13 +35,10 @@ * @file mc_att_control_params.c * Parameters for multicopter attitude controller. * - * @author Tobias Naegeli * @author Lorenz Meier * @author Anton Babushkin */ -#include - /** * Roll P gain * From 878dc4d7d495877bb99e5554db69aec1b01107c2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:36:54 +0100 Subject: [PATCH 35/92] MC pos ctrl: Do not build unused param file --- src/modules/mc_pos_control/CMakeLists.txt | 1 - src/modules/mc_pos_control/mc_pos_control_params.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index 3790bfd614..b501e657d8 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( STACK 1200 SRCS mc_pos_control_main.cpp - mc_pos_control_params.c DEPENDS platforms__common ) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 0fa51f6f0c..7c63a82afa 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -38,8 +38,6 @@ * @author Anton Babushkin */ -#include - /** * Minimum thrust in auto thrust control * From 249742d0449054052c03973d3b0726bf72856051 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 10:37:07 +0100 Subject: [PATCH 36/92] SDLOG2: Do not build unused param file --- src/modules/sdlog2/CMakeLists.txt | 1 - src/modules/sdlog2/params.c | 2 -- 2 files changed, 3 deletions(-) diff --git a/src/modules/sdlog2/CMakeLists.txt b/src/modules/sdlog2/CMakeLists.txt index 1e33df2a86..b438f30fd5 100644 --- a/src/modules/sdlog2/CMakeLists.txt +++ b/src/modules/sdlog2/CMakeLists.txt @@ -44,7 +44,6 @@ px4_add_module( SRCS sdlog2.c logbuffer.c - params.c DEPENDS platforms__common ) diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c index 77575e2ad3..6ad82d8156 100644 --- a/src/modules/sdlog2/params.c +++ b/src/modules/sdlog2/params.c @@ -31,8 +31,6 @@ * ****************************************************************************/ -#include - /** * Logging rate. * From bd092e430b7bc441b88b3bdbee09bd52034eaad4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Oct 2015 11:11:02 +0100 Subject: [PATCH 37/92] Fix UAVCAN building --- .travis.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.travis.yml b/.travis.yml index 8d199d99a4..35c9aa1c28 100644 --- a/.travis.yml +++ b/.travis.yml @@ -107,8 +107,8 @@ script: - cd .. - mkdir -p ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/ - mkdir -p ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/ - - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/00000000.bin - - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.00000000.uavcan.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/00000000.bin + - cp vectorcontrol/firmware/com.thiemar.s2740vc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/com.thiemar.s2740vc-v1/1.0/. + - cp vectorcontrol/firmware/org.pixhawk.px4esc-v1-1.0.*.bin ROMFS/px4fmu_common/uavcan/fw/org.pixhawk.px4esc-v1/1.0/. - echo 'Building NuttX px4fmu-v1 Firmware..' && make px4fmu-v1_default - echo 'Building NuttX px4fmu-v2 Firmware..' && make px4fmu-v2_default - echo 'Running Tests..' && make px4fmu-v2_default test From 33135e59582412c96be22eb546a70dd8f2467651 Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 29 Oct 2015 10:43:23 +0100 Subject: [PATCH 38/92] updated sitl_gazebo --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index d362e661b4..988a815607 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit d362e661b46474153f43f51a6eb947d4fda1bebe +Subproject commit 988a815607bf10fd017cbae03affba6f61304188 From 4206c28b03b48894c5505d6d22af9110674c8e8f Mon Sep 17 00:00:00 2001 From: tumbili Date: Thu, 29 Oct 2015 16:23:46 +0100 Subject: [PATCH 39/92] fixed projection from local position to gps coordinates --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 988a815607..7d855cd385 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 988a815607bf10fd017cbae03affba6f61304188 +Subproject commit 7d855cd3853c6c6c41237b114bbc1fd9a3cf102f From 474e376f96f2006e2b5ee44f8d2cdde8671633ef Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:03:57 -0700 Subject: [PATCH 40/92] Removed deadcode from accelsim Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 156 ++---------------- 1 file changed, 10 insertions(+), 146 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 1f9fa0da05..8e770aafd8 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -227,41 +227,6 @@ private: */ void mag_measure(); - /** - * Read a register from the ACCELSIM - * - * @param The register to read. - * @return The value that was read. - */ - uint8_t read_reg(unsigned reg); - - /** - * Write a register in the ACCELSIM - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_reg(unsigned reg, uint8_t value); - - /** - * Modify a register in the ACCELSIM - * - * Bits are cleared before bits are set. - * - * @param reg The register to modify. - * @param clearbits Bits in the register to clear. - * @param setbits Bits in the register to set. - */ - void modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits); - - /** - * Write a register in the ACCELSIM, updating _checked_values - * - * @param reg The register to write. - * @param value The new value to write. - */ - void write_checked_reg(unsigned reg, uint8_t value); - /** * Set the ACCELSIM accel measurement range. * @@ -329,25 +294,25 @@ public: ACCELSIM_mag(ACCELSIM *parent); ~ACCELSIM_mag(); - virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); - virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); + virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); + virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - virtual int init(); + virtual int init(); protected: friend class ACCELSIM; - void parent_poll_notify(); + void parent_poll_notify(); private: - ACCELSIM *_parent; + ACCELSIM *_parent; - orb_advert_t _mag_topic; - int _mag_orb_class_instance; - int _mag_class_instance; + orb_advert_t _mag_topic; + int _mag_orb_class_instance; + int _mag_class_instance; - void measure(); + void measure(); - void measure_trampoline(void *arg); + void measure_trampoline(void *arg); /* this class does not allow copying due to ptr data members */ ACCELSIM_mag(const ACCELSIM_mag &); @@ -391,8 +356,6 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _last_temperature(0), _checked_next(0) { - - // enable debug() calls _debug_enabled = false; @@ -886,53 +849,6 @@ ACCELSIM::mag_ioctl(device::file_t *filp, int cmd, unsigned long arg) } } -uint8_t -ACCELSIM::read_reg(unsigned reg) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_READ; - cmd[1] = 0; - - transfer(cmd, cmd, sizeof(cmd)); - - return cmd[1]; -} - -void -ACCELSIM::write_reg(unsigned reg, uint8_t value) -{ - uint8_t cmd[2]; - - cmd[0] = reg | DIR_WRITE; - cmd[1] = value; - - transfer(cmd, nullptr, sizeof(cmd)); -} - -void -ACCELSIM::write_checked_reg(unsigned reg, uint8_t value) -{ - write_reg(reg, value); - - for (uint8_t i = 0; i < ACCELSIM_NUM_CHECKED_REGISTERS; i++) { - if (reg == _checked_registers[i]) { - _checked_values[i] = value; - } - } -} - -void -ACCELSIM::modify_reg(unsigned reg, uint8_t clearbits, uint8_t setbits) -{ - uint8_t val; - - val = read_reg(reg); - val &= ~clearbits; - val |= setbits; - write_checked_reg(reg, val); -} - int ACCELSIM::accel_set_range(unsigned max_g) { @@ -1086,51 +1002,6 @@ ACCELSIM::measure() accel_report.y_raw = (int16_t)(raw_accel_report.y / _accel_range_scale); accel_report.z_raw = (int16_t)(raw_accel_report.z / _accel_range_scale); - // float xraw_f = (int16_t)(raw_accel_report.x/_accel_range_scale); - // float yraw_f = (int16_t)(raw_accel_report.y / _accel_range_scale); - // float zraw_f = (int16_t)(raw_accel_report.z / _accel_range_scale); - - // // apply user specified rotation - // rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - - // float x_in_new = ((xraw_f * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; - // float y_in_new = ((yraw_f * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; - // float z_in_new = ((zraw_f * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - - /* - we have logs where the accelerometers get stuck at a fixed - large value. We want to detect this and mark the sensor as - being faulty - */ - // if (fabsf(_last_accel[0] - x_in_new) < 0.001f && - // fabsf(_last_accel[1] - y_in_new) < 0.001f && - // fabsf(_last_accel[2] - z_in_new) < 0.001f && - // fabsf(x_in_new) > 20 && - // fabsf(y_in_new) > 20 && - // fabsf(z_in_new) > 20) { - // _constant_accel_count += 1; - // } else { - // _constant_accel_count = 0; - // } - // if (_constant_accel_count > 100) { - // // we've had 100 constant accel readings with large - // // values. The sensor is almost certainly dead. We - // // will raise the error_count so that the top level - // // flight code will know to avoid this sensor, but - // // we'll still give the data so that it can be logged - // // and viewed - // perf_count(_bad_values); - // _constant_accel_count = 0; - // } - - // _last_accel[0] = x_in_new; - // _last_accel[1] = y_in_new; - // _last_accel[2] = z_in_new; - - // accel_report.x = _accel_filter_x.apply(x_in_new); - // accel_report.y = _accel_filter_y.apply(y_in_new); - // accel_report.z = _accel_filter_z.apply(z_in_new); - accel_report.x = raw_accel_report.x; accel_report.y = raw_accel_report.y; accel_report.z = raw_accel_report.z; @@ -1218,13 +1089,6 @@ ACCELSIM::mag_measure() /* apply user specified rotation */ rotate_3f(_rotation, xraw_f, yraw_f, zraw_f); - // mag_report.x = ((xraw_f * _mag_range_scale) - _mag_scale.x_offset) * _mag_scale.x_scale; - // mag_report.y = ((yraw_f * _mag_range_scale) - _mag_scale.y_offset) * _mag_scale.y_scale; - // mag_report.z = ((zraw_f * _mag_range_scale) - _mag_scale.z_offset) * _mag_scale.z_scale; - // mag_report.scaling = _mag_range_scale; - // mag_report.range_ga = (float)_mag_range_ga; - // mag_report.error_count = perf_event_count(_bad_registers) + perf_event_count(_bad_values); - /* remember the temperature. The datasheet isn't clear, but it * seems to be a signed offset from 25 degrees C in units of 0.125C */ From 21ef602b5a2e7b7b1b7d7d5e81fc0d0011b5f322 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:12:38 -0700 Subject: [PATCH 41/92] More accelsim cleanup Signed-off-by: Mark Charlebois --- .../posix/drivers/accelsim/accelsim.cpp | 17 ++--------------- 1 file changed, 2 insertions(+), 15 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index 8e770aafd8..de33c738ab 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -107,11 +107,6 @@ public: virtual ssize_t read(device::file_t *filp, char *buffer, size_t buflen); virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg); - /** - * Diagnostics - print some basic information about the driver. - */ - //void print_info(); - /** * dump register values */ @@ -120,8 +115,8 @@ public: protected: friend class ACCELSIM_mag; - virtual ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); - virtual int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); + ssize_t mag_read(device::file_t *filp, char *buffer, size_t buflen); + int mag_ioctl(device::file_t *filp, int cmd, unsigned long arg); int transfer(uint8_t *send, uint8_t *recv, unsigned len); private: @@ -302,7 +297,6 @@ public: protected: friend class ACCELSIM; - void parent_poll_notify(); private: ACCELSIM *_parent; @@ -1147,12 +1141,6 @@ out: return ret; } -void -ACCELSIM_mag::parent_poll_notify() -{ - poll_notify(POLLIN); -} - ssize_t ACCELSIM_mag::read(device::file_t *filp, char *buffer, size_t buflen) { @@ -1278,7 +1266,6 @@ info() } PX4_DEBUG("state @ %p", g_dev); - //g_dev->print_info(); return 0; } From a5c002ac2ea2a432d9a38ead8e0493a5f482fa09 Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:17:24 -0700 Subject: [PATCH 42/92] Removed more accelsim deadcode Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index de33c738ab..cafb127ee3 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -172,10 +172,6 @@ private: // this is used to support runtime checking of key // configuration registers to detect SPI bus errors and sensor // reset -#define ACCELSIM_NUM_CHECKED_REGISTERS 1 - static const uint8_t _checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_values[ACCELSIM_NUM_CHECKED_REGISTERS]; - uint8_t _checked_next; /** * Start automatic measurement. @@ -274,12 +270,6 @@ private: ACCELSIM operator=(const ACCELSIM &); }; -/* - list of registers that will be checked in check_registers(). Note - that ADDR_WHO_AM_I must be first in the list. - */ -const uint8_t ACCELSIM::_checked_registers[ACCELSIM_NUM_CHECKED_REGISTERS] = { ADDR_WHO_AM_I }; - /** * Helper class implementing the mag driver node. */ @@ -347,8 +337,7 @@ ACCELSIM::ACCELSIM(const char *path, enum Rotation rotation) : _accel_filter_z(ACCELSIM_ACCEL_DEFAULT_RATE, ACCELSIM_ACCEL_DEFAULT_DRIVER_FILTER_FREQ), _rotation(rotation), _constant_accel_count(0), - _last_temperature(0), - _checked_next(0) + _last_temperature(0) { // enable debug() calls _debug_enabled = false; From 8d59d08ad6f37eb83acd1a8b5c41b41c39796b5c Mon Sep 17 00:00:00 2001 From: Mark Charlebois Date: Thu, 29 Oct 2015 12:21:52 -0700 Subject: [PATCH 43/92] Removed unnecessary header files from accelsim Signed-off-by: Mark Charlebois --- src/platforms/posix/drivers/accelsim/accelsim.cpp | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/platforms/posix/drivers/accelsim/accelsim.cpp b/src/platforms/posix/drivers/accelsim/accelsim.cpp index cafb127ee3..5017bb111c 100644 --- a/src/platforms/posix/drivers/accelsim/accelsim.cpp +++ b/src/platforms/posix/drivers/accelsim/accelsim.cpp @@ -41,15 +41,10 @@ #include #include #include -#include #include #include -#include #include -#include #include -#include -#include #include #include #include @@ -64,7 +59,6 @@ #include #include #include -#include #include #include From 29db75fe56d7f00fa393bf4ed01a5ba1c6606a4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 19:18:36 +0200 Subject: [PATCH 44/92] Commander: Disarm after 5 seconds on the ground. --- src/modules/commander/commander.cpp | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 95fcf9e0a5..679835a73a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -183,6 +183,7 @@ static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; /* To remember when last notification was sent */ static uint64_t last_print_mode_reject_time = 0; +static uint64_t _inair_last_time = 0; static float eph_threshold = 5.0f; static float epv_threshold = 10.0f; @@ -1639,6 +1640,16 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "TAKEOFF DETECTED"); } } + + if (land_detector.landed) { + if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > 5 * 1000 * 1000)) { + mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, mavlink_fd, "auto disarm on land"); + _inair_last_time = 0; + } + } else { + _inair_last_time = land_detector.timestamp; + } } /* update battery status */ From a71164ea1169408bee04ddbabc8c6999458ffd59 Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 26 Oct 2015 09:22:34 +0100 Subject: [PATCH 45/92] added parameter to disable auto disarming when landed --- src/modules/commander/commander.cpp | 29 +++++++++++++++++------- src/modules/commander/commander_params.c | 13 +++++++++++ 2 files changed, 34 insertions(+), 8 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 679835a73a..5190c477a1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -921,6 +921,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_eph = param_find("COM_HOME_H_T"); param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); + param_t _param_disarm_land = param_find("COM_DISARM_LAND"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1270,6 +1271,8 @@ int commander_thread_main(int argc, char *argv[]) int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ + int32_t disarm_when_landed = 0; + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1349,6 +1352,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); + param_get(_param_disarm_land, &disarm_when_landed); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); @@ -1623,12 +1627,13 @@ int commander_thread_main(int argc, char *argv[]) &(status.condition_local_altitude_valid), &status_changed); /* Update land detector */ + static bool check_for_disarming = false; orb_check(land_detector_sub, &updated); if (updated) { orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector); } - if (updated && status.condition_local_altitude_valid) { + if ((updated && status.condition_local_altitude_valid) || check_for_disarming) { if (status.condition_landed != land_detector.landed) { status.condition_landed = land_detector.landed; status_changed = true; @@ -1641,14 +1646,22 @@ int commander_thread_main(int argc, char *argv[]) } } - if (land_detector.landed) { - if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > 5 * 1000 * 1000)) { - mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); - arm_disarm(false, mavlink_fd, "auto disarm on land"); - _inair_last_time = 0; + if (disarm_when_landed > 0) { + if (land_detector.landed) { + if (!check_for_disarming && _inair_last_time > 0) { + _inair_last_time = land_detector.timestamp; + check_for_disarming = true; + } + + if (_inair_last_time > 0 && ((hrt_absolute_time() - _inair_last_time) > (hrt_abstime)disarm_when_landed * 1000 * 1000)) { + mavlink_log_critical(mavlink_fd, "AUTO DISARMING AFTER LANDING"); + arm_disarm(false, mavlink_fd, "auto disarm on land"); + _inair_last_time = 0; + check_for_disarming = false; + } + } else { + _inair_last_time = land_detector.timestamp; } - } else { - _inair_last_time = land_detector.timestamp; } } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 214b7220c3..1c4108017f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -274,3 +274,16 @@ PARAM_DEFINE_INT32(COM_AUTOS_PAR, 1); * @max 2 */ PARAM_DEFINE_INT32(COM_RC_IN_MODE, 0); + +/** + * Time-out for auto disarm after landing + * + * A non-zero, positive value specifies the time-out period in seconds after which the vehicle will be + * automatically disarmed in case a landing situation has been detected during this period. + * A value of zero means that automatic disarming is disabled. + * + * @group Commander + * @min 0 + */ +PARAM_DEFINE_INT32(COM_DISARM_LAND, 0); + From 1aa9304b638eacff7f491836d44db16612807ddd Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Fri, 30 Oct 2015 15:06:47 +0100 Subject: [PATCH 46/92] Logging rate was limited to 1 Hz I set the maximum to 100 Hz, since most SD cards are not fast enough for more. (but more is still possible with forcing) --- src/modules/sdlog2/params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/params.c b/src/modules/sdlog2/params.c index 77575e2ad3..af1c6287f9 100644 --- a/src/modules/sdlog2/params.c +++ b/src/modules/sdlog2/params.c @@ -43,7 +43,7 @@ * commonly is before arming). * * @min -1 - * @max 1 + * @max 100 * @group SD Logging */ PARAM_DEFINE_INT32(SDLOG_RATE, -1); From b622080a65be29656df1557cc459b061fdacb96b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 15:40:22 +0100 Subject: [PATCH 47/92] Switch GIT command for revision hash --- cmake/common/px4_base.cmake | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 26598fef70..eb3b9c68fc 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git log -n 1 --pretty=format:"%H" + COMMAND git rev-parse HEAD OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} From f042c077b2f61feaf6046eb78148116232cf3365 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:01:21 +0100 Subject: [PATCH 48/92] Fix compile fail for GIT version --- .travis.yml | 2 ++ cmake/common/px4_base.cmake | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 35c9aa1c28..0b2dcd51d9 100644 --- a/.travis.yml +++ b/.travis.yml @@ -128,6 +128,8 @@ after_success: && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip ; fi + - git log -n 1 --pretty=format:"%H" + - git rev-parse HEAD deploy: provider: releases diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index eb3b9c68fc..26598fef70 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git rev-parse HEAD + COMMAND git log -n 1 --pretty=format:"%H" OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} From c912be70d1576f5b5c2e2fe850a179a60967655a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:25:08 +0100 Subject: [PATCH 49/92] Update approach to obtain the head git hash --- .travis.yml | 2 -- cmake/common/px4_base.cmake | 2 +- cmake/templates/build_git_version.h.in | 2 +- 3 files changed, 2 insertions(+), 4 deletions(-) diff --git a/.travis.yml b/.travis.yml index 0b2dcd51d9..35c9aa1c28 100644 --- a/.travis.yml +++ b/.travis.yml @@ -128,8 +128,6 @@ after_success: && echo https://px4-travis.s3.amazonaws.com/archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/Firmware.zip ; fi - - git log -n 1 --pretty=format:"%H" - - git rev-parse HEAD deploy: provider: releases diff --git a/cmake/common/px4_base.cmake b/cmake/common/px4_base.cmake index 26598fef70..eb3b9c68fc 100644 --- a/cmake/common/px4_base.cmake +++ b/cmake/common/px4_base.cmake @@ -700,7 +700,7 @@ function(px4_create_git_hash_header) REQUIRED HEADER ARGN ${ARGN}) execute_process( - COMMAND git log -n 1 --pretty=format:"%H" + COMMAND git rev-parse HEAD OUTPUT_VARIABLE git_desc OUTPUT_STRIP_TRAILING_WHITESPACE WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} diff --git a/cmake/templates/build_git_version.h.in b/cmake/templates/build_git_version.h.in index 1d1adb9ba3..83c59c37d9 100644 --- a/cmake/templates/build_git_version.h.in +++ b/cmake/templates/build_git_version.h.in @@ -1,4 +1,4 @@ /* Auto Magically Generated file */ /* Do not edit! */ -#define PX4_GIT_VERSION_STR @git_desc@ +#define PX4_GIT_VERSION_STR "@git_desc@" #define PX4_GIT_VERSION_BINARY 0x@git_desc_short@ From 4e87ac601485a8086819cadaf4c3a7d05b60024b Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 30 Oct 2015 16:18:17 +0100 Subject: [PATCH 50/92] run preflight checks after mode switch has changed --- src/modules/commander/commander.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5190c477a1..a2d5dc003a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -92,6 +92,7 @@ #include #include #include +#include #include #include @@ -922,6 +923,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_epv = param_find("COM_HOME_V_T"); param_t _param_geofence_action = param_find("GF_ACTION"); param_t _param_disarm_land = param_find("COM_DISARM_LAND"); + param_t _param_map_mode_sw = param_find("RC_MAP_MODE_SW"); // const char *main_states_str[vehicle_status_s::MAIN_STATE_MAX]; // main_states_str[vehicle_status_s::MAIN_STATE_MANUAL] = "MANUAL"; @@ -1272,6 +1274,7 @@ int commander_thread_main(int argc, char *argv[]) int autosave_params; /**< Autosave of parameters enabled/disabled, loaded from parameter */ int32_t disarm_when_landed = 0; + int32_t map_mode_sw = 0; /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1337,6 +1340,16 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; + // check if main mode switch has been assigned and if so run preflight checks in order + // to update status.condition_sensors_initialised + int32_t map_mode_sw_new; + param_get(_param_map_mode_sw, &map_mode_sw_new); + + if (map_mode_sw == 0 && map_mode_sw != map_mode_sw_new && map_mode_sw_new < input_rc_s::RC_INPUT_MAX_CHANNELS && map_mode_sw_new > 0) { + status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, + !(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check); + } + /* re-check RC calibration */ rc_calibration_check(mavlink_fd); } @@ -1353,6 +1366,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_ef_time_thres, &ef_time_thres); param_get(_param_geofence_action, &geofence_action); param_get(_param_disarm_land, &disarm_when_landed); + param_get(_param_map_mode_sw, &map_mode_sw); /* Autostart id */ param_get(_param_autostart_id, &autostart_id); From 3d13e771a8d5b58cacb3369934a802b312cfee7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:33:19 +0100 Subject: [PATCH 51/92] Support runs without simulation --- Tools/sitl_run.sh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 7f7808c9e6..521ebeef21 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -42,13 +42,13 @@ cp Tools/posix.gdbinit $build_path/src/firmware/posix/.gdbinit SIM_PID=0 -if [ "$program" == "jmavsim" ] +if [ "$program" == "jmavsim" ] && [ "$no_sim" == "" ] then cd Tools/jMAVSim ant java -Djava.ext.dirs= -cp lib/*:out/production/jmavsim.jar me.drton.jmavsim.Simulator -udp 127.0.0.1:14560 & SIM_PID=`echo $!` -elif [ "$3" == "gazebo" ] +elif [ "$3" == "gazebo" ] && [ "$no_sim" == "" ] then if [ -x "$(command -v gazebo)" ] then From e0e7a2414e6195fb59c973c443fee304f70afc63 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 16:59:37 +0100 Subject: [PATCH 52/92] Do not check component ID for mission handling partner --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7e9f546d26..d710558ef0 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -403,7 +403,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_msg_mission_ack_decode(msg, &wpa); if (CHECK_SYSID_COMPID_MISSION(wpa)) { - if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { + if ((msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -504,7 +504,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_msg_mission_request_decode(msg, &wpr); if (CHECK_SYSID_COMPID_MISSION(wpr)) { - if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { + if (msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); From 90798a7cb6ab003b351b8b9d6b848ad96f06e7c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 17:08:12 +0100 Subject: [PATCH 53/92] Revert "Do not check component ID for mission handling partner" This reverts commit e0e7a2414e6195fb59c973c443fee304f70afc63. --- src/modules/mavlink/mavlink_mission.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index d710558ef0..7e9f546d26 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -403,7 +403,7 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) mavlink_msg_mission_ack_decode(msg, &wpa); if (CHECK_SYSID_COMPID_MISSION(wpa)) { - if ((msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/)) { + if ((msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid)) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); @@ -504,7 +504,7 @@ MavlinkMissionManager::handle_mission_request(const mavlink_message_t *msg) mavlink_msg_mission_request_decode(msg, &wpr); if (CHECK_SYSID_COMPID_MISSION(wpr)) { - if (msg->sysid == _transfer_partner_sysid/* && msg->compid == _transfer_partner_compid*/) { + if (msg->sysid == _transfer_partner_sysid && msg->compid == _transfer_partner_compid) { if (_state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); From fe4c67a781db8a192229f22e70f08a36d7fcd70f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 30 Oct 2015 18:40:55 +0100 Subject: [PATCH 54/92] Fixed MAVLink mission transmission --- src/modules/mavlink/mavlink_mission.cpp | 21 ++++++++++----------- 1 file changed, 10 insertions(+), 11 deletions(-) diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index 7e9f546d26..bb0908908c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -422,7 +422,9 @@ MavlinkMissionManager::handle_mission_ack(const mavlink_message_t *msg) } else { _mavlink->send_statustext_critical("REJ. WP CMD: partner id mismatch"); - if (_verbose) { warnx("WPM: MISSION_ACK ERROR: rejected, partner ID mismatch"); } + if (_verbose) { + warnx("WPM: MISSION_ACK ERR: ID mismatch"); + } } } } @@ -473,13 +475,13 @@ MavlinkMissionManager::handle_mission_request_list(const mavlink_message_t *msg) if (_state == MAVLINK_WPM_STATE_IDLE || _state == MAVLINK_WPM_STATE_SENDLIST) { _time_last_recv = hrt_absolute_time(); - if (_count > 0) { - _state = MAVLINK_WPM_STATE_SENDLIST; - _transfer_seq = 0; - _transfer_count = _count; - _transfer_partner_sysid = msg->sysid; - _transfer_partner_compid = msg->compid; + _state = MAVLINK_WPM_STATE_SENDLIST; + _transfer_seq = 0; + _transfer_count = _count; + _transfer_partner_sysid = msg->sysid; + _transfer_partner_compid = msg->compid; + if (_count > 0) { if (_verbose) { warnx("WPM: MISSION_REQUEST_LIST OK, %u mission items to send", _transfer_count); } } else { @@ -598,9 +600,8 @@ MavlinkMissionManager::handle_mission_count(const mavlink_message_t *msg) /* alternate dataman ID anyway to let navigator know about changes */ update_active_mission(_dataman_id == 0 ? 1 : 0, 0, 0); - _mavlink->send_statustext_info("WPM: COUNT 0: CLEAR MISSION"); - // TODO send ACK? + send_mission_ack(_transfer_partner_sysid, _transfer_partner_compid, MAV_MISSION_ACCEPTED); _transfer_in_progress = false; return; } @@ -712,8 +713,6 @@ MavlinkMissionManager::handle_mission_item(const mavlink_message_t *msg) /* got all new mission items successfully */ if (_verbose) { warnx("WPM: MISSION_ITEM got all %u items, current_seq=%u, changing state to MAVLINK_WPM_STATE_IDLE", _transfer_count, _transfer_current_seq); } - _mavlink->send_statustext_info("WPM: Transfer complete."); - _state = MAVLINK_WPM_STATE_IDLE; if (update_active_mission(_transfer_dataman_id, _transfer_count, _transfer_current_seq) == OK) { From 5394c661c7bf2b660a113999bcd7e0509f07845a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 23 Oct 2015 20:22:50 +0200 Subject: [PATCH 55/92] Navigator: Fix RTL to return in a straight line --- src/modules/navigator/rtl.cpp | 10 +++++++++- src/modules/navigator/rtl.h | 2 ++ 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 8a4d89ef35..6e754588bb 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -58,6 +58,7 @@ RTL::RTL(Navigator *navigator, const char *name) : MissionBlock(navigator, name), _rtl_state(RTL_STATE_NONE), + _rtl_start_lock(false), _param_return_alt(this, "RTL_RETURN_ALT", false), _param_descend_alt(this, "RTL_DESCEND_ALT", false), _param_land_delay(this, "RTL_LAND_DELAY", false) @@ -95,6 +96,7 @@ RTL::on_activation() } else if (_navigator->get_global_position()->alt < _navigator->get_home_position()->alt + _param_return_alt.get()) { _rtl_state = RTL_STATE_CLIMB; + _rtl_start_lock = false; /* otherwise go straight to return */ } else { @@ -102,6 +104,7 @@ RTL::on_activation() _rtl_state = RTL_STATE_RETURN; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_global_position()->alt; + _rtl_start_lock = false; } } @@ -126,7 +129,10 @@ RTL::set_rtl_item() /* make sure we have the latest params */ updateParams(); - set_previous_pos_setpoint(); + if (!_rtl_start_lock) { + set_previous_pos_setpoint(); + } + _navigator->set_can_loiter_at_sp(false); switch (_rtl_state) { @@ -182,6 +188,8 @@ RTL::set_rtl_item() mavlink_log_critical(_navigator->get_mavlink_fd(), "RTL: return at %d m (%d m above home)", (int)(_mission_item.altitude), (int)(_mission_item.altitude - _navigator->get_home_position()->alt)); + + _rtl_start_lock = true; break; } diff --git a/src/modules/navigator/rtl.h b/src/modules/navigator/rtl.h index 3506065802..464a1d4ee4 100644 --- a/src/modules/navigator/rtl.h +++ b/src/modules/navigator/rtl.h @@ -87,6 +87,8 @@ private: RTL_STATE_LANDED, } _rtl_state; + bool _rtl_start_lock; + control::BlockParamFloat _param_return_alt; control::BlockParamFloat _param_descend_alt; control::BlockParamFloat _param_land_delay; From 85fcfed083126fe72774c664e030ce7995591c8e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Oct 2015 20:17:13 +0100 Subject: [PATCH 56/92] Add missing yaw entries --- src/modules/navigator/rtl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/rtl.cpp b/src/modules/navigator/rtl.cpp index 6e754588bb..a77f5895e1 100644 --- a/src/modules/navigator/rtl.cpp +++ b/src/modules/navigator/rtl.cpp @@ -221,7 +221,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt + _param_descend_alt.get(); - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = autoland ? NAV_CMD_LOITER_TIME_LIMIT : NAV_CMD_LOITER_UNLIMITED; @@ -247,7 +247,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; - _mission_item.yaw = NAN; + _mission_item.yaw = _navigator->get_home_position()->yaw; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; _mission_item.nav_cmd = NAV_CMD_LAND; @@ -266,6 +266,7 @@ RTL::set_rtl_item() _mission_item.lon = _navigator->get_home_position()->lon; _mission_item.altitude_is_relative = false; _mission_item.altitude = _navigator->get_home_position()->alt; + // Do not change / control yaw in landed _mission_item.yaw = NAN; _mission_item.loiter_radius = _navigator->get_loiter_radius(); _mission_item.loiter_direction = 1; From 35d8e0bc550645b470af5c8624243b8f3a56d081 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Oct 2015 20:32:58 +0100 Subject: [PATCH 57/92] Raise frame size --- src/modules/commander/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/CMakeLists.txt b/src/modules/commander/CMakeLists.txt index 6cf2531ade..c1ec35b5ee 100644 --- a/src/modules/commander/CMakeLists.txt +++ b/src/modules/commander/CMakeLists.txt @@ -32,7 +32,7 @@ ############################################################################ set(MODULE_CFLAGS -Os) if(${OS} STREQUAL "nuttx") - list(APPEND MODULE_CFLAGS -Wframe-larger-than=2400) + list(APPEND MODULE_CFLAGS -Wframe-larger-than=2450) endif() px4_add_module( MODULE modules__commander From 601da2818b3e73f5fc12a7615c570ee24a4e52d7 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 1 Nov 2015 10:13:55 +0100 Subject: [PATCH 58/92] do not set position setpoint too early --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 80847d8e47..2c2010e895 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -714,16 +714,18 @@ MulticopterPositionControl::control_manual(float dt) (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) { _pos_hold_engaged = true; + } else { + _pos_hold_engaged = false; } } else { _pos_hold_engaged = false; - _pos_sp(0) = _pos(0); - _pos_sp(1) = _pos(1); } /* set requested velocity setpoint */ if (!_pos_hold_engaged) { + _pos_sp(0) = _pos(0); + _pos_sp(1) = _pos(1); _run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */ _vel_sp(0) = req_vel_sp_scaled(0); _vel_sp(1) = req_vel_sp_scaled(1); From 5050da0ba00c45377f4ef91340fa89eb8faa1b9d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Oct 2015 11:29:44 +0200 Subject: [PATCH 59/92] TECS: Add function to reset system --- src/lib/external_lgpl/tecs/tecs.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 914e41eba0..1ca7428849 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -118,6 +118,9 @@ public: return get_throttle_demand(); } + void reset_state() { + _states_initalized = false; + } float get_pitch_demand() { return _pitch_dem; } From eeecf01628eae742ce1d9ea64484e8e063b61a78 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 4 Oct 2015 11:30:14 +0200 Subject: [PATCH 60/92] Fixed wing controller: Call state reset on flight mode change. --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index fa12c5822a..a629b9d2cb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -1118,6 +1118,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_AUTO; @@ -1477,6 +1479,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_POSITION; @@ -1588,6 +1592,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if (_mTecs.getEnabled()) { _mTecs.resetIntegrators(); _mTecs.resetDerivatives(_airspeed.true_airspeed_m_s); + } else { + _tecs.reset_state(); } } _control_mode_current = FW_POSCTRL_MODE_ALTITUDE; From 42523be0ebf6b8bb3312f58a8f31fdd3b97399fb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:38:27 +0100 Subject: [PATCH 61/92] EKF: Allow more delta velocity deviation before resetting --- src/modules/ekf_att_pos_estimator/estimator_22states.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp index 0e7b459b72..b794b9f90d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator_22states.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator_22states.cpp @@ -2825,7 +2825,7 @@ bool AttPosEKF::VelNEDDiverged() Vector3f delta = current_vel - gps_vel; float delta_len = delta.length(); - bool excessive = (delta_len > 20.0f); + bool excessive = (delta_len > 30.0f); current_ekf_state.error |= excessive; current_ekf_state.velOffsetExcessive = excessive; From 3edf532e90f89f9e58059610b6f3310e7ca1cc0b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:39:22 +0100 Subject: [PATCH 62/92] MAVLink receiver: Fix HIL handling --- src/modules/mavlink/mavlink_receiver.cpp | 58 ++++++++++++------------ 1 file changed, 28 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d0b95b7f89..5f36354191 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1116,7 +1116,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg) /* yaw */ rc.values[2] = man.r / 2 + 1500; /* throttle */ - rc.values[3] = man.z / 1 + 1000; + rc.values[3] = fminf(fmaxf(man.z / 0.9f + 800, 1000.0f), 2000.0f); /* decode all switches which fit into the channel mask */ unsigned max_switch = (sizeof(man.buttons) * 8); @@ -1307,18 +1307,22 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) float dt; if (_hil_last_frame == 0 || - (imu.time_usec - _hil_last_frame) > (0.1f * 1e6f) || - (_hil_last_frame >= imu.time_usec)) { + (timestamp <= _hil_last_frame) || + (timestamp - _hil_last_frame) > (0.1f * 1e6f) || + (_hil_last_frame >= timestamp)) { dt = 0.01f; /* default to 100 Hz */ + memset(&_hil_prev_gyro[0], 0, sizeof(_hil_prev_gyro)); + _hil_prev_accel[0] = 0.0f; + _hil_prev_accel[1] = 0.0f; + _hil_prev_accel[2] = 9.866f; } else { - dt = (imu.time_usec - _hil_last_frame) / 1e6f; + dt = (timestamp - _hil_last_frame) / 1e6f; } - _hil_last_frame = imu.time_usec; + _hil_last_frame = timestamp; /* airspeed */ { - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); + struct airspeed_s airspeed = {}; float ias = calc_indicated_airspeed(imu.diff_pressure * 1e2f); // XXX need to fix this @@ -1338,8 +1342,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* gyro */ { - struct gyro_report gyro; - memset(&gyro, 0, sizeof(gyro)); + struct gyro_report gyro = {}; gyro.timestamp = timestamp; gyro.x_raw = imu.xgyro * 1000.0f; @@ -1360,8 +1363,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* accelerometer */ { - struct accel_report accel; - memset(&accel, 0, sizeof(accel)); + struct accel_report accel = {}; accel.timestamp = timestamp; accel.x_raw = imu.xacc / mg2ms2; @@ -1382,8 +1384,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* magnetometer */ { - struct mag_report mag; - memset(&mag, 0, sizeof(mag)); + struct mag_report mag = {}; mag.timestamp = timestamp; mag.x_raw = imu.xmag * 1000.0f; @@ -1404,8 +1405,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* baro */ { - struct baro_report baro; - memset(&baro, 0, sizeof(baro)); + struct baro_report baro = {}; baro.timestamp = timestamp; baro.pressure = imu.abs_pressure; @@ -1422,21 +1422,20 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* sensor combined */ { - struct sensor_combined_s hil_sensors; - memset(&hil_sensors, 0, sizeof(hil_sensors)); + struct sensor_combined_s hil_sensors = {}; hil_sensors.timestamp = timestamp; hil_sensors.gyro_raw[0] = imu.xgyro * 1000.0f; hil_sensors.gyro_raw[1] = imu.ygyro * 1000.0f; hil_sensors.gyro_raw[2] = imu.zgyro * 1000.0f; - hil_sensors.gyro_rad_s[0] = ((imu.xgyro * dt + _hil_prev_gyro[0]) / 2.0f) / dt; + hil_sensors.gyro_rad_s[0] = imu.xgyro; hil_sensors.gyro_rad_s[1] = imu.ygyro; hil_sensors.gyro_rad_s[2] = imu.zgyro; - hil_sensors.gyro_integral_rad[0] = (hil_sensors.gyro_rad_s[0] * dt + _hil_prev_gyro[0]) / 2.0f; - hil_sensors.gyro_integral_rad[1] = (hil_sensors.gyro_rad_s[1] * dt + _hil_prev_gyro[1]) / 2.0f; - hil_sensors.gyro_integral_rad[2] = (hil_sensors.gyro_rad_s[2] * dt + _hil_prev_gyro[2]) / 2.0f; - memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_integral_rad[0], sizeof(_hil_prev_gyro)); + hil_sensors.gyro_integral_rad[0] = 0.5f * (imu.xgyro + _hil_prev_gyro[0]) * dt; + hil_sensors.gyro_integral_rad[1] = 0.5f * (imu.ygyro + _hil_prev_gyro[1]) * dt; + hil_sensors.gyro_integral_rad[2] = 0.5f * (imu.zgyro + _hil_prev_gyro[2]) * dt; + memcpy(&_hil_prev_gyro[0], &hil_sensors.gyro_rad_s[0], sizeof(_hil_prev_gyro)); hil_sensors.gyro_integral_dt[0] = dt * 1e6f; hil_sensors.gyro_timestamp[0] = timestamp; hil_sensors.gyro_priority[0] = ORB_PRIO_HIGH; @@ -1447,10 +1446,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) hil_sensors.accelerometer_m_s2[0] = imu.xacc; hil_sensors.accelerometer_m_s2[1] = imu.yacc; hil_sensors.accelerometer_m_s2[2] = imu.zacc; - hil_sensors.accelerometer_integral_m_s[0] = (imu.xacc * dt + _hil_prev_accel[0]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[1] = (imu.yacc * dt + _hil_prev_accel[1]) / 2.0f; - hil_sensors.accelerometer_integral_m_s[2] = (imu.zacc * dt + _hil_prev_accel[2]) / 2.0f; - memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_integral_m_s[0], sizeof(_hil_prev_accel)); + hil_sensors.accelerometer_integral_m_s[0] = 0.5f * (imu.xacc + _hil_prev_accel[0]) * dt; + hil_sensors.accelerometer_integral_m_s[1] = 0.5f * (imu.yacc + _hil_prev_accel[1]) * dt; + hil_sensors.accelerometer_integral_m_s[2] = 0.5f * (imu.zacc + _hil_prev_accel[2]) * dt; + memcpy(&_hil_prev_accel[0], &hil_sensors.accelerometer_m_s2[0], sizeof(_hil_prev_accel)); hil_sensors.accelerometer_integral_dt[0] = dt * 1e6f; hil_sensors.accelerometer_mode[0] = 0; // TODO what is this? hil_sensors.accelerometer_range_m_s2[0] = 32.7f; // int16 @@ -1493,12 +1492,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) /* battery status */ { - struct battery_status_s hil_battery_status; - memset(&hil_battery_status, 0, sizeof(hil_battery_status)); + struct battery_status_s hil_battery_status = {}; hil_battery_status.timestamp = timestamp; - hil_battery_status.voltage_v = 11.1f; - hil_battery_status.voltage_filtered_v = 11.1f; + hil_battery_status.voltage_v = 11.5f; + hil_battery_status.voltage_filtered_v = 11.5f; hil_battery_status.current_a = 10.0f; hil_battery_status.discharged_mah = -1.0f; From fa78e8523e15eb6750610b1637ede0ff42d6e015 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 1 Nov 2015 17:39:56 +0100 Subject: [PATCH 63/92] EKF: Fix time handling --- .../ekf_att_pos_estimator_main.cpp | 51 ++++++++++++++----- 1 file changed, 37 insertions(+), 14 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 04be54eb33..2ea78ddf14 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -588,6 +588,11 @@ void AttitudePositionEstimatorEKF::task_main() /* system is in HIL now, wait for measurements to come in one last round */ usleep(60000); + /* HIL is slow, set permissive timeouts */ + _voter_gyro.set_timeout(500000); + _voter_accel.set_timeout(500000); + _voter_mag.set_timeout(500000); + /* now read all sensor publications to ensure all real sensor data is purged */ orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); @@ -1223,6 +1228,23 @@ void AttitudePositionEstimatorEKF::pollData() orb_copy(ORB_ID(sensor_combined), _sensor_combined_sub, &_sensor_combined); + /* set time fields */ + IMUusec = _sensor_combined.timestamp; + float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; + + /* guard against too large deltaT's */ + if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.0001f) { + + if (PX4_ISFINITE(_ekf->dtIMUfilt) && _ekf->dtIMUfilt < 0.5f && _ekf->dtIMUfilt > 0.0001f) { + deltaT = _ekf->dtIMUfilt; + } else { + deltaT = 0.01f; + } + } + + /* fill in last data set */ + _ekf->dtIMU = deltaT; + // Feed validator with recent sensor data for (unsigned i = 0; i < (sizeof(_sensor_combined.gyro_timestamp) / sizeof(_sensor_combined.gyro_timestamp[0])); i++) { @@ -1245,6 +1267,11 @@ void AttitudePositionEstimatorEKF::pollData() _ekf->dAngIMU.y = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 1]; _ekf->dAngIMU.z = _sensor_combined.gyro_integral_rad[_gyro_main * 3 + 2]; } else { + float dt_gyro = _sensor_combined.gyro_integral_dt[_gyro_main] / 1e6f; + if (PX4_ISFINITE(dt_gyro) && (dt_gyro < 0.5f) && (dt_gyro > 0.00001f)) { + deltaT = dt_gyro; + _ekf->dtIMU = deltaT; + } _ekf->dAngIMU.x = 0.5f * (_ekf->angRate.x + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 0]) * _ekf->dtIMU; _ekf->dAngIMU.y = 0.5f * (_ekf->angRate.y + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 1]) * _ekf->dtIMU; _ekf->dAngIMU.z = 0.5f * (_ekf->angRate.z + _sensor_combined.gyro_rad_s[_gyro_main * 3 + 2]) * _ekf->dtIMU; @@ -1317,20 +1344,8 @@ void AttitudePositionEstimatorEKF::pollData() _vibration_warning_timestamp = 0; } - IMUusec = _sensor_combined.timestamp; - - float deltaT = (_sensor_combined.timestamp - _last_sensor_timestamp) / 1e6f; _last_sensor_timestamp = _sensor_combined.timestamp; - /* guard against too large deltaT's */ - if (!PX4_ISFINITE(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { - deltaT = 0.01f; - } - - // Always store data, independent of init status - /* fill in last data set */ - _ekf->dtIMU = deltaT; - // XXX this is for assessing the filter performance // leave this in as long as larger improvements are still being made. #if 0 @@ -1342,7 +1357,7 @@ void AttitudePositionEstimatorEKF::pollData() static unsigned dtoverflow10 = 0; static hrt_abstime lastprint = 0; - if (hrt_elapsed_time(&lastprint) > 1000000) { + if (hrt_elapsed_time(&lastprint) > 1000000 || _sensor_combined.gyro_rad_s[0] > 4.0f) { PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u", (double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc, dtoverflow5, dtoverflow10); @@ -1361,7 +1376,15 @@ void AttitudePositionEstimatorEKF::pollData() (double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[1] * deltaT), - (double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT)); + (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT)); + + PX4_WARN("EKF rate: %8.4f, %8.4f, %8.4f", + (double)_att.rollspeed, (double)_att.pitchspeed, (double)_att.yawspeed); + + PX4_WARN("DRV rate: %8.4f, %8.4f, %8.4f", + (double)_sensor_combined.gyro_rad_s[0], + (double)_sensor_combined.gyro_rad_s[1], + (double)_sensor_combined.gyro_rad_s[2]); lastprint = hrt_absolute_time(); } From c0a663540039643b05c5ed2e2624fcc38fa93c25 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 10:18:12 +0100 Subject: [PATCH 64/92] Lock VDEV table to prevent concurrent access --- src/drivers/device/vdev_posix.cpp | 66 ++++++++++++++++++++++++------- 1 file changed, 51 insertions(+), 15 deletions(-) diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index a30bc8d06b..c5e1c9a4f5 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -52,6 +52,8 @@ using namespace device; +pthread_mutex_t filemutex = PTHREAD_MUTEX_INITIALIZER; + extern "C" { static void timer_cb(void *data) @@ -68,7 +70,27 @@ extern "C" { inline bool valid_fd(int fd) { - return (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + pthread_mutex_lock(&filemutex); + bool ret = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + pthread_mutex_unlock(&filemutex); + return ret; + } + + inline VDev *get_vdev(int fd) + { + pthread_mutex_lock(&filemutex); + bool valid = (fd < PX4_MAX_FD && fd >= 0 && filemap[fd] != NULL); + VDev *dev; + + if (valid) { + dev = (VDev *)(filemap[fd]->vdev); + + } else { + dev = nullptr; + } + + pthread_mutex_unlock(&filemutex); + return dev; } int px4_open(const char *path, int flags, ...) @@ -93,6 +115,9 @@ extern "C" { } if (dev) { + + pthread_mutex_lock(&filemutex); + for (i = 0; i < PX4_MAX_FD; ++i) { if (filemap[i] == 0) { filemap[i] = new device::file_t(flags, dev, i); @@ -100,6 +125,8 @@ extern "C" { } } + pthread_mutex_unlock(&filemutex); + if (i < PX4_MAX_FD) { ret = dev->open(filemap[i]); @@ -125,11 +152,14 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); - PX4_DEBUG("px4_close fd = %d", fd); + VDev *dev = get_vdev(fd); + + if (dev) { + pthread_mutex_lock(&filemutex); ret = dev->close(filemap[fd]); - filemap[fd] = NULL; + filemap[fd] = nullptr; + pthread_mutex_unlock(&filemutex); + PX4_DEBUG("px4_close fd = %d", fd); } else { ret = -EINVAL; @@ -147,8 +177,9 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { PX4_DEBUG("px4_read fd = %d", fd); ret = dev->read(filemap[fd], (char *)buffer, buflen); @@ -168,8 +199,9 @@ extern "C" { { int ret; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { PX4_DEBUG("px4_write fd = %d", fd); ret = dev->write(filemap[fd], (const char *)buffer, buflen); @@ -190,8 +222,9 @@ extern "C" { PX4_DEBUG("px4_ioctl fd = %d", fd); int ret = 0; - if (valid_fd(fd)) { - VDev *dev = (VDev *)(filemap[fd]->vdev); + VDev *dev = get_vdev(fd); + + if (dev) { ret = dev->ioctl(filemap[fd], cmd, arg); } else { @@ -221,9 +254,10 @@ extern "C" { fds[i].revents = 0; fds[i].priv = NULL; + VDev *dev = get_vdev(fds[i].fd); + // If fd is valid - if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + if (dev) { PX4_DEBUG("px4_poll: VDev->poll(setup) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], true); @@ -251,9 +285,11 @@ extern "C" { // For each fd for (i = 0; i < nfds; ++i) { + + VDev *dev = get_vdev(fds[i].fd); + // If fd is valid - if (valid_fd(fds[i].fd)) { - VDev *dev = (VDev *)(filemap[fds[i].fd]->vdev);; + if (dev) { PX4_DEBUG("px4_poll: VDev->poll(teardown) %d", fds[i].fd); ret = dev->poll(filemap[fds[i].fd], &fds[i], false); From 06987c624481201495334fa114e90a2ff26331c1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 11:05:09 +0100 Subject: [PATCH 65/92] Lock vdev devmap --- src/drivers/device/vdev.cpp | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/drivers/device/vdev.cpp b/src/drivers/device/vdev.cpp index 60761ed511..4aa58447f3 100644 --- a/src/drivers/device/vdev.cpp +++ b/src/drivers/device/vdev.cpp @@ -67,6 +67,7 @@ private: #define PX4_MAX_DEV 500 static px4_dev_t *devmap[PX4_MAX_DEV]; +pthread_mutex_t devmutex = PTHREAD_MUTEX_INITIALIZER; /* * The standard NuttX operation dispatch table can't call C++ member functions @@ -142,8 +143,12 @@ VDev::register_driver(const char *name, void *data) // Make sure the device does not already exist // FIXME - convert this to a map for efficiency + + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && (strcmp(devmap[i]->name, name) == 0)) { + pthread_mutex_unlock(&devmutex); return -EEXIST; } } @@ -157,6 +162,8 @@ VDev::register_driver(const char *name, void *data) } } + pthread_mutex_unlock(&devmutex); + if (ret != PX4_OK) { PX4_ERR("No free devmap entries - increase PX4_MAX_DEV"); } @@ -174,6 +181,8 @@ VDev::unregister_driver(const char *name) return -EINVAL; } + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && (strcmp(name, devmap[i]->name) == 0)) { delete devmap[i]; @@ -184,6 +193,8 @@ VDev::unregister_driver(const char *name) } } + pthread_mutex_unlock(&devmutex); + return ret; } @@ -194,15 +205,20 @@ VDev::unregister_class_devname(const char *class_devname, unsigned class_instanc char name[32]; snprintf(name, sizeof(name), "%s%u", class_devname, class_instance); + pthread_mutex_lock(&devmutex); + for (int i = 0; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strcmp(devmap[i]->name, name) == 0) { delete devmap[i]; PX4_DEBUG("Unregistered class DEV %s", name); devmap[i] = NULL; + pthread_mutex_unlock(&devmutex); return PX4_OK; } } + pthread_mutex_unlock(&devmutex); + return -EINVAL; } @@ -497,15 +513,20 @@ VDev *VDev::getDev(const char *path) PX4_DEBUG("VDev::getDev"); int i = 0; + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { //if (devmap[i]) { // printf("%s %s\n", devmap[i]->name, path); //} if (devmap[i] && (strcmp(devmap[i]->name, path) == 0)) { + pthread_mutex_unlock(&devmutex); return (VDev *)(devmap[i]->cdev); } } + pthread_mutex_unlock(&devmutex); + return NULL; } @@ -514,11 +535,15 @@ void VDev::showDevices() int i = 0; PX4_INFO("Devices:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/dev/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showTopics() @@ -526,11 +551,15 @@ void VDev::showTopics() int i = 0; PX4_INFO("Devices:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) == 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } void VDev::showFiles() @@ -538,12 +567,16 @@ void VDev::showFiles() int i = 0; PX4_INFO("Files:"); + pthread_mutex_lock(&devmutex); + for (; i < PX4_MAX_DEV; ++i) { if (devmap[i] && strncmp(devmap[i]->name, "/obj/", 5) != 0 && strncmp(devmap[i]->name, "/dev/", 5) != 0) { PX4_INFO(" %s", devmap[i]->name); } } + + pthread_mutex_unlock(&devmutex); } const char *VDev::topicList(unsigned int *next) From 013e34661206a69fec61ec4d0998d36d299b5a42 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 11:51:50 +0100 Subject: [PATCH 66/92] Commander: Fix Geofence action access --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 54463ee94d..5cdd7cf4e4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1269,7 +1269,7 @@ int commander_thread_main(int argc, char *argv[]) float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - uint8_t geofence_action = 0; + int32_t geofence_action = 0; /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; From 7f242d71706919b4f6b2ad329cd67882976abf87 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 13:42:20 +0100 Subject: [PATCH 67/92] Update jMAVSim --- Tools/jMAVSim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/jMAVSim b/Tools/jMAVSim index 96029523d1..fd9ed5df04 160000 --- a/Tools/jMAVSim +++ b/Tools/jMAVSim @@ -1 +1 @@ -Subproject commit 96029523d13fc7fdac04c1a50b08c0eb0b39f9a9 +Subproject commit fd9ed5df0496d42f20b91cb405639ceccbcf9e17 From a567b9956f29da1ac057ca549bc41d7a563a7e5b Mon Sep 17 00:00:00 2001 From: tumbili Date: Mon, 2 Nov 2015 16:11:24 +0100 Subject: [PATCH 68/92] fix position hold logic --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 2c2010e895..a465cc64f5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -710,12 +710,12 @@ MulticopterPositionControl::control_manual(float dt) /* check for pos. hold */ if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) { - if (!_pos_hold_engaged && (_params.hold_max_xy < FLT_EPSILON || - (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy))) - { - _pos_hold_engaged = true; - } else { - _pos_hold_engaged = false; + if (!_pos_hold_engaged) { + if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy && fabsf(_vel(1)) < _params.hold_max_xy)) { + _pos_hold_engaged = true; + } else { + _pos_hold_engaged = false; + } } } else { From 484bd3bd89e2e8429e5c53b63dc55fea9adaea03 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 2 Nov 2015 18:48:53 +0100 Subject: [PATCH 69/92] Let commander be less verbose --- src/modules/commander/commander.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5cdd7cf4e4..79ea8dd893 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3051,11 +3051,8 @@ void *commander_low_prio_loop(void *arg) if (need_param_autosave_timeout > 0 && hrt_elapsed_time(&need_param_autosave_timeout) > 200000ULL) { int ret = param_save_default(); - if (ret == OK) { - mavlink_and_console_log_info(mavlink_fd, "settings autosaved"); - - } else { - mavlink_and_console_log_critical(mavlink_fd, "settings save error"); + if (ret != OK) { + mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); } need_param_autosave = false; From c20e7f8424c40175c47abd14a2566dd8045eeba1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 08:57:30 +0100 Subject: [PATCH 70/92] Commander: Provide console output on autosave --- src/modules/commander/commander.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 79ea8dd893..1a70d0f6dd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -3053,6 +3053,8 @@ void *commander_low_prio_loop(void *arg) if (ret != OK) { mavlink_and_console_log_critical(mavlink_fd, "settings auto save error"); + } else { + warnx("settings saved."); } need_param_autosave = false; From 8a6e9d17bad1348682c8e6f727c4261f4530df07 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:02:53 +0100 Subject: [PATCH 71/92] MAVLink: Do a better job at receive rate limiting --- src/modules/mavlink/mavlink_main.cpp | 2 ++ src/modules/mavlink/mavlink_main.h | 2 ++ src/modules/mavlink/mavlink_receiver.cpp | 11 ++++++++--- 3 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a12133c021..0c017c5477 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -698,6 +698,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * } else { _is_usb_uart = true; + /* USB has no baudrate, but use a magic number for 'fast' */ + _baudrate = 2000000; } #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ef14562ccc..95e0fdb91a 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -288,6 +288,8 @@ public: float get_rate_mult(); + float get_baudrate() { return _baudrate; } + /* Functions for waiting to start transmission until message received. */ void set_has_received_messages(bool received_messages) { _received_messages = received_messages; } bool get_has_received_messages() { return _received_messages; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5f36354191..29f299f918 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1757,7 +1757,7 @@ MavlinkReceiver::receive_thread(void *arg) uint8_t buf[1600]; #else /* the serial port buffers internally as well, we just need to fit a small chunk */ - uint8_t buf[32]; + uint8_t buf[64]; #endif mavlink_message_t msg; @@ -1799,8 +1799,13 @@ MavlinkReceiver::receive_thread(void *arg) if (_mavlink->get_protocol() == SERIAL) { /* non-blocking read. read may return negative values */ if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* to avoid reading very small chunks wait for data before reading */ - usleep(1000); + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target ~30 bytes at a time + */ + const unsigned character_count = 20; + unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; + usleep(sleeptime); } } #ifdef __PX4_POSIX From c934286f6c6f22cc0c7ee21d633e0ef3531eeb5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:16:38 +0100 Subject: [PATCH 72/92] MAVLink app: Do not try to fill the complete buffer, just try to get a complete message --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 29f299f918..c4f1cbceec 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1797,13 +1797,15 @@ MavlinkReceiver::receive_thread(void *arg) while (!_mavlink->_task_should_exit) { if (poll(&fds[0], 1, timeout) > 0) { if (_mavlink->get_protocol() == SERIAL) { + + /* + * to avoid reading very small chunks wait for data before reading + * this is designed to target ~30 bytes at a time + */ + const unsigned character_count = 20; + /* non-blocking read. read may return negative values */ - if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)sizeof(buf)) { - /* - * to avoid reading very small chunks wait for data before reading - * this is designed to target ~30 bytes at a time - */ - const unsigned character_count = 20; + if ((nread = ::read(uart_fd, buf, sizeof(buf))) < (ssize_t)character_count) { unsigned sleeptime = (1.0f / (_mavlink->get_baudrate() / 10)) * character_count * 1000000; usleep(sleeptime); } From 9647f0622c22b9b1dd3603970a75bb3006a056e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 00:17:44 +0100 Subject: [PATCH 73/92] MAVLink: Comment lied --- src/modules/mavlink/mavlink_receiver.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c4f1cbceec..0c2180a4d8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1800,7 +1800,7 @@ MavlinkReceiver::receive_thread(void *arg) /* * to avoid reading very small chunks wait for data before reading - * this is designed to target ~30 bytes at a time + * this is designed to target one message, so >20 bytes at a time */ const unsigned character_count = 20; From 4076383c4989db17988bc77c9ec5f414f2c77090 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:53:03 +0100 Subject: [PATCH 74/92] Commander: Fix PX4_INFO call --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1a70d0f6dd..67ce676df5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -403,11 +403,11 @@ int commander_main(int argc, char *argv[]) void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason > 0) { + PX4_INFO("%s", reason); } - PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n\n"); + PX4_INFO("usage: commander {start|stop|status|calibrate|check|arm|disarm}\n"); } void print_status() From f4741f8bf1e107160bc0ce531942465b75e25d18 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:03 +0100 Subject: [PATCH 75/92] EKF: Fix output --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 2ea78ddf14..8080fd9c09 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -1672,8 +1672,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[]) PX4_INFO("."); } - PX4_INFO(" "); - return 0; } From f776c57fb27a7ad06325e111bd27b4fcdb1c9a73 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:18 +0100 Subject: [PATCH 76/92] INAV: Clean up output --- .../position_estimator_inav/position_estimator_inav_main.c | 6 +++--- 1 file changed, 3 insertions(+), 3 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 04f3c4d918..aa7fe0d972 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -116,11 +116,11 @@ static inline int max(int val1, int val2) */ static void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); } - PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n\n"); + PX4_INFO("usage: position_estimator_inav {start|stop|status} [-v]\n"); return; } From ad4d841b8afede6f5f65e1ea3c8986b1b57d9ddf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 10:46:29 +0100 Subject: [PATCH 77/92] Mixer command: Clean up output --- src/systemcmds/mixer/mixer.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/systemcmds/mixer/mixer.cpp b/src/systemcmds/mixer/mixer.cpp index 56419714b5..c83eabc9a7 100644 --- a/src/systemcmds/mixer/mixer.cpp +++ b/src/systemcmds/mixer/mixer.cpp @@ -93,12 +93,12 @@ mixer_main(int argc, char *argv[]) static void usage(const char *reason) { - if (reason) { - PX4_INFO("%s\n", reason); + if (reason && *reason) { + PX4_INFO("%s", reason); } - PX4_INFO("usage:\n"); - PX4_INFO(" mixer load \n"); + PX4_INFO("usage:"); + PX4_INFO(" mixer load "); } static int From 50c6be5d25bf61517cda27604caf7bd388f8ed52 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 11:09:09 +0100 Subject: [PATCH 78/92] SDLOG2: Comment lied --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 94719380f1..d68c90cf98 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1898,7 +1898,7 @@ void sdlog2_status() } /** - * @return 0 if file exists + * @return true if file exists */ bool file_exist(const char *filename) { From 7be83bd46e10d66e75438cbb35e4fb9825c343ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 13:15:26 +0100 Subject: [PATCH 79/92] Auto-upload config files --- .travis.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.travis.yml b/.travis.yml index 35c9aa1c28..e6b5239344 100644 --- a/.travis.yml +++ b/.travis.yml @@ -118,7 +118,7 @@ after_success: cp build_px4fmu-v1_default/src/firmware/nuttx/nuttx-px4fmu-v1-default.px4 px4fmu-v1_default.px4 && cp build_px4fmu-v2_default/src/firmware/nuttx/nuttx-px4fmu-v2-default.px4 px4fmu-v2_default.px4 && zip Firmware.zip px4fmu-v1_default.px4 px4fmu-v2_default.px4 - && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ + && ./CI-Tools/s3cmd-put px4fmu-v1_default.px4 px4fmu-v2_default.px4 build_px4fmu-v2_default/parameters.xml build_px4fmu-v2_default/airframes.xml CI-Tools/directory/index.html Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put Firmware.zip archives/Firmware/$TRAVIS_BRANCH/$TRAVIS_BUILD_ID/ && ./CI-Tools/s3cmd-put CI-Tools/directory/index.html archives/Firmware/$TRAVIS_BRANCH/ && ./CI-Tools/s3cmd-put CI-Tools/index.html index.html From 2727333b3d50bd93a9f2e23b8ebe50c3d8ca235f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:09 +0100 Subject: [PATCH 80/92] Telem status: Add USB type --- msg/telemetry_status.msg | 1 + 1 file changed, 1 insertion(+) diff --git a/msg/telemetry_status.msg b/msg/telemetry_status.msg index 221dab79e0..cdcef931af 100644 --- a/msg/telemetry_status.msg +++ b/msg/telemetry_status.msg @@ -2,6 +2,7 @@ uint8 TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0 uint8 TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO = 1 uint8 TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET = 2 uint8 TELEMETRY_STATUS_RADIO_TYPE_WIRE = 3 +uint8 TELEMETRY_STATUS_RADIO_TYPE_USB = 4 uint64 timestamp uint64 heartbeat_time # Time of last received heartbeat from remote system From 0fe2bb588672a6de33c864a4807856ae7ffec846 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:19 +0100 Subject: [PATCH 81/92] Commander: Read USB status --- src/modules/commander/commander.cpp | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 67ce676df5..3573f5d6eb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -180,6 +180,7 @@ static volatile bool thread_should_exit = false; /**< daemon exit flag */ static volatile bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ static bool need_param_autosave = false; /**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */ +static bool _usb_telemetry_active = false; static hrt_abstime commander_boot_timestamp = 0; static unsigned int leds_counter; @@ -200,7 +201,7 @@ static struct home_position_s _home; static unsigned _last_mission_instance = 0; static manual_control_setpoint_s _last_sp_man; -struct vtol_vehicle_status_s vtol_status; +static struct vtol_vehicle_status_s vtol_status; /** * The daemon app only briefly exists to start @@ -1463,6 +1464,11 @@ int commander_thread_main(int argc, char *argv[]) } } + /* set (and don't reset) telemetry via USB as active once a MAVLink connection is up */ + if (telemetry.type == telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB) { + _usb_telemetry_active = true; + } + telemetry_last_heartbeat[i] = telemetry.heartbeat_time; } } @@ -1519,18 +1525,19 @@ int commander_thread_main(int argc, char *argv[]) /* copy avionics voltage */ status.avionics_power_rail_voltage = system_power.voltage5V_v; + /* if the USB hardware connection went away, reboot */ if (status.usb_connected && !system_power.usb_connected) { /* * apparently the USB cable went away but we are still powered, * so lets reset to a classic non-usb state. */ - usleep(100000); mavlink_log_critical(mavlink_fd, "USB disconnected, rebooting.") usleep(400000); px4_systemreset(false); } - status.usb_connected = system_power.usb_connected; + /* finally judge the USB connected state based on software detection */ + status.usb_connected = _usb_telemetry_active; } } From 85ad5a622a0ad4e6accf566ed4ea9357b1287393 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 4 Nov 2015 18:23:30 +0100 Subject: [PATCH 82/92] MAVLink: Send USB telem status --- src/modules/mavlink/mavlink_main.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 0c017c5477..3a2f1d583b 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -700,6 +700,7 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios * _is_usb_uart = true; /* USB has no baudrate, but use a magic number for 'fast' */ _baudrate = 2000000; + _rstatus.type = telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB; } #if defined (__PX4_LINUX) || defined (__PX4_DARWIN) @@ -2096,8 +2097,12 @@ Mavlink::display_status() printf("3DR RADIO\n"); break; + case telemetry_status_s::TELEMETRY_STATUS_RADIO_TYPE_USB: + printf("USB CDC\n"); + break; + default: - printf("UNKNOWN RADIO\n"); + printf("GENERIC LINK OR RADIO\n"); break; } From f6a9647fe61167e77af955ec5d8756253e78f8db Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Fri, 30 Oct 2015 17:52:17 +0530 Subject: [PATCH 83/92] attitude_estimator_q : add external heading support --- .../attitude_estimator_q_main.cpp | 98 +++++++++++++++++-- .../attitude_estimator_q_params.c | 20 ++++ 2 files changed, 112 insertions(+), 6 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a9ffbe0c42..7af8fa3954 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -56,6 +56,8 @@ #include #include #include +#include +#include #include #include @@ -117,22 +119,27 @@ private: int _sensors_sub = -1; int _params_sub = -1; + int _vision_sub = -1; + int _mocap_sub = -1; int _global_pos_sub = -1; orb_advert_t _att_pub = nullptr; struct { param_t w_acc; param_t w_mag; + param_t w_ext_hdg; param_t w_gyro_bias; param_t mag_decl; param_t mag_decl_auto; param_t acc_comp; param_t bias_max; param_t vibe_thresh; + param_t ext_hdg_mode; } _params_handles; /**< handles for interesting parameters */ float _w_accel = 0.0f; float _w_mag = 0.0f; + float _w_ext_hdg = 0.0f; float _w_gyro_bias = 0.0f; float _mag_decl = 0.0f; bool _mag_decl_auto = false; @@ -140,11 +147,18 @@ private: float _bias_max = 0.0f; float _vibration_warning_threshold = 1.0f; hrt_abstime _vibration_warning_timestamp = 0; + int _ext_hdg_mode = 0; Vector<3> _gyro; Vector<3> _accel; Vector<3> _mag; + vision_position_estimate_s _vision = {}; + Vector<3> _vision_hdg; + + att_pos_mocap_s _mocap = {}; + Vector<3> _mocap_hdg; + Quaternion _q; Vector<3> _rates; Vector<3> _gyro_bias; @@ -168,6 +182,7 @@ private: bool _data_good = false; bool _failsafe = false; bool _vibration_warning = false; + bool _ext_hdg_good = false; int _mavlink_fd = -1; @@ -196,12 +211,14 @@ AttitudeEstimatorQ::AttitudeEstimatorQ() : _params_handles.w_acc = param_find("ATT_W_ACC"); _params_handles.w_mag = param_find("ATT_W_MAG"); + _params_handles.w_ext_hdg = param_find("ATT_W_EXT_HDG"); _params_handles.w_gyro_bias = param_find("ATT_W_GYRO_BIAS"); _params_handles.mag_decl = param_find("ATT_MAG_DECL"); _params_handles.mag_decl_auto = param_find("ATT_MAG_DECL_A"); _params_handles.acc_comp = param_find("ATT_ACC_COMP"); _params_handles.bias_max = param_find("ATT_BIAS_MAX"); _params_handles.vibe_thresh = param_find("ATT_VIBE_THRESH"); + _params_handles.ext_hdg_mode = param_find("ATT_EXT_HDG_M"); } /** @@ -269,6 +286,10 @@ void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) void AttitudeEstimatorQ::task_main() { _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + + _vision_sub = orb_subscribe(ORB_ID(vision_position_estimate)); + _mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); _global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position)); @@ -374,6 +395,47 @@ void AttitudeEstimatorQ::task_main() } } + // Update vision and motion capture heading + bool vision_updated = false; + orb_check(_vision_sub, &vision_updated); + + bool mocap_updated = false; + orb_check(_mocap_sub, &mocap_updated); + + if (vision_updated) { + orb_copy(ORB_ID(vision_position_estimate), _vision_sub, &_vision); + math::Quaternion q(_vision.q); + + math::Matrix<3, 3> Rvis = q.to_dcm(); + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rvis is Rwr (robot respect to world) while v is respect to world. + // Hence Rvis must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _vision_hdg = Rvis.transposed() * v; + } + + if (mocap_updated) { + orb_copy(ORB_ID(att_pos_mocap), _mocap_sub, &_mocap); + math::Quaternion q(_mocap.q); + math::Matrix<3, 3> Rmoc = q.to_dcm(); + + math::Vector<3> v(1.0f, 0.0f, 0.4f); + + // Rmoc is Rwr (robot respect to world) while v is respect to world. + // Hence Rmoc must be transposed having (Rwr)' * Vw + // Rrw * Vw = vn. This way we have consistency + _mocap_hdg = Rmoc.transposed() * v; + } + + // Check for timeouts on data + if (_ext_hdg_mode == 1) { + _ext_hdg_good = _vision.timestamp_boot > 0 && (hrt_elapsed_time(&_vision.timestamp_boot) < 500000); + + } else if (_ext_hdg_mode == 2) { + _ext_hdg_good = _mocap.timestamp_boot > 0 && (hrt_elapsed_time(&_mocap.timestamp_boot) < 500000); + } + bool gpos_updated; orb_check(_global_pos_sub, &gpos_updated); @@ -478,6 +540,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) param_get(_params_handles.w_acc, &_w_accel); param_get(_params_handles.w_mag, &_w_mag); + param_get(_params_handles.w_ext_hdg, &_w_ext_hdg); param_get(_params_handles.w_gyro_bias, &_w_gyro_bias); float mag_decl_deg = 0.0f; param_get(_params_handles.mag_decl, &mag_decl_deg); @@ -490,6 +553,7 @@ void AttitudeEstimatorQ::update_parameters(bool force) _acc_comp = acc_comp_int != 0; param_get(_params_handles.bias_max, &_bias_max); param_get(_params_handles.vibe_thresh, &_vibration_warning_threshold); + param_get(_params_handles.ext_hdg_mode, &_ext_hdg_mode); } } @@ -545,12 +609,34 @@ bool AttitudeEstimatorQ::update(float dt) // Angular rate of correction Vector<3> corr; - // Magnetometer correction - // Project mag field vector to global frame and extract XY component - Vector<3> mag_earth = _q.conjugate(_mag); - float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); - // Project magnetometer correction to body frame - corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + if (_ext_hdg_mode > 0 && _ext_hdg_good) { + if (_ext_hdg_mode == 1) { + // Vision heading correction + // Project heading to global frame and extract XY component + Vector<3> vision_hdg_earth = _q.conjugate(_vision_hdg); + float vision_hdg_err = _wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -vision_hdg_err)) * _w_ext_hdg; + } + + if (_ext_hdg_mode == 2) { + // Mocap heading correction + // Project heading to global frame and extract XY component + Vector<3> mocap_hdg_earth = _q.conjugate(_mocap_hdg); + float mocap_hdg_err = _wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0))); + // Project correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mocap_hdg_err)) * _w_ext_hdg; + } + } + + if (_ext_hdg_mode == 0 || !_ext_hdg_good) { + // Magnetometer correction + // Project mag field vector to global frame and extract XY component + Vector<3> mag_earth = _q.conjugate(_mag); + float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl); + // Project magnetometer correction to body frame + corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag; + } // Accelerometer correction // Project 'k' unit vector of earth frame to body frame diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c index d3959db4cf..41b62f04e1 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_params.c +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_params.c @@ -61,6 +61,15 @@ PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f); */ PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f); +/** + * Complimentary filter external heading weight + * + * @group Attitude Q estimator + * @min 0 + * @max 1 + */ +PARAM_DEFINE_FLOAT(ATT_W_EXT_HDG, 0.1f); + /** * Complimentary filter gyroscope bias weight * @@ -93,6 +102,17 @@ PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f); */ PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1); +/** + * External heading usage mode (from Motion capture/Vision) + * Set to 1 to use heading estimate from vision. + * Set to 2 to use heading from motion capture. + * + * @group Attitude Q estimator + * @min 0 + * @max 2 + */ +PARAM_DEFINE_INT32(ATT_EXT_HDG_M, 0); + /** * Enable acceleration compensation based on GPS * velocity. From 87269c0fab820f37fd1b8981f8b32ec0fbdc46db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:00 +0100 Subject: [PATCH 84/92] Update autostart docs --- ROMFS/px4fmu_common/init.d/11001_hexa_cox | 11 +++++++++++ ROMFS/px4fmu_common/init.d/3033_wingwing | 5 ++++- 2 files changed, 15 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox index f85b06f87d..8c9c28fe37 100644 --- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox +++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox @@ -4,6 +4,17 @@ # # @type Hexarotor Coaxial # +# @output MAIN1 front right top, CW; angle:60; direction:CW +# @output MAIN2 front right bottom, CCW; angle:60; direction:CCW +# @output MAIN3 back top, CW; angle:180; direction:CW +# @output MAIN4 back bottom, CCW; angle:180; direction:CCW +# @output MAIN5 front left top, CW; angle:-60; direction:CW +# @output MAIN6 front left bottom, CCW;angle:-60; direction:CCW +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# # @maintainer Lorenz Meier # diff --git a/ROMFS/px4fmu_common/init.d/3033_wingwing b/ROMFS/px4fmu_common/init.d/3033_wingwing index 25a14312f3..3a0aa28150 100644 --- a/ROMFS/px4fmu_common/init.d/3033_wingwing +++ b/ROMFS/px4fmu_common/init.d/3033_wingwing @@ -14,7 +14,7 @@ # @output AUX2 feed-through of RC AUX2 channel # @output AUX3 feed-through of RC AUX3 channel # -# @maintainer Simon Wilks +# @maintainer Lorenz Meier # sh /etc/init.d/rc.fw_defaults @@ -39,6 +39,9 @@ then param set FW_RR_P 0.04 fi +# Configure this as plane +set MAV_TYPE 1 +# Set mixer set MIXER wingwing # Provide ESC a constant 1000 us pulse set PWM_OUT 4 From 9b1fefb8d0044e63bb7c0849485d8d49cb46ed10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:17 +0100 Subject: [PATCH 85/92] Remove unused build file --- src/modules/systemlib/mixer/multi_tables.mk | 42 --------------------- 1 file changed, 42 deletions(-) delete mode 100644 src/modules/systemlib/mixer/multi_tables.mk diff --git a/src/modules/systemlib/mixer/multi_tables.mk b/src/modules/systemlib/mixer/multi_tables.mk deleted file mode 100644 index c537c83a47..0000000000 --- a/src/modules/systemlib/mixer/multi_tables.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 Anton Matosov . All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - - -SELF_DIR := $(dir $(lastword $(MAKEFILE_LIST))) -MULTI_TABLES := $(SELF_DIR)multi_tables.py - -# Add explicit dependency, as implicit one doesn't work often. -$(SELF_DIR)mixer_multirotor.cpp : $(SELF_DIR)mixer_multirotor.generated.h - -$(SELF_DIR)mixer_multirotor.generated.h : $(MULTI_TABLES) - $(Q) $(PYTHON) $(MULTI_TABLES) > $(SELF_DIR)mixer_multirotor.generated.h From 628e5c8649150ce6bf399a0a0aba81da5895e612 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:29:44 +0100 Subject: [PATCH 86/92] Mixer: Fix naming inconsistency --- src/modules/systemlib/mixer/mixer.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index 43c815238c..5ed152127a 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -481,7 +481,7 @@ public: * compared to thrust. * @param yaw_wcale Scaling factor applied to yaw inputs compared * to thrust. - * @param deadband Minumum rotor control output value; usually + * @param idle_speed Minumum rotor control output value; usually * tuned to ensure that rotors never stall at the * low end of their control range. */ @@ -491,7 +491,7 @@ public: float roll_scale, float pitch_scale, float yaw_scale, - float deadband); + float idle_speed); ~MultirotorMixer(); /** From ca7a7dfd102893621edd8bfc4992c72dbac140ec Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 5 Nov 2015 21:30:10 +0100 Subject: [PATCH 87/92] Output rotor config as XML attributes --- Tools/px4airframes/xmlout.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 427b1090a2..994fb6eeb2 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -72,9 +72,13 @@ class XMLOutput(): xml_field.text = value for code in param.GetOutputCodes(): value = param.GetOutputValue(code) + valstrs = value.split(";") xml_field = ET.SubElement(xml_param, "output") xml_field.attrib["name"] = code - xml_field.text = value + for attrib in valstrs[1:]: + attribstrs = attrib.split(":") + xml_field.attrib[attribstrs[0].strip()] = attribstrs[1].strip() + xml_field.text = valstrs[0] if last_param_name != param.GetName(): board_specific_param_set = False indent(xml_parameters) From 083dbbb71b59b00c5c51c47f2eb6b700b95a268a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:53:38 +0100 Subject: [PATCH 88/92] IO firmware: Do mixer load as block operation --- src/modules/px4iofirmware/mixer.cpp | 17 +---------------- src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/registers.c | 13 ++++++++++++- 3 files changed, 15 insertions(+), 17 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index b15ec09610..0a0962367d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -88,9 +88,6 @@ static int mixer_callback(uintptr_t handle, static MixerGroup mixer_group(mixer_callback, 0); -/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ -static void mixer_set_failsafe(); - void mixer_tick(void) { @@ -479,15 +476,6 @@ mixer_handle_text(const void *buffer, size_t length) /* if anything was parsed */ if (resid != mixer_text_length) { - /* only set mixer ok if no residual is left over */ - if (resid == 0) { - r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; - - } else { - /* not yet reached the end of the mixer, set as not ok */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_MIXER_OK; - } - isr_debug(2, "used %u", mixer_text_length - resid); /* copy any leftover text to the base of the buffer for re-use */ @@ -496,9 +484,6 @@ mixer_handle_text(const void *buffer, size_t length) } mixer_text_length = resid; - - /* update failsafe values */ - mixer_set_failsafe(); } break; @@ -507,7 +492,7 @@ mixer_handle_text(const void *buffer, size_t length) return 0; } -static void +void mixer_set_failsafe() { /* diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index a5e7a5500c..51ed9c6e40 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -190,6 +190,8 @@ extern pwm_limit_t pwm_limit; */ extern void mixer_tick(void); extern int mixer_handle_text(const void *buffer, size_t length); +/* Set the failsafe values of all mixed channels (based on zero throttle, controls centered) */ +extern void mixer_set_failsafe(void); /** * Safety switch/LED. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 46852003c8..18f5db5645 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -469,8 +469,19 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * Allow FMU override of arming state (to allow in-air restores), * but only if the arming state is not in sync on the IO side. */ - if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & value) { + r_status_flags |= PX4IO_P_STATUS_FLAGS_MIXER_OK; + + } else if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { r_status_flags = value; + + } + + if (PX4IO_P_STATUS_FLAGS_MIXER_OK & r_status_flags) { + + /* update failsafe values, now that the mixer is set to ok */ + mixer_set_failsafe(); } break; From 1baf103ee7458f0a6c11bb766101226a58b37f3f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 22:54:08 +0100 Subject: [PATCH 89/92] IO driver: Set mixer ok flag once the mixer load completed --- src/drivers/px4io/px4io.cpp | 32 ++++++++++++++------------------ 1 file changed, 14 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9a6581437e..80c5824019 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2090,12 +2090,11 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } while (buflen > 0); - /* ensure a closing newline */ - msg->text[0] = '\n'; - msg->text[1] = '\0'; - int ret; + /* send the closing newline */ + msg->text[0] = '\n'; + msg->text[1] = '\0'; for (int i = 0; i < 30; i++) { /* failed, but give it a 2nd shot */ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); @@ -2108,27 +2107,24 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) } } - if (ret) { - return ret; + if (ret == 0) { + /* success, exit */ + break; } retries--; - DEVICE_LOG("mixer sent"); + } while (retries > 0); - } while (retries > 0 && (!(io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK))); + if (retries == 0) { + mavlink_and_console_log_info(_mavlink_fd, "[IO] mixer upload fail"); + /* load must have failed for some reason */ + return -EINVAL; - /* check for the mixer-OK flag */ - if (io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS) & PX4IO_P_STATUS_FLAGS_MIXER_OK) { - mavlink_log_info(_mavlink_fd, "[IO] mixer upload ok"); - return 0; + } else { + /* all went well, set the mixer ok flag */ + return io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_MIXER_OK); } - - DEVICE_LOG("mixer rejected by IO"); - mavlink_log_info(_mavlink_fd, "[IO] mixer upload fail"); - - /* load must have failed for some reason */ - return -EINVAL; } void From 8154fcdc8aa87c680a69b92dfbc467a63fbf94d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 3 Nov 2015 23:10:06 +0100 Subject: [PATCH 90/92] IO driver: Fix code style --- src/drivers/px4io/px4io.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 80c5824019..719bacb41f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2095,6 +2095,7 @@ PX4IO::mixer_send(const char *buf, unsigned buflen, unsigned retries) /* send the closing newline */ msg->text[0] = '\n'; msg->text[1] = '\0'; + for (int i = 0; i < 30; i++) { /* failed, but give it a 2nd shot */ ret = io_reg_set(PX4IO_PAGE_MIXERLOAD, 0, (uint16_t *)frame, (sizeof(px4io_mixdata) + 2) / 2); From 256f81fe4d058bf0972fceb68600f9c51e71e2c6 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 5 Nov 2015 14:21:57 -0700 Subject: [PATCH 91/92] add new config for QAV250 racing quad --- ROMFS/px4fmu_common/init.d/4009_qav250 | 39 ++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/4009_qav250 diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 new file mode 100644 index 0000000000..9165a78168 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -0,0 +1,39 @@ +#!nsh +# +# @name Lumenier QAV250 +# +# @type Quadrotor x +# +# @output AUX1 feed-through of RC AUX1 channel +# @output AUX2 feed-through of RC AUX2 channel +# @output AUX3 feed-through of RC AUX3 channel +# +# @maintainer Mark Whitehorn +# + +sh /etc/init.d/4001_quad_x + +if [ $AUTOCNF == yes ] +then + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.08 + param set MC_ROLLRATE_I 0.05 + param set MC_ROLLRATE_D 0.003 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.08 + param set MC_PITCHRATE_I 0.1 + param set MC_PITCHRATE_D 0.003 + param set MC_YAW_P 2.8 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.1 + param set MC_YAWRATE_D 0.0 + param set PWM_MIN 1230 + param set PWM_THR_MIN 0.06 + param set PWM_MANTHR_MIN 0.06 +fi + +# Transitional support: ensure suitable PWM min/max param values +#if param compare PWM_MIN 1075 +#then +# param set PWM_MIN 1230 +#fi From fc62461e5b99c1ebb346bdefd8c8e99f12553efb Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Thu, 5 Nov 2015 20:39:43 -0700 Subject: [PATCH 92/92] lower default PWM_MIN to 1075 --- ROMFS/px4fmu_common/init.d/4009_qav250 | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/4009_qav250 b/ROMFS/px4fmu_common/init.d/4009_qav250 index 9165a78168..b27dc9b3e3 100644 --- a/ROMFS/px4fmu_common/init.d/4009_qav250 +++ b/ROMFS/px4fmu_common/init.d/4009_qav250 @@ -27,13 +27,8 @@ then param set MC_YAWRATE_P 0.2 param set MC_YAWRATE_I 0.1 param set MC_YAWRATE_D 0.0 - param set PWM_MIN 1230 + param set PWM_MIN 1075 param set PWM_THR_MIN 0.06 param set PWM_MANTHR_MIN 0.06 fi -# Transitional support: ensure suitable PWM min/max param values -#if param compare PWM_MIN 1075 -#then -# param set PWM_MIN 1230 -#fi