From ba3681d3a09df42f5926c1cd1a407d5ed4b34bd6 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 12:34:51 -0500 Subject: Updated backside controller/ added backside config. --- src/modules/fixedwing_backside/fixedwing.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp index 3876e4630..567efeb35 100644 --- a/src/modules/fixedwing_backside/fixedwing.hpp +++ b/src/modules/fixedwing_backside/fixedwing.hpp @@ -255,13 +255,13 @@ private: BlockWaypointGuidance _guide; // block params - BlockParam _trimAil; - BlockParam _trimElv; - BlockParam _trimRdr; - BlockParam _trimThr; - BlockParam _trimV; - BlockParam _vCmd; - BlockParam _crMax; + BlockParamFloat _trimAil; + BlockParamFloat _trimElv; + BlockParamFloat _trimRdr; + BlockParamFloat _trimThr; + BlockParamFloat _trimV; + BlockParamFloat _vCmd; + BlockParamFloat _crMax; struct pollfd _attPoll; vehicle_global_position_set_triplet_s _lastPosCmd; -- cgit v1.2.3 From 5c66899bfbb76cd973f249338507267cdffd0fad Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:23:39 -0500 Subject: Added local position pub to att_pos_esitmator_ekf --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 69 ++++++++++++++++++------- src/modules/att_pos_estimator_ekf/KalmanNav.hpp | 11 ++-- 2 files changed, 56 insertions(+), 24 deletions(-) (limited to 'src') diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 18fb6dcbc..ecca04dd7 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -41,6 +41,7 @@ #include "KalmanNav.hpp" #include +#include // constants // Titterton pg. 52 @@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : // attitude representations C_nb(), q(), - _accel_sub(-1), - _gyro_sub(-1), - _mag_sub(-1), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), + _localPos(&getPublications(), ORB_ID(vehicle_local_position)), _att(&getPublications(), ORB_ID(vehicle_attitude)), // timestamps _pubTimeStamp(hrt_absolute_time()), @@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : phi(0), theta(0), psi(0), vN(0), vE(0), vD(0), lat(0), lon(0), alt(0), + lat0(0), lon0(0), alt0(0), // parameters for ground station _vGyro(this, "V_GYRO"), _vAccel(this, "V_ACCEL"), @@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : lon = 0.0f; alt = 0.0f; - // gyro, accel and mag subscriptions - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag)); - - struct accel_report accel; - orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel); - - struct mag_report mag; - orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag); - // initialize rotation quaternion with a single raw sensor measurement - q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z); + _sensors.update(); + q = init( + _sensors.accelerometer_m_s2[0], + _sensors.accelerometer_m_s2[1], + _sensors.accelerometer_m_s2[2], + _sensors.magnetometer_ga[0], + _sensors.magnetometer_ga[1], + _sensors.magnetometer_ga[2]); // initialize dcm C_nb = Dcm(q); @@ -252,11 +248,21 @@ void KalmanNav::update() setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); + // set reference position for + // local position + lat0 = lat; + lon0 = lon; + alt0 = alt; + // XXX map_projection has internal global + // states that multiple things could change, + // should make map_projection take reference + // lat/lon and not have init + map_projection_init(lat0, lon0); _positionInitialized = true; warnx("initialized EKF state with GPS\n"); warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n", double(vN), double(vE), double(vD), - lat, lon, alt); + lat, lon, double(alt)); } // prediction step @@ -311,7 +317,7 @@ void KalmanNav::updatePublications() { using namespace math; - // position publication + // global position publication _pos.timestamp = _pubTimeStamp; _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; @@ -324,6 +330,31 @@ void KalmanNav::updatePublications() _pos.vz = vD; _pos.yaw = psi; + // local position publication + float x; + float y; + bool landed = alt < (alt0 + 0.1); // XXX improve? + map_projection_project(lat, lon, &x, &y); + _localPos.timestamp = _pubTimeStamp; + _localPos.xy_valid = true; + _localPos.z_valid = true; + _localPos.v_xy_valid = true; + _localPos.v_z_valid = true; + _localPos.x = x; + _localPos.y = y; + _localPos.z = alt0 - alt; + _localPos.vx = vN; + _localPos.vy = vE; + _localPos.vz = vD; + _localPos.yaw = psi; + _localPos.xy_global = true; + _localPos.z_global = true; + _localPos.ref_timestamp = _pubTimeStamp; + _localPos.ref_lat = getLatDegE7(); + _localPos.ref_lon = getLonDegE7(); + _localPos.ref_alt = 0; + _localPos.landed = landed; + // attitude publication _att.timestamp = _pubTimeStamp; _att.roll = phi; @@ -347,8 +378,10 @@ void KalmanNav::updatePublications() // selectively update publications, // do NOT call superblock do-all method - if (_positionInitialized) + if (_positionInitialized) { _pos.update(); + _localPos.update(); + } if (_attitudeInitialized) _att.update(); diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp index 799fc2fb9..a69bde1a6 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp @@ -52,6 +52,7 @@ #include #include +#include #include #include #include @@ -142,12 +143,8 @@ protected: control::UOrbSubscription _param_update; /**< parameter update sub. */ // publications control::UOrbPublication _pos; /**< position pub. */ + control::UOrbPublication _localPos; /**< local position pub. */ control::UOrbPublication _att; /**< attitude pub. */ - - int _accel_sub; /**< Accelerometer subscription */ - int _gyro_sub; /**< Gyroscope subscription */ - int _mag_sub; /**< Magnetometer subscription */ - // time stamps uint64_t _pubTimeStamp; /**< output data publication time stamp */ uint64_t _predictTimeStamp; /**< prediction time stamp */ @@ -164,8 +161,10 @@ protected: float phi, theta, psi; /**< 3-2-1 euler angles */ float vN, vE, vD; /**< navigation velocity, m/s */ double lat, lon; /**< lat, lon radians */ - float alt; /**< altitude, meters */ // parameters + float alt; /**< altitude, meters */ + double lat0, lon0; /**< reference latitude and longitude */ + float alt0; /**< refeerence altitude (ground height) */ control::BlockParamFloat _vGyro; /**< gyro process noise */ control::BlockParamFloat _vAccel; /**< accelerometer process noise */ control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ -- cgit v1.2.3 From ea156f556fe6815e01ea6973bb07de8299851c76 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:24:07 -0500 Subject: Added local position publication to mavlink receiver for HIL. --- src/modules/mavlink/mavlink_receiver.cpp | 65 +++++++++++++++++++++++++++----- 1 file changed, 55 insertions(+), 10 deletions(-) (limited to 'src') diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index c51a6de08..bfccd5fb0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -72,6 +72,7 @@ #include #include #include +#include __BEGIN_DECLS @@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; struct vehicle_global_position_s hil_global_pos; +struct vehicle_local_position_s hil_local_pos; struct vehicle_attitude_s hil_attitude; struct vehicle_gps_position_s hil_gps; struct sensor_combined_s hil_sensors; struct battery_status_s hil_battery_status; static orb_advert_t pub_hil_global_pos = -1; +static orb_advert_t pub_hil_local_pos = -1; static orb_advert_t pub_hil_attitude = -1; static orb_advert_t pub_hil_gps = -1; static orb_advert_t pub_hil_sensors = -1; @@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; +// variables for HIL reference position +static int32_t lat0 = 0; +static int32_t lon0 = 0; +static double alt0 = 0; + static void handle_message(mavlink_message_t *msg) { @@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg) orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed); } - hil_global_pos.valid = true; - hil_global_pos.lat = hil_state.lat; - hil_global_pos.lon = hil_state.lon; - hil_global_pos.alt = hil_state.alt / 1000.0f; - hil_global_pos.vx = hil_state.vx / 100.0f; - hil_global_pos.vy = hil_state.vy / 100.0f; - hil_global_pos.vz = hil_state.vz / 100.0f; - - /* set timestamp and notify processes (broadcast) */ - hil_global_pos.timestamp = hrt_absolute_time(); + uint64_t timestamp = hrt_absolute_time(); + // publish global position if (pub_hil_global_pos > 0) { orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos); + // global position packet + hil_global_pos.timestamp = timestamp; + hil_global_pos.valid = true; + hil_global_pos.lat = hil_state.lat; + hil_global_pos.lon = hil_state.lon; + hil_global_pos.alt = hil_state.alt / 1000.0f; + hil_global_pos.vx = hil_state.vx / 100.0f; + hil_global_pos.vy = hil_state.vy / 100.0f; + hil_global_pos.vz = hil_state.vz / 100.0f; } else { pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos); } + // publish local position + if (pub_hil_local_pos > 0) { + float x; + float y; + bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve? + double lat = hil_state.lat*1e-7; + double lon = hil_state.lon*1e-7; + map_projection_project(lat, lon, &x, &y); + hil_local_pos.timestamp = timestamp; + hil_local_pos.xy_valid = true; + hil_local_pos.z_valid = true; + hil_local_pos.v_xy_valid = true; + hil_local_pos.v_z_valid = true; + hil_local_pos.x = x; + hil_local_pos.y = y; + hil_local_pos.z = alt0 - hil_state.alt/1000.0f; + hil_local_pos.vx = hil_state.vx/100.0f; + hil_local_pos.vy = hil_state.vy/100.0f; + hil_local_pos.vz = hil_state.vz/100.0f; + hil_local_pos.yaw = hil_attitude.yaw; + hil_local_pos.xy_global = true; + hil_local_pos.z_global = true; + hil_local_pos.ref_timestamp = timestamp; + hil_local_pos.ref_lat = hil_state.lat; + hil_local_pos.ref_lon = hil_state.lon; + hil_local_pos.ref_alt = alt0; + hil_local_pos.landed = landed; + orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos); + } else { + pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos); + lat0 = hil_state.lat; + lon0 = hil_state.lon; + alt0 = hil_state.alt / 1000.0f; + map_projection_init(hil_state.lat, hil_state.lon); + } + /* Calculate Rotation Matrix */ math::Quaternion q(hil_state.attitude_quaternion); math::Dcm C_nb(q); -- cgit v1.2.3 From 2138a1c816e1856245bf9cb08dcd1304b1f827bb Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 15:24:34 -0500 Subject: Improved mode mapping for fixedwing_backside. --- src/modules/fixedwing_backside/fixedwing.cpp | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) (limited to 'src') diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index d65045d68..dd067e5c4 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // TODO use vehicle_control_mode here? + if (_status.main_state == MAIN_STATE_AUTO) { + // TODO use vehicle_control_mode here? // update guidance _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); } @@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || - _status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + if (_status.main_state != MAIN_STATE_AUTO) { - // update guidance - _guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current); - - // calculate velocity, XXX should be airspeed, but using ground speed for now - // for the purpose of control we will limit the velocity feedback between + // calculate velocity, XXX should be airspeed, + // but using ground speed for now for the purpose + // of control we will limit the velocity feedback between // the min/max velocity float v = _vLimit.update(sqrtf( _pos.vx * _pos.vx + @@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update() // a first binary release can be targeted. // This is not a hack, but a design choice. - /* do not limit in HIL */ + // do not limit in HIL if (_status.hil_state != HIL_STATE_ON) { /* limit to value of manual throttle */ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? _actuators.control[CH_THR] : _manual.throttle; } - } else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) { // TODO use vehicle_control_mode here? + } else if (_status.main_state == MAIN_STATE_MANUAL) { _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) { // TODO use vehicle_control_mode here? + + } else if (_status.main_state == MAIN_STATE_SEATBELT || + _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between // the min/max velocity -- cgit v1.2.3 From 1ffb71946d6291b0e6c49515a07b67e3787ed785 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Thu, 14 Nov 2013 16:15:30 -0500 Subject: Fixed backside automode typo. --- src/modules/fixedwing_backside/fixedwing.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index dd067e5c4..6dc19df41 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -167,7 +167,7 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.main_state != MAIN_STATE_AUTO) { + if (_status.main_state == MAIN_STATE_AUTO) { // calculate velocity, XXX should be airspeed, // but using ground speed for now for the purpose -- cgit v1.2.3 From 39634d100104b64f205b69017562b3ac549cf264 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Nov 2013 16:07:06 +0400 Subject: px4io driver: bug fixed --- src/drivers/px4io/px4io.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08e697b9f..ef6ca04e9 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -2530,7 +2530,7 @@ px4io_main(int argc, char *argv[]) } PX4IO_Uploader *up; - const char *fn[3]; + const char *fn[4]; /* work out what we're uploading... */ if (argc > 2) { -- cgit v1.2.3