From 839710daf841a9528a58e915f8b04484bf54e7dc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 14:42:59 +0200 Subject: [PATCH 001/138] Update offboard control uorb topic Work towards supporting the new external setpoint mavlink messages --- src/modules/commander/commander.cpp | 16 ++---- src/modules/mavlink/mavlink_receiver.cpp | 28 +++++++++ src/modules/mavlink/mavlink_receiver.h | 1 + src/modules/navigator/offboard.cpp | 57 ++++++++++++------- .../uORB/topics/offboard_control_setpoint.h | 36 +++++++++--- 5 files changed, 97 insertions(+), 41 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 38297745df..7185eebc41 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -946,7 +946,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Perform system checks (again) once params are loaded and MAVLink is up. */ - if (!system_checked && mavlink_fd && + if (!system_checked && mavlink_fd && (telemetry.heartbeat_time > 0) && (hrt_elapsed_time(&telemetry.heartbeat_time) < 1 * 1000 * 1000)) { @@ -1847,21 +1847,17 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; - case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = true; - control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_climb_rate_enabled = true; - control_mode.flag_control_position_enabled = true; /* XXX: hack for now */ - control_mode.flag_control_velocity_enabled = true; - break; - case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: control_mode.flag_control_rates_enabled = true; control_mode.flag_control_attitude_enabled = true; control_mode.flag_control_altitude_enabled = true; control_mode.flag_control_climb_rate_enabled = true; control_mode.flag_control_position_enabled = true; control_mode.flag_control_velocity_enabled = true; + //XXX: the flags could depend on sp_offboard.ignore break; default: control_mode.flag_control_rates_enabled = false; diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3747ad3ba2..73222637fe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -154,6 +154,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_quad_swarm_roll_pitch_yaw_thrust(msg); break; + case MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL: + handle_message_local_ned_position_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -393,6 +397,30 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message } } +void +MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg) +{ + mavlink_local_ned_position_setpoint_external_t local_ned_position_setpoint_external; + mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + + //XXX check if target system/component is correct + + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 65ef884a24..658f515626 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -115,6 +115,7 @@ private: void handle_message_set_mode(mavlink_message_t *msg); void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); + void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index ef7d11a039..27ce46a1d6 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -79,33 +79,46 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) bool changed = false; /* copy offboard setpoints to the corresponding topics */ - if (_navigator->get_control_mode()->flag_control_position_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; + if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { + /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_position_enabled && + (!_offboard_control_sp.ignore[0] && + !_offboard_control_sp.ignore[1] && + !_offboard_control_sp.ignore[2])) { + /* position control */ + pos_sp_triplet->current.x = _offboard_control_sp.p1; + pos_sp_triplet->current.y = _offboard_control_sp.p2; + pos_sp_triplet->current.yaw = _offboard_control_sp.p3; + pos_sp_triplet->current.z = -_offboard_control_sp.p4; - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.position_valid = true; - changed = true; + changed = true; - } else if (_navigator->get_control_mode()->flag_control_velocity_enabled - && _offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; + } + /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not + * support deactivation of individual directions) */ + if (_navigator->get_control_mode()->flag_control_velocity_enabled && + (!_offboard_control_sp.ignore[3] && + !_offboard_control_sp.ignore[4] && + !_offboard_control_sp.ignore[5])) { + /* velocity control */ + pos_sp_triplet->current.vx = _offboard_control_sp.p2; + pos_sp_triplet->current.vy = _offboard_control_sp.p1; + pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; + pos_sp_triplet->current.vz = _offboard_control_sp.p4; - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; + pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; + pos_sp_triplet->current.valid = true; + pos_sp_triplet->current.velocity_valid = true; - changed = true; + changed = true; + } + + //XXX: map acceleration setpoint once supported in setpoint triplet } return changed; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index d7b131e3c3..427ccc6c8c 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -53,11 +53,22 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT = 0, OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1, OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2, - OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3, - OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED = 3, + OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED = 4, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, + OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, + OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ +}; + +enum OFFBOARD_CONTROL_FRAME { + OFFBOARD_CONTROL_FRAME_LOCAL_NED = 0, + OFFBOARD_CONTROL_FRAME_LOCAL_OFFSET_NED = 1, + OFFBOARD_CONTROL_FRAME_BODY_NED = 2, + OFFBOARD_CONTROL_FRAME_BODY_OFFSET_NED = 3, + OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; /** @@ -70,10 +81,17 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - float p1; /**< ailerons roll / roll rate input */ - float p2; /**< elevator / pitch / pitch rate */ - float p3; /**< rudder / yaw rate / yaw */ - float p4; /**< throttle / collective thrust / altitude */ + double p1; /**< ailerons / roll / x pos / lat */ + double p2; /**< elevator / pitch / y pos / lon */ + float p3; /**< rudder / yaw / z pos / alt */ + float p4; /**< throttle / x vel */ + float p5; /**< roll rate / y vel */ + float p6; /**< pitch rate / z vel */ + float p7; /**< yaw rate / x acc */ + float p8; /**< y acc */ + float p9; /**< z acc */ + + bool ignore[9]; /**< if field i is set to true, pi should be ignored */ float override_mode_switch; From cbd602c27cb081502a2f1f1d910abdd1c2d7be13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 15:00:32 +0200 Subject: [PATCH 002/138] check sysid and compid for external setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 73222637fe..f2b9a4e1e8 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -406,18 +406,20 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); - //XXX check if target system/component is correct + if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && + mavlink_system.compid == local_ned_position_setpoint_external.target_component) { - /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // - offboard_control_sp.timestamp = hrt_absolute_time(); + /* convert mavlink type (local, NED) to uORB offboard control struct */ + //XXX do the conversion + // + offboard_control_sp.timestamp = hrt_absolute_time(); - if (_offboard_control_sp_pub < 0) { - _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); - } else { - orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } } } From 822403e34b5e1b4adf15783b5bd701e1f52484fe Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:20:58 +0200 Subject: [PATCH 003/138] uorb offboard control topic: add force sp flag The flag has the same meaning as bit 10 of type_mask in MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/uORB/topics/offboard_control_setpoint.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 427ccc6c8c..2741f0c073 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -91,7 +91,9 @@ struct offboard_control_setpoint_s { float p8; /**< y acc */ float p9; /**< z acc */ + //XXX: use a bitmask with wrapper functions instead bool ignore[9]; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ float override_mode_switch; From 31474a75fd1ad6e86909bf4af2b484d1decd932a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 3 Jul 2014 16:22:58 +0200 Subject: [PATCH 004/138] parsing of MAVLINK_MSG_ID_LOCAL_NED_POSITION_SETPOINT_EXTERNAL --- src/modules/mavlink/mavlink_receiver.cpp | 34 ++++++++++++++++++++++-- 1 file changed, 32 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index f2b9a4e1e8..c8e8c6d513 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,38 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_system.compid == local_ned_position_setpoint_external.target_component) { /* convert mavlink type (local, NED) to uORB offboard control struct */ - //XXX do the conversion - // + switch (local_ned_position_setpoint_external.coordinate_frame) { + case MAV_FRAME_LOCAL_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED; + break; + case MAV_FRAME_LOCAL_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED; + break; + case MAV_FRAME_BODY_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED; + break; + case MAV_FRAME_BODY_OFFSET_NED: + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED; + break; + default: + /* invalid setpoint, avoid publishing */ + return; + } + offboard_control_sp.p1 = local_ned_position_setpoint_external.x; + offboard_control_sp.p2 = local_ned_position_setpoint_external.y; + offboard_control_sp.p3 = local_ned_position_setpoint_external.z; + offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; + offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; + offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; + offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; + offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; + offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + for (int i = 0; i < 9; i++) { + offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + } + + offboard_control_sp.timestamp = hrt_absolute_time(); if (_offboard_control_sp_pub < 0) { From 2d26e913921ce80e509bd79462319d09d12c2171 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 7 Jul 2014 13:00:45 +0200 Subject: [PATCH 005/138] WIP, change uorb offboard control sp topic --- src/modules/mavlink/mavlink_receiver.cpp | 30 ++++++++-------- src/modules/navigator/offboard.cpp | 30 ++++++++-------- .../uORB/topics/offboard_control_setpoint.h | 34 +++++++++++++------ 3 files changed, 55 insertions(+), 39 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b72bfa858c..e1a74f599b 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -379,10 +379,11 @@ MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); /* Convert values * 1000 back */ - offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; - offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; + //XXX: convert to quaternion + //offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f; + //offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f; offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode; @@ -427,18 +428,19 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes /* invalid setpoint, avoid publishing */ return; } - offboard_control_sp.p1 = local_ned_position_setpoint_external.x; - offboard_control_sp.p2 = local_ned_position_setpoint_external.y; - offboard_control_sp.p3 = local_ned_position_setpoint_external.z; - offboard_control_sp.p4 = local_ned_position_setpoint_external.vx; - offboard_control_sp.p5 = local_ned_position_setpoint_external.vy; - offboard_control_sp.p6 = local_ned_position_setpoint_external.vz; - offboard_control_sp.p7 = local_ned_position_setpoint_external.afx; - offboard_control_sp.p8 = local_ned_position_setpoint_external.afy; - offboard_control_sp.p9 = local_ned_position_setpoint_external.afz; + offboard_control_sp.position[0] = local_ned_position_setpoint_external.x; + offboard_control_sp.position[1] = local_ned_position_setpoint_external.y; + offboard_control_sp.position[2] = local_ned_position_setpoint_external.z; + offboard_control_sp.velocity[0] = local_ned_position_setpoint_external.vx; + offboard_control_sp.velocity[1] = local_ned_position_setpoint_external.vy; + offboard_control_sp.velocity[2] = local_ned_position_setpoint_external.vz; + offboard_control_sp.acceleration[0] = local_ned_position_setpoint_external.afx; + offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; + offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore[i] = (bool)(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 27ce46a1d6..71a14c029d 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -83,14 +83,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_position_enabled && - (!_offboard_control_sp.ignore[0] && - !_offboard_control_sp.ignore[1] && - !_offboard_control_sp.ignore[2])) { + (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.p1; - pos_sp_triplet->current.y = _offboard_control_sp.p2; - pos_sp_triplet->current.yaw = _offboard_control_sp.p3; - pos_sp_triplet->current.z = -_offboard_control_sp.p4; + pos_sp_triplet->current.x = _offboard_control_sp.position[0]; + pos_sp_triplet->current.y = _offboard_control_sp.position[1]; + //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; + //XXX: copy yaw + pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; @@ -102,14 +103,15 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!_offboard_control_sp.ignore[3] && - !_offboard_control_sp.ignore[4] && - !_offboard_control_sp.ignore[5])) { + (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.p2; - pos_sp_triplet->current.vy = _offboard_control_sp.p1; - pos_sp_triplet->current.yawspeed = _offboard_control_sp.p3; - pos_sp_triplet->current.vz = _offboard_control_sp.p4; + pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; + pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; +// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; +// //XXX: copy yaw speed + pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; pos_sp_triplet->current.valid = true; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 2741f0c073..688135533e 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -81,19 +81,15 @@ struct offboard_control_setpoint_s { enum OFFBOARD_CONTROL_MODE mode; /**< The current control inputs mode */ - double p1; /**< ailerons / roll / x pos / lat */ - double p2; /**< elevator / pitch / y pos / lon */ - float p3; /**< rudder / yaw / z pos / alt */ - float p4; /**< throttle / x vel */ - float p5; /**< roll rate / y vel */ - float p6; /**< pitch rate / z vel */ - float p7; /**< yaw rate / x acc */ - float p8; /**< y acc */ - float p9; /**< z acc */ + double position[3]; /**< lat, lon, alt / x, y, z */ + float velocity[3]; /**< x vel, y vel, z vel */ + float acceleration[3]; /**< x acc, y acc, z acc */ + float attitude[4]; /**< attitude of vehicle (quaternion) */ + float attitude_rate[3]; /**< body angular rates (x, y, z) */ //XXX: use a bitmask with wrapper functions instead - bool ignore[9]; /**< if field i is set to true, pi should be ignored */ - bool isForceSetpoint; /**< if set to true: p7 to p9 should be interpreted as force instead of acceleration */ + uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -107,6 +103,22 @@ struct offboard_control_setpoint_s { * @} */ +inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << index)); +} + +inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); +} + +inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); +} + +inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { + return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); From 2c74babafcc3eae4bd513185395ac754ada6d86a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 11:16:19 +0200 Subject: [PATCH 006/138] fix merge of mavlink submodule --- mavlink/include/mavlink/v1.0 | 1 + 1 file changed, 1 insertion(+) create mode 160000 mavlink/include/mavlink/v1.0 diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 new file mode 160000 index 0000000000..3711190d23 --- /dev/null +++ b/mavlink/include/mavlink/v1.0 @@ -0,0 +1 @@ +Subproject commit 3711190d23d9928ea0687e00621c8d9ecf145f50 From 19017f100101dbf638bb91f7a520296f979ebb32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:29:59 +0200 Subject: [PATCH 007/138] remove comment --- src/modules/uORB/topics/offboard_control_setpoint.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 688135533e..809412fc09 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -87,7 +87,6 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ - //XXX: use a bitmask with wrapper functions instead uint16_t ignore; /**< if field i is set to true, pi should be ignored */ bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ From cf4a11c7e7cfa524992a96d41d885da38ab95ebd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 8 Jul 2014 13:47:53 +0200 Subject: [PATCH 008/138] fix merge errors in offboard.cpp --- src/modules/navigator/offboard.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp index 71a14c029d..fc4d183cdf 100644 --- a/src/modules/navigator/offboard.cpp +++ b/src/modules/navigator/offboard.cpp @@ -67,17 +67,22 @@ Offboard::~Offboard() { } -bool -Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) +void +Offboard::on_activation() { +} + +void +Offboard::on_active() +{ + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + bool updated; orb_check(_navigator->get_offboard_control_sp_sub(), &updated); if (updated) { update_offboard_control_setpoint(); } - bool changed = false; - /* copy offboard setpoints to the corresponding topics */ if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not @@ -97,8 +102,7 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.position_valid = true; - changed = true; - + _navigator->set_position_setpoint_triplet_updated(); } /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not * support deactivation of individual directions) */ @@ -117,13 +121,12 @@ Offboard::on_active(struct position_setpoint_triplet_s *pos_sp_triplet) pos_sp_triplet->current.valid = true; pos_sp_triplet->current.velocity_valid = true; - changed = true; + _navigator->set_position_setpoint_triplet_updated(); } //XXX: map acceleration setpoint once supported in setpoint triplet } - return changed; } void From 373b1705c19627b97f1c65c1e947802e6b88af83 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 10:57:53 +0200 Subject: [PATCH 009/138] vehicle command: add latest guided commands --- src/modules/uORB/topics/vehicle_command.h | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h index c21a29b130..faf8cc7db1 100644 --- a/src/modules/uORB/topics/vehicle_command.h +++ b/src/modules/uORB/topics/vehicle_command.h @@ -61,6 +61,9 @@ enum VEHICLE_CMD { VEHICLE_CMD_NAV_TAKEOFF = 22, /* Takeoff from ground / hand |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer| Latitude| Longitude| Altitude| */ VEHICLE_CMD_NAV_ROI = 80, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicles control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of intereset mode. (see MAV_ROI enum)| MISSION index/ target ID. (see MAV_ROI enum)| ROI index (allows a vehicle to manage multiple ROI's)| Empty| x the location of the fixed ROI (see MAV_FRAME)| y| z| */ VEHICLE_CMD_NAV_PATHPLANNING = 81, /* Control autonomous path planning on the MAV. |0: Disable local obstacle avoidance / local path planning (without resetting map), 1: Enable local path planning, 2: Enable and reset local path planning| 0: Disable full path planning (without resetting map), 1: Enable, 2: Enable and reset map/occupancy grid, 3: Enable and reset planned route, but not occupancy grid| Empty| Yaw angle at goal, in compass degrees, [0..360]| Latitude/X of goal| Longitude/Y of goal| Altitude/Z of goal| */ + VEHICLE_CMD_NAV_GUIDED_LIMITS=90, /* set limits for external control |timeout - maximum time (in seconds) that external controller will be allowed to control vehicle. 0 means no timeout| absolute altitude min (in meters, WGS84) - if vehicle moves below this alt, the command will be aborted and the mission will continue. 0 means no lower altitude limit| absolute altitude max (in meters)- if vehicle moves above this alt, the command will be aborted and the mission will continue. 0 means no upper altitude limit| horizontal move limit (in meters, WGS84) - if vehicle moves more than this distance from it's location at the moment the command was executed, the command will be aborted and the mission will continue. 0 means no horizontal altitude limit| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_MASTER=91, /* set id of master controller |System ID| Component ID| Empty| Empty| Empty| Empty| Empty| */ + VEHICLE_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_NAV_LAST = 95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_DELAY = 112, /* Delay mission state machine. |Delay in seconds (decimal)| Empty| Empty| Empty| Empty| Empty| Empty| */ VEHICLE_CMD_CONDITION_CHANGE_ALT = 113, /* Ascend/descend at rate. Delay mission state machine until desired altitude reached. |Descent / Ascend rate (m/s)| Empty| Empty| Empty| Empty| Empty| Finish Altitude| */ From 747eda92b9396e38746d505e5d79a7528e117c89 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 11:19:06 +0200 Subject: [PATCH 010/138] commander: handle VEHICLE_CMD_NAV_GUIDED_ENABLE --- src/modules/commander/commander.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c112867897..e9db923e6e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -611,7 +611,24 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s } } break; - + case VEHICLE_CMD_NAV_GUIDED_ENABLE: { + transition_result_t res = TRANSITION_DENIED; + static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL; + if (status_local->main_state != MAIN_STATE_OFFBOARD) { + main_state_pre_offboard = status_local->main_state; + } + if (cmd->param1 > 0.5f) { + res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status_local, "OFFBOARD"); + } + } else { + /* If the mavlink command is used to enable or disable offboard control: + * switch back to previous mode when disabling */ + res = main_state_transition(status_local, main_state_pre_offboard); + } + } + break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: case VEHICLE_CMD_PREFLIGHT_CALIBRATION: case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS: From 7a064174110d827182820960b245031e0b4d42ab Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 14 Jul 2014 13:34:06 +0200 Subject: [PATCH 011/138] mavlink: external setpoint feed trough functionality --- src/modules/mavlink/mavlink.c | 7 +++++++ src/modules/mavlink/mavlink_main.cpp | 16 ++++++++++++---- src/modules/mavlink/mavlink_main.h | 8 ++++++-- src/modules/mavlink/mavlink_receiver.cpp | 15 +++++++++++++++ 4 files changed, 40 insertions(+), 6 deletions(-) diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index e49288a746..bec706badd 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING); * @group MAVLink */ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0); +/** + * Forward external setpoint messages + * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard + * control mode + * @group MAVLink + */ +PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1); mavlink_system_t mavlink_system = { 100, diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index cd0581af43..7707c0bc88 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -220,6 +220,7 @@ Mavlink::Mavlink() : _task_running(false), _hil_enabled(false), _use_hil_gps(false), + _forward_externalsp(false), _is_usb_uart(false), _wait_to_transmit(false), _received_messages(false), @@ -246,6 +247,7 @@ Mavlink::Mavlink() : _param_component_id(0), _param_system_type(0), _param_use_hil_gps(0), + _param_forward_externalsp(0), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")), @@ -517,6 +519,7 @@ void Mavlink::mavlink_update_system(void) _param_component_id = param_find("MAV_COMP_ID"); _param_system_type = param_find("MAV_TYPE"); _param_use_hil_gps = param_find("MAV_USEHILGPS"); + _param_forward_externalsp = param_find("MAV_FWDEXTSP"); _param_initialized = true; } @@ -546,6 +549,11 @@ void Mavlink::mavlink_update_system(void) param_get(_param_use_hil_gps, &use_hil_gps); _use_hil_gps = (bool)use_hil_gps; + + int32_t forward_externalsp; + param_get(_param_forward_externalsp, &forward_externalsp); + + _forward_externalsp = (bool)forward_externalsp; } int Mavlink::get_system_id() @@ -1501,14 +1509,14 @@ Mavlink::task_main(int argc, char *argv[]) if (read_count > sizeof(mavlink_message_t)) { read_count = sizeof(mavlink_message_t); } - + memcpy(write_ptr, read_ptr, read_count); - + // We hold the mutex until after we complete the second part of the buffer. If we don't // we may end up breaking the empty slot overflow detection semantics when we mark the // possibly partial read below. pthread_mutex_lock(&_message_buffer_mutex); - + message_buffer_mark_read(read_count); /* write second part of buffer if there is some */ @@ -1519,7 +1527,7 @@ Mavlink::task_main(int argc, char *argv[]) memcpy(write_ptr, read_ptr, read_count); message_buffer_mark_read(available); } - + pthread_mutex_unlock(&_message_buffer_mutex); _mavlink_resend_uart(_channel, &msg); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index acfc8b90eb..8738a6f181 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -134,6 +134,8 @@ public: bool get_use_hil_gps() { return _use_hil_gps; } + bool get_forward_externalsp() { return _forward_externalsp; } + bool get_flow_control_enabled() { return _flow_control_enabled; } bool get_forwarding_on() { return _forwarding_on; } @@ -205,7 +207,7 @@ public: * Send a status text with loglevel * * @param string the message to send (will be capped by mavlink max string length) - * @param severity the log level, one of + * @param severity the log level, one of */ int send_statustext(unsigned severity, const char *string); MavlinkStream * get_streams() const { return _streams; } @@ -220,7 +222,7 @@ public: bool should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); } bool message_buffer_write(const void *ptr, int size); - + void lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); } void unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); } @@ -241,6 +243,7 @@ private: /* states */ bool _hil_enabled; /**< Hardware In the Loop mode */ bool _use_hil_gps; /**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */ + bool _forward_externalsp; /**< Forward external setpoint messages to controllers directly if in offboard mode */ bool _is_usb_uart; /**< Port is USB */ bool _wait_to_transmit; /**< Wait to transmit until received messages. */ bool _received_messages; /**< Whether we've received valid mavlink messages. */ @@ -301,6 +304,7 @@ private: param_t _param_component_id; param_t _param_system_type; param_t _param_use_hil_gps; + param_t _param_forward_externalsp; perf_counter_t _loop_perf; /**< loop performance counter */ perf_counter_t _txerr_perf; /**< TX error counter */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bd1296f626..9df843e586 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -451,6 +451,21 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + //XXX: copy to and publish setpoint triplet here + } + + } } } From d0cca02e97a24cf0b9554cf429bbd5716f947e4e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 10:34:56 +0200 Subject: [PATCH 012/138] add parsing of external attitude message --- src/modules/mavlink/mavlink_receiver.cpp | 88 ++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 1 + .../uORB/topics/offboard_control_setpoint.h | 6 ++ 3 files changed, 93 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9df843e586..9e38426145 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -157,6 +157,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) handle_message_local_ned_position_setpoint_external(msg); break; + case MAVLINK_MSG_ID_ATTITUDE_SETPOINT_EXTERNAL: + handle_message_attitude_setpoint_external(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -404,7 +408,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes mavlink_msg_local_ned_position_setpoint_external_decode(msg, &local_ned_position_setpoint_external); struct offboard_control_setpoint_s offboard_control_sp; - memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && mavlink_system.compid == local_ned_position_setpoint_external.target_component) { @@ -438,7 +442,7 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); for (int i = 0; i < 9; i++) { - offboard_control_sp.ignore &= ~(local_ned_position_setpoint_external.type_mask & (1 << i)); + offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); } @@ -469,6 +473,86 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } } +void +MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *msg) +{ + mavlink_attitude_setpoint_external_t attitude_setpoint_external; + mavlink_msg_attitude_setpoint_external_decode(msg, &attitude_setpoint_external); + + struct offboard_control_setpoint_s offboard_control_sp; + memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints + + if (mavlink_system.sysid == attitude_setpoint_external.target_system && + mavlink_system.compid == attitude_setpoint_external.target_component) { + for (int i = 0; i < 4; i++) { + offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; + } + offboard_control_sp.attitude_rate[0] = attitude_setpoint_external.body_roll_rate; + offboard_control_sp.attitude_rate[1] = attitude_setpoint_external.body_pitch_rate; + offboard_control_sp.attitude_rate[2] = attitude_setpoint_external.body_yaw_rate; + + for (int i = 0; i < 3; i++) { + offboard_control_sp.ignore &= ~(1 << (i + 9)); + offboard_control_sp.ignore |= (attitude_setpoint_external.type_mask & (1 << i)) << 9; + } + offboard_control_sp.ignore &= ~(1 << 10); + offboard_control_sp.ignore |= ((attitude_setpoint_external.type_mask & (1 << 7)) << 3); + + + offboard_control_sp.timestamp = hrt_absolute_time(); + + if (_offboard_control_sp_pub < 0) { + _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); + + } else { + orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); + } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + + /* Publish attitude setpoint if ignore bit is not set */ + if (!(attitude_setpoint_external.type_mask & (1 << 7))) { + struct vehicle_attitude_setpoint_s att_sp; + mavlink_quaternion_to_euler(attitude_setpoint_external.q, + &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + att_sp.thrust = attitude_setpoint_external.thrust; + if (_att_sp_pub < 0) { + _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + } else { + orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp); + } + } + + /* Publish attitude rate setpoint if ignore bit are not set */ + ///XXX add support for ignoring individual axes + if (!(attitude_setpoint_external.type_mask & (0b111))) { + struct vehicle_rates_setpoint_s rates_sp; + rates_sp.roll = attitude_setpoint_external.body_roll_rate; + rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; + rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; + rates_sp.thrust = attitude_setpoint_external.thrust; + + if (_att_sp_pub < 0) { + _rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp); + } else { + orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp); + } + } + } + + } + } +} + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 4fd80f4acc..0f12cf32c7 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -114,6 +114,7 @@ private: void handle_message_vicon_position_estimate(mavlink_message_t *msg); void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg); void handle_message_local_ned_position_setpoint_external(mavlink_message_t *msg); + void handle_message_attitude_setpoint_external(mavlink_message_t *msg); void handle_message_radio_status(mavlink_message_t *msg); void handle_message_manual_control(mavlink_message_t *msg); void handle_message_heartbeat(mavlink_message_t *msg); diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809412fc09..809a2ace4d 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -88,6 +88,7 @@ struct offboard_control_setpoint_s { float attitude_rate[3]; /**< body angular rates (x, y, z) */ uint16_t ignore; /**< if field i is set to true, pi should be ignored */ + //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -118,6 +119,11 @@ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_s return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << 10)); +} + + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); From 596850e9f5343193ab4dc75cfa7849e0eb63ceea Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:19:13 +0200 Subject: [PATCH 013/138] mavlink external sp: accept 0 sysid and compid --- src/modules/mavlink/mavlink_receiver.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 9e38426145..64ba93f9c7 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -410,8 +410,11 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == local_ned_position_setpoint_external.target_system && - mavlink_system.compid == local_ned_position_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == local_ned_position_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { /* convert mavlink type (local, NED) to uORB offboard control struct */ switch (local_ned_position_setpoint_external.coordinate_frame) { @@ -482,8 +485,11 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms struct offboard_control_setpoint_s offboard_control_sp; memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints - if (mavlink_system.sysid == attitude_setpoint_external.target_system && - mavlink_system.compid == attitude_setpoint_external.target_component) { + /* Only accept messages which are intended for this system */ + if ((mavlink_system.sysid == attitude_setpoint_external.target_system || + local_ned_position_setpoint_external.target_system == 0) && + (mavlink_system.compid == local_ned_position_setpoint_external.target_component || + local_ned_position_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } From 35a9dad998961c9f8aa5ab5d015cb4b3a642a9ce Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 15 Jul 2014 14:42:53 +0200 Subject: [PATCH 014/138] mavlink receiver: fix copy paste error --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 64ba93f9c7..30eb6d0e58 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -487,9 +487,9 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Only accept messages which are intended for this system */ if ((mavlink_system.sysid == attitude_setpoint_external.target_system || - local_ned_position_setpoint_external.target_system == 0) && - (mavlink_system.compid == local_ned_position_setpoint_external.target_component || - local_ned_position_setpoint_external.target_component == 0)) { + attitude_setpoint_external.target_system == 0) && + (mavlink_system.compid == attitude_setpoint_external.target_component || + attitude_setpoint_external.target_component == 0)) { for (int i = 0; i < 4; i++) { offboard_control_sp.attitude[i] = attitude_setpoint_external.q[i]; } From 3c8927c42386a6528f120d04ec93f5ab9b453a5b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 09:38:57 +0200 Subject: [PATCH 015/138] once offboard is set by mavlink command ignore RC mode --- src/modules/commander/commander.cpp | 10 ++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 12 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e9db923e6e..cc68c4e363 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -621,11 +621,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); if (res == TRANSITION_DENIED) { print_reject_mode(status_local, "OFFBOARD"); + status_local->offboard_control_set_by_command = false; + } else { + /* Set flag that offboard was set via command, main state is not overridden by rc */ + status_local->offboard_control_set_by_command = true; } } else { /* If the mavlink command is used to enable or disable offboard control: * switch back to previous mode when disabling */ res = main_state_transition(status_local, main_state_pre_offboard); + status_local->offboard_control_set_by_command = false; } } break; @@ -1749,6 +1754,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* if offboard is set allready by a mavlink command, abort */ + if (status.offboard_control_set_by_command) { + return main_state_transition(status_local, MAIN_STATE_OFFBOARD); + } + /* offboard switch overrides main switch */ if (sp_man->offboard_switch == SWITCH_POS_ON) { res = main_state_transition(status_local, MAIN_STATE_OFFBOARD); diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b46c00b758..7a40ac636b 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -206,6 +206,8 @@ struct vehicle_status_s { bool offboard_control_signal_lost; bool offboard_control_signal_weak; uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ + bool offboard_control_set_by_command; /**< true if the offboard mode was set by a mavlink command + and should not be overridden by RC */ /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; From a63f3a173174e28fb5df63a1646c62dcbdc52bbb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 10:36:02 +0200 Subject: [PATCH 016/138] external attitude sp: set timestamp --- src/modules/mavlink/mavlink_receiver.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 30eb6d0e58..952bc8735a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -528,6 +528,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms /* Publish attitude setpoint if ignore bit is not set */ if (!(attitude_setpoint_external.type_mask & (1 << 7))) { struct vehicle_attitude_setpoint_s att_sp; + att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; @@ -542,6 +543,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms ///XXX add support for ignoring individual axes if (!(attitude_setpoint_external.type_mask & (0b111))) { struct vehicle_rates_setpoint_s rates_sp; + rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = attitude_setpoint_external.body_roll_rate; rates_sp.pitch = attitude_setpoint_external.body_pitch_rate; rates_sp.yaw = attitude_setpoint_external.body_yaw_rate; From 9527cc8293749b9ccdec015f587afef9698be1e6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 16 Jul 2014 14:03:34 +0200 Subject: [PATCH 017/138] att external sp: also write quaternion --- src/modules/mavlink/mavlink_receiver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 952bc8735a..e5d380cfb0 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -506,6 +506,7 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms offboard_control_sp.timestamp = hrt_absolute_time(); + offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode if (_offboard_control_sp_pub < 0) { _offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); @@ -532,6 +533,8 @@ MavlinkReceiver::handle_message_attitude_setpoint_external(mavlink_message_t *ms mavlink_quaternion_to_euler(attitude_setpoint_external.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); att_sp.thrust = attitude_setpoint_external.thrust; + att_sp.q_d_valid = true; + memcpy(att_sp.q_d, attitude_setpoint_external.q, sizeof(att_sp.q_d)); if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { From f4608707389dbc30eb25db524d6e008c8033d052 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 17 Jul 2014 09:11:57 +0200 Subject: [PATCH 018/138] support force setpoints --- src/modules/commander/commander.cpp | 9 +++++++ src/modules/mavlink/mavlink_receiver.cpp | 24 ++++++++++++++++++- src/modules/mavlink/mavlink_receiver.h | 2 ++ .../uORB/topics/offboard_control_setpoint.h | 7 +++--- .../uORB/topics/vehicle_control_mode.h | 1 + 5 files changed, 39 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index cc68c4e363..fc9560e18f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1951,6 +1951,15 @@ set_control_mode() control_mode.flag_control_position_enabled = false; control_mode.flag_control_velocity_enabled = false; break; + case OFFBOARD_CONTROL_MODE_DIRECT_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_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; + break; case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED: case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5d380cfb0..2cc2d61626 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -105,6 +105,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), + _force_sp_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -444,6 +445,13 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes offboard_control_sp.acceleration[1] = local_ned_position_setpoint_external.afy; offboard_control_sp.acceleration[2] = local_ned_position_setpoint_external.afz; offboard_control_sp.isForceSetpoint = (bool)(local_ned_position_setpoint_external.type_mask & (1 << 9)); + + /* If we are in force control mode, for now set offboard mode to force control */ + if (offboard_control_sp.isForceSetpoint) { + offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE; + } + + /* set ignore flags */ for (int i = 0; i < 9; i++) { offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (local_ned_position_setpoint_external.type_mask & (1 << i)); @@ -467,9 +475,23 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes if (updated) { orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } - if (_control_mode.flag_control_offboard_enabled) { + if (offboard_control_sp.isForceSetpoint) { + struct vehicle_force_setpoint_s force_sp; + force_sp.x = offboard_control_sp.acceleration[0]; + force_sp.y = offboard_control_sp.acceleration[1]; + force_sp.z = offboard_control_sp.acceleration[2]; + //XXX: yaw + if (_force_sp_pub < 0) { + _force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp); + } else { + orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); + } + } else { + //XXX: copy to and publish setpoint triplet here + } + } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 0f12cf32c7..0044b42cba 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -70,6 +70,7 @@ #include #include #include +#include #include "mavlink_ftp.h" @@ -146,6 +147,7 @@ private: orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; + orb_advert_t _force_sp_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 809a2ace4d..19f11ba924 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -58,9 +58,10 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED = 5, OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED = 6, OFFBOARD_CONTROL_MODE_DIRECT_GLOBAL = 7, - OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 8, - OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 9, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 10, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, + OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, + OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 49e2ba4b56..ca77054569 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s { bool flag_control_offboard_enabled; /**< true if offboard control should be used */ bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_force_enabled; /**< true if force control is mixed in */ bool flag_control_velocity_enabled; /**< true if horizontal velocity (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ bool flag_control_altitude_enabled; /**< true if altitude is controlled */ From 7bc0e26734a0319295e488e413db8f618b9b621c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 23 Jul 2014 11:37:27 +0200 Subject: [PATCH 019/138] mavlink external sp: support for local pos and vel --- src/modules/mavlink/mavlink_receiver.cpp | 46 ++++++++++++++++++++++-- src/modules/mavlink/mavlink_receiver.h | 2 +- 2 files changed, 45 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index b39aebf9e2..b7cddf5231 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -37,6 +37,7 @@ * * @author Lorenz Meier * @author Anton Babushkin + * @author Thomas Gubler */ /* XXX trim includes */ @@ -103,11 +104,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _cmd_pub(-1), _flow_pub(-1), _offboard_control_sp_pub(-1), - _local_pos_sp_pub(-1), _global_vel_sp_pub(-1), _att_sp_pub(-1), _rates_sp_pub(-1), _force_sp_pub(-1), + _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), @@ -487,8 +488,49 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { + /* It's not a force setpoint: publish to setpoint triplet topic */ + struct position_setpoint_triplet_s pos_sp_triplet; + pos_sp_triplet.previous.valid = false; + pos_sp_triplet.next.valid = false; + pos_sp_triplet.current.valid = true; + + /* set the local pos values if the setpoint type is 'local pos' and none + * of the local pos fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_position(offboard_control_sp, 0) && + !offboard_control_sp_ignore_position(offboard_control_sp, 1) && + !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.position_valid = true; + pos_sp_triplet.current.x = offboard_control_sp.position[0]; + pos_sp_triplet.current.y = offboard_control_sp.position[1]; + pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } + + /* set the local vel values if the setpoint type is 'local pos' and none + * of the local vel fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && + !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } + + //XXX handle global pos setpoints (different MAV frames) + + + if (_pos_sp_triplet_pub < 0) { + _pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet), + &pos_sp_triplet); + } else { + orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub, + &pos_sp_triplet); + } - //XXX: copy to and publish setpoint triplet here } } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index a6553cb0ae..a2ed4264f5 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -143,11 +143,11 @@ private: orb_advert_t _cmd_pub; orb_advert_t _flow_pub; orb_advert_t _offboard_control_sp_pub; - orb_advert_t _local_pos_sp_pub; orb_advert_t _global_vel_sp_pub; orb_advert_t _att_sp_pub; orb_advert_t _rates_sp_pub; orb_advert_t _force_sp_pub; + orb_advert_t _pos_sp_triplet_pub; orb_advert_t _vicon_position_pub; orb_advert_t _telemetry_status_pub; orb_advert_t _rc_pub; From a04d70ed5a800ff6d74afc6800f7bb94ed472ad4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 14:54:35 +0200 Subject: [PATCH 020/138] offboard setpoints: correctly check for force setpoint --- src/modules/mavlink/mavlink_receiver.cpp | 6 +++- .../uORB/topics/offboard_control_setpoint.h | 30 +++++++++++++++++++ 2 files changed, 35 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index dfc5ddc915..418b81ee17 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -441,7 +441,11 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); } if (_control_mode.flag_control_offboard_enabled) { - if (offboard_control_sp.isForceSetpoint) { + if (offboard_control_sp.isForceSetpoint && + offboard_control_sp_ignore_position_all(offboard_control_sp) && + offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + /* The offboard setpoint is a force setpoint only, directly writing to the force + * setpoint topic and not publishing the setpoint triplet topic */ struct vehicle_force_setpoint_s force_sp; force_sp.x = offboard_control_sp.acceleration[0]; force_sp.y = offboard_control_sp.acceleration[1]; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 19f11ba924..4f45ac14ff 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -104,14 +104,44 @@ struct offboard_control_setpoint_s { * @} */ +/** + * Returns true if the position setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << index)); } +/** + * Returns true if all position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the velocity setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); } +/** + * Returns true if all velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return true; + } + } + return false; +} + inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } From 85eed22150e4a24098554992a6d77bce6f1ddf31 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 12 Aug 2014 16:27:02 +0200 Subject: [PATCH 021/138] offboard sp: correctly map acceleration/force in combined setpoint to setpoint triple Conflicts: src/modules/uORB/topics/position_setpoint_triplet.h --- src/modules/mavlink/mavlink_receiver.cpp | 39 +++++++++++++------ .../uORB/topics/offboard_control_setpoint.h | 21 ++++++++++ .../uORB/topics/position_setpoint_triplet.h | 5 +++ 3 files changed, 53 insertions(+), 12 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 418b81ee17..da651e4e76 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -457,7 +457,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp); } } else { - /* It's not a force setpoint: publish to setpoint triplet topic */ + /* It's not a pure force setpoint: publish to setpoint triplet topic */ struct position_setpoint_triplet_s pos_sp_triplet; pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; @@ -466,29 +466,44 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position(offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(offboard_control_sp, 2)) { + !offboard_control_sp_ignore_position_all(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; pos_sp_triplet.current.z = offboard_control_sp.position[2]; + } else { + pos_sp_triplet.current.position_valid = false; } /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(offboard_control_sp, 2)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others - pos_sp_triplet.current.velocity_valid = true; - pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; - pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; - pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.velocity_valid = true; + pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; + pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; + pos_sp_triplet.current.vz = offboard_control_sp.velocity[2]; + } else { + pos_sp_triplet.current.velocity_valid = false; } + /* set the local acceleration values if the setpoint type is 'local pos' and none + * of the accelerations fields is set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others + pos_sp_triplet.current.acceleration_valid = true; + pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; + pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; + pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2]; + pos_sp_triplet.current.acceleration_is_force = + offboard_control_sp.isForceSetpoint; + + } else { + pos_sp_triplet.current.acceleration_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 4f45ac14ff..47c89f7083 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -142,14 +142,35 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro return false; } +/** + * Returns true if the acceleration setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); } +/** + * Returns true if all acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return true; + } + } + return false; +} + +/** + * Returns true if the bodyrate setpoint at index should be ignored + */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); } +/** + * Returns true if the attitude setpoint should be ignored + */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { return (bool)(offboard_control_sp.ignore & (1 << 10)); } diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index ec2131abd8..3ea3f671e0 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -83,6 +83,11 @@ struct position_setpoint_s float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ + float a_x; //**< acceleration x setpoint */ + float a_y; //**< acceleration y setpoint */ + float a_z; //**< acceleration z setpoint */ + bool acceleration_valid; //*< true if acceleration setpoint is valid/should be used */ + bool acceleration_is_force; //*< interprete acceleration as force */ }; /** From 1e9d77f1d2dfd3f500c8486edf8940bc4a15162e Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 15 Aug 2014 12:58:06 +0200 Subject: [PATCH 022/138] offboard: ignore mask cleanup add handling of thrust ignore bit, add defines for the bitnumbers, improve and fix interface to ignore bitmask --- src/modules/mavlink/mavlink_receiver.cpp | 29 +++--- .../uORB/topics/offboard_control_setpoint.h | 90 +++++++++++++++++-- 2 files changed, 100 insertions(+), 19 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 7dd043bbec..6b3c3accba 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -472,7 +472,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_position_all(offboard_control_sp)) { + !offboard_control_sp_ignore_position_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; @@ -485,7 +485,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local vel values if the setpoint type is 'local pos' and none * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_velocity_all(offboard_control_sp)) { + !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; @@ -498,7 +498,7 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t /* set the local acceleration values if the setpoint type is 'local pos' and none * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && - !offboard_control_sp_ignore_acceleration_all(offboard_control_sp)) { + !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; @@ -589,12 +589,17 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate; offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate; + /* set correct ignore flags for body rate fields: copy from mavlink message */ for (int i = 0; i < 3; i++) { - offboard_control_sp.ignore &= ~(1 << (i + 9)); - offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << 9; + offboard_control_sp.ignore &= ~(1 << (i + OFB_IGN_BIT_BODYRATE_X)); + offboard_control_sp.ignore |= (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X; } - offboard_control_sp.ignore &= ~(1 << 10); - offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << 3); + /* set correct ignore flags for thrust field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST); + /* set correct ignore flags for attitude field: copy from mavlink message */ + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT); + offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT); offboard_control_sp.timestamp = hrt_absolute_time(); @@ -618,8 +623,9 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) if (_control_mode.flag_control_offboard_enabled) { - /* Publish attitude setpoint if ignore bit is not set */ - if (!(set_attitude_target.type_mask & (1 << 7))) { + /* Publish attitude setpoint if attitude and thrust ignore bits are not set */ + if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_attitude_setpoint_s att_sp; att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, @@ -634,9 +640,10 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } - /* Publish attitude rate setpoint if ignore bit are not set */ + /* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */ ///XXX add support for ignoring individual axes - if (!(set_attitude_target.type_mask & (0b111))) { + if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) || + offboard_control_sp_ignore_thrust(offboard_control_sp))) { struct vehicle_rates_setpoint_s rates_sp; rates_sp.timestamp = hrt_absolute_time(); rates_sp.roll = set_attitude_target.body_roll_rate; diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 47c89f7083..6e37896afe 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -61,7 +61,7 @@ enum OFFBOARD_CONTROL_MODE { OFFBOARD_CONTROL_MODE_DIRECT_FORCE = 8, OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 9, OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 10, - OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ + OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 11 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */ }; enum OFFBOARD_CONTROL_FRAME { @@ -72,6 +72,23 @@ enum OFFBOARD_CONTROL_FRAME { OFFBOARD_CONTROL_FRAME_GLOBAL = 4 }; +/* mappings for the ignore bitmask */ +enum {OFB_IGN_BIT_POS_X, + OFB_IGN_BIT_POS_Y, + OFB_IGN_BIT_POS_Z, + OFB_IGN_BIT_VEL_X, + OFB_IGN_BIT_VEL_Y, + OFB_IGN_BIT_VEL_Z, + OFB_IGN_BIT_ACC_X, + OFB_IGN_BIT_ACC_Y, + OFB_IGN_BIT_ACC_Z, + OFB_IGN_BIT_BODYRATE_X, + OFB_IGN_BIT_BODYRATE_Y, + OFB_IGN_BIT_BODYRATE_Z, + OFB_IGN_BIT_ATT, + OFB_IGN_BIT_THRUST +}; + /** * @addtogroup topics * @{ @@ -87,9 +104,11 @@ struct offboard_control_setpoint_s { float acceleration[3]; /**< x acc, y acc, z acc */ float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ + float thrust; /**< thrust */ + + uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file + for mapping */ - uint16_t ignore; /**< if field i is set to true, pi should be ignored */ - //XXX define constants for bit offsets bool isForceSetpoint; /**< the acceleration vector should be interpreted as force */ float override_mode_switch; @@ -108,13 +127,25 @@ struct offboard_control_setpoint_s { * Returns true if the position setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_position(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << index)); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_POS_X + index))); } /** * Returns true if all position setpoints should be ignored */ inline bool offboard_control_sp_ignore_position_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_position(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some position setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_position_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_position(offboard_control_sp, i)) { return true; @@ -127,13 +158,25 @@ inline bool offboard_control_sp_ignore_position_all(const struct offboard_contro * Returns true if the velocity setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_velocity(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (3 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_VEL_X + index))); } /** * Returns true if all velocity setpoints should be ignored */ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some velocity setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_velocity_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_velocity(offboard_control_sp, i)) { return true; @@ -146,13 +189,25 @@ inline bool offboard_control_sp_ignore_velocity_all(const struct offboard_contro * Returns true if the acceleration setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_acceleration(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (6 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_ACC_X + index))); } /** * Returns true if all acceleration setpoints should be ignored */ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (!offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { + return false; + } + } + return true; +} + +/** + * Returns true if some acceleration setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_acceleration_some(const struct offboard_control_setpoint_s &offboard_control_sp) { for (int i = 0; i < 3; i++) { if (offboard_control_sp_ignore_acceleration(offboard_control_sp, i)) { return true; @@ -165,14 +220,33 @@ inline bool offboard_control_sp_ignore_acceleration_all(const struct offboard_co * Returns true if the bodyrate setpoint at index should be ignored */ inline bool offboard_control_sp_ignore_bodyrates(const struct offboard_control_setpoint_s &offboard_control_sp, int index) { - return (bool)(offboard_control_sp.ignore & (1 << (9 + index))); + return (bool)(offboard_control_sp.ignore & (1 << (OFB_IGN_BIT_BODYRATE_X + index))); +} + +/** + * Returns true if some of the bodyrate setpoints should be ignored + */ +inline bool offboard_control_sp_ignore_bodyrates_some(const struct offboard_control_setpoint_s &offboard_control_sp) { + for (int i = 0; i < 3; i++) { + if (offboard_control_sp_ignore_bodyrates(offboard_control_sp, i)) { + return true; + } + } + return false; } /** * Returns true if the attitude setpoint should be ignored */ inline bool offboard_control_sp_ignore_attitude(const struct offboard_control_setpoint_s &offboard_control_sp) { - return (bool)(offboard_control_sp.ignore & (1 << 10)); + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_ATT)); +} + +/** + * Returns true if the thrust setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } From ee254be845f16531990f4574ca7505ed359c4a61 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 11:16:27 +0200 Subject: [PATCH 023/138] navigator: skip takeoff if already above takeoff altitude --- src/modules/navigator/mission.cpp | 75 ++++++++++++++++++------------- src/modules/navigator/mission.h | 4 +- 2 files changed, 45 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index c0e37a3eda..98a4340e1c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -107,6 +107,7 @@ Mission::on_inactive() update_offboard_mission(); } + /* require takeoff after non-loiter or landing */ if (!_navigator->get_can_loiter_at_sp() || _navigator->get_vstatus()->condition_landed) { _need_takeoff = true; } @@ -399,7 +400,7 @@ Mission::set_mission_items() takeoff_alt += _navigator->get_home_position()->alt; } - /* perform takeoff at least to NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ if (_navigator->get_vstatus()->condition_landed) { takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); @@ -407,45 +408,55 @@ Mission::set_mission_items() takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); } - mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); + /* check if we already above takeoff altitude */ + if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { + mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - } else { - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - - /* require takeoff after landing or idle */ - if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { - _need_takeoff = true; - } - - _navigator->set_can_loiter_at_sp(false); - reset_mission_item_reached(); - - if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); - } - // TODO: report onboard mission item somehow - - /* try to read next mission item */ - struct mission_item_s mission_item_next; - - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + _navigator->set_position_setpoint_triplet_updated(); + return; } else { - /* next mission item is not available */ - pos_sp_triplet->next.valid = false; + /* skip takeoff */ + _takeoff = false; } } + /* set current position setpoint from mission item */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* require takeoff after landing or idle */ + if (pos_sp_triplet->current.type == SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == SETPOINT_TYPE_IDLE) { + _need_takeoff = true; + } + + _navigator->set_can_loiter_at_sp(false); + reset_mission_item_reached(); + + if (_mission_type == MISSION_TYPE_OFFBOARD) { + report_current_offboard_mission_item(); + } + // TODO: report onboard mission item somehow + + /* try to read next mission item */ + struct mission_item_s mission_item_next; + + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } + _navigator->set_position_setpoint_triplet_updated(); } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 4da6a11553..1edaa5c8ac 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -142,8 +142,8 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; - bool _need_takeoff; - bool _takeoff; + bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ + bool _takeoff; /**< takeoff state flag */ orb_advert_t _mission_result_pub; struct mission_result_s _mission_result; From fc0bdfb6f5021f5cc2d155ea3cee9f5d5102cdef Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:42:12 +0200 Subject: [PATCH 024/138] mc_pos_control: trajectory following, using previous and next waypoints --- .../mc_pos_control/mc_pos_control_main.cpp | 184 +++++++++++++++--- 1 file changed, 156 insertions(+), 28 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 ecc40428de..488ca924a6 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -76,6 +76,7 @@ #define TILT_COS_MAX 0.7f #define SIGMA 0.000001f +#define MIN_DIST 0.01f /** * Multicopter position control app start / stop handling function @@ -229,6 +230,14 @@ private: */ void control_offboard(float dt); + bool cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res); + + /** + * Set position setpoint for AUTO + */ + void control_auto(); + /** * Select between barometric and global (AMSL) altitudes */ @@ -647,6 +656,152 @@ MulticopterPositionControl::control_offboard(float dt) } } +bool +MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r, + const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res) +{ + /* project center of sphere on line */ + /* normalized AB */ + math::Vector<3> ab_norm = line_b - line_a; + ab_norm.normalize(); + math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm); + float cd_len = (sphere_c - d).length(); + + /* we have triangle CDX with known CD and CX = R, find DX */ + if (sphere_r > cd_len) { + /* have two roots, select one in A->B direction from D */ + float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len); + res = d + ab_norm * dx_len; + return true; + + } else { + /* have no roots, return D */ + res = d; + return false; + } +} + +void +MulticopterPositionControl::control_auto() +{ + bool updated; + orb_check(_pos_sp_triplet_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); + } + + 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); + + /* by default use current setpoint as is */ + _pos_sp = curr_sp; + + if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.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) { + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); + + /* find X - cross point of L1 sphere and trajectory */ + math::Vector<3> pos_s = _pos.emult(scale); + math::Vector<3> prev_sp_s = prev_sp.emult(scale); + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> curr_pos_s = pos_s - curr_sp_s; + float curr_pos_s_len = curr_pos_s.length(); + math::Vector<3> x; + if (curr_pos_s_len < 1.0f) { + /* copter is closer to waypoint than L1 radius */ + /* check next waypoint and use it to avoid slowing down when passing via waypoint */ + if (_pos_sp_triplet.next.valid) { + math::Vector<3> next_sp; + map_projection_project(&_ref_pos, + _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon, + &next_sp.data[0], &next_sp.data[1]); + next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt); + + if ((next_sp - curr_sp).length() > MIN_DIST) { + math::Vector<3> next_sp_s = next_sp.emult(scale); + + /* calculate angle prev - curr - next */ + math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; + math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + + /* cos(a) = angle between current and next trajectory segments */ + float cos_a = prev_curr_s_norm * curr_next_s; + + /* cos(b) = angle pos - curr_sp - prev_sp */ + float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; + + if (cos_a > 0.0f && cos_b > 0.0f) { + float curr_next_s_len = curr_next_s.length(); + /* if curr - next distance is larger than L1 radius, limit it */ + if (curr_next_s_len > 1.0f) { + cos_a /= curr_next_s_len; + } + + /* feed forward position setpoint offset */ + math::Vector<3> pos_ff = prev_curr_s_norm * + cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); + _pos_sp += pos_ff.edivide(scale); + } + } + } + + } else { + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + if (near) { + /* L1 sphere crosses trajectory */ + + } else { + /* copter is too far from trajectory */ + /* if copter is behind prev waypoint, go directly to prev waypoint */ + if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { + x = prev_sp_s; + } + + /* if copter is in front of curr waypoint, go directly to curr waypoint */ + if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { + x = curr_sp_s; + } + + x = pos_s + (x - pos_s).normalized(); + } + + /* scale result back to normal space */ + _pos_sp = x.edivide(scale); + } + } + } + + /* update yaw setpoint if needed */ + if (isfinite(_pos_sp_triplet.current.yaw)) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } + + } else { + /* no waypoint, loiter, reset position setpoint if needed */ + reset_pos_sp(); + reset_alt_sp(); + } +} + void MulticopterPositionControl::task_main() { @@ -757,34 +912,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - bool updated; - orb_check(_pos_sp_triplet_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet); - } - - 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 */ - map_projection_project(&_ref_pos, - _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon, - &_pos_sp.data[0], &_pos_sp.data[1]); - _pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); - - /* update yaw setpoint if needed */ - if (isfinite(_pos_sp_triplet.current.yaw)) { - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; - } - - } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); - } + control_auto(); } /* fill local position setpoint */ From 7b4448510436ddb3d5c548a4039bfa53c3aabfc4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 16 Aug 2014 18:46:42 +0200 Subject: [PATCH 025/138] mission: takeoff and next waypoint related fixes --- src/modules/navigator/mission.cpp | 35 +++++++++++++++++++++---------- 1 file changed, 24 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 98a4340e1c..128dab7d77 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -344,6 +344,10 @@ Mission::set_mission_items() /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering"); + + /* use last setpoint for loiter */ + _navigator->set_can_loiter_at_sp(true); + } else if (!user_feedback_done) { /* only tell users that we got no mission if there has not been any * better, more specific feedback yet @@ -360,6 +364,7 @@ Mission::set_mission_items() mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); pos_sp_triplet->next.valid = false; + /* reuse setpoint for LOITER only if it's not IDLE */ _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); @@ -412,11 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); @@ -445,15 +452,21 @@ Mission::set_mission_items() } // TODO: report onboard mission item somehow - /* try to read next mission item */ - struct mission_item_s mission_item_next; + if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { + /* try to read next mission item */ + struct mission_item_s mission_item_next; - if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { - /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { + /* got next mission item, update setpoint triplet */ + mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + + } else { + /* next mission item is not available */ + pos_sp_triplet->next.valid = false; + } } else { - /* next mission item is not available */ + /* vehicle will be paused on current waypoint, don't set next item */ pos_sp_triplet->next.valid = false; } From f31c3243b05fae6ffa4ab8cccb8e009470ca1294 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 17 Aug 2014 23:28:48 +0200 Subject: [PATCH 026/138] mc_pos_control: navigation fixes, smooth position setpoint movements --- .../mc_pos_control/mc_pos_control_main.cpp | 77 +++++++++++-------- 1 file changed, 44 insertions(+), 33 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 488ca924a6..847a92151c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -236,7 +236,7 @@ private: /** * Set position setpoint for AUTO */ - void control_auto(); + void control_auto(float dt); /** * Select between barometric and global (AMSL) altitudes @@ -682,7 +682,7 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f } void -MulticopterPositionControl::control_auto() +MulticopterPositionControl::control_auto(float dt) { bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -703,8 +703,14 @@ MulticopterPositionControl::control_auto() &curr_sp.data[0], &curr_sp.data[1]); curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt); + /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ + math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); // TODO add mult param here + + /* convert current setpoint to scaled space */ + math::Vector<3> curr_sp_s = curr_sp.emult(scale); + /* by default use current setpoint as is */ - _pos_sp = curr_sp; + math::Vector<3> pos_sp_s = curr_sp_s; if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) { /* follow "previous - current" line */ @@ -715,16 +721,13 @@ MulticopterPositionControl::control_auto() prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt); if ((curr_sp - prev_sp).length() > MIN_DIST) { - /* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */ - math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max); /* find X - cross point of L1 sphere and trajectory */ math::Vector<3> pos_s = _pos.emult(scale); math::Vector<3> prev_sp_s = prev_sp.emult(scale); - math::Vector<3> curr_sp_s = curr_sp.emult(scale); + math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s; math::Vector<3> curr_pos_s = pos_s - curr_sp_s; float curr_pos_s_len = curr_pos_s.length(); - math::Vector<3> x; if (curr_pos_s_len < 1.0f) { /* copter is closer to waypoint than L1 radius */ /* check next waypoint and use it to avoid slowing down when passing via waypoint */ @@ -740,56 +743,64 @@ MulticopterPositionControl::control_auto() /* calculate angle prev - curr - next */ math::Vector<3> curr_next_s = next_sp_s - curr_sp_s; - math::Vector<3> prev_curr_s_norm = (curr_sp_s - prev_sp_s).normalized(); + math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized(); - /* cos(a) = angle between current and next trajectory segments */ - float cos_a = prev_curr_s_norm * curr_next_s; + /* cos(a) * curr_next, a = angle between current and next trajectory segments */ + float cos_a_curr_next = prev_curr_s_norm * curr_next_s; - /* cos(b) = angle pos - curr_sp - prev_sp */ + /* cos(b), b = angle pos - curr_sp - prev_sp */ float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len; - if (cos_a > 0.0f && cos_b > 0.0f) { + if (cos_a_curr_next > 0.0f && cos_b > 0.0f) { float curr_next_s_len = curr_next_s.length(); /* if curr - next distance is larger than L1 radius, limit it */ if (curr_next_s_len > 1.0f) { - cos_a /= curr_next_s_len; + cos_a_curr_next /= curr_next_s_len; } /* feed forward position setpoint offset */ math::Vector<3> pos_ff = prev_curr_s_norm * - cos_a * cos_b * cos_b * (1.0f - curr_pos_s_len) * + cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) * (1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f)); - _pos_sp += pos_ff.edivide(scale); + pos_sp_s += pos_ff; } } } } else { - bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, x); + bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s); if (near) { /* L1 sphere crosses trajectory */ } else { /* copter is too far from trajectory */ /* if copter is behind prev waypoint, go directly to prev waypoint */ - if ((x - prev_sp_s) * (curr_sp_s - prev_sp) < 0.0f) { - x = prev_sp_s; + if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) { + pos_sp_s = prev_sp_s; } /* if copter is in front of curr waypoint, go directly to curr waypoint */ - if ((x - curr_sp_s) * (curr_sp_s - prev_sp) > 0.0f) { - x = curr_sp_s; + if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) { + pos_sp_s = curr_sp_s; } - x = pos_s + (x - pos_s).normalized(); + pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized(); } - - /* scale result back to normal space */ - _pos_sp = x.edivide(scale); } } } + /* move setpoint not faster than max allowed speed */ + math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); + math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; + float d_pos_s_len = d_pos_s.length(); + if (d_pos_s_len > dt) { + pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + } + + /* scale result back to normal space */ + _pos_sp = pos_sp_s.edivide(scale); + /* update yaw setpoint if needed */ if (isfinite(_pos_sp_triplet.current.yaw)) { _att_sp.yaw_body = _pos_sp_triplet.current.yaw; @@ -912,7 +923,7 @@ MulticopterPositionControl::task_main() } else { /* AUTO */ - control_auto(); + control_auto(dt); } /* fill local position setpoint */ @@ -975,14 +986,14 @@ MulticopterPositionControl::task_main() } - if (!_control_mode.flag_control_manual_enabled) { - /* limit 3D speed only in non-manual modes */ - float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); - - if (vel_sp_norm > 1.0f) { - _vel_sp /= vel_sp_norm; - } - } +// if (!_control_mode.flag_control_manual_enabled) { +// /* limit 3D speed only in non-manual modes */ +// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); +// +// if (vel_sp_norm > 1.0f) { +// _vel_sp /= vel_sp_norm; +// } +// } _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); From 57a250f9461f0ab8fe10cf6988f9e2dfd269bdf2 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 19 Aug 2014 09:23:41 +0200 Subject: [PATCH 027/138] navigator: remove offboard mode offboard setpoints are now forwarded directly from mavlink --- src/modules/navigator/module.mk | 1 - src/modules/navigator/navigator.h | 6 +- src/modules/navigator/navigator_main.cpp | 13 +-- src/modules/navigator/offboard.cpp | 142 ----------------------- src/modules/navigator/offboard.h | 72 ------------ 5 files changed, 4 insertions(+), 230 deletions(-) delete mode 100644 src/modules/navigator/offboard.cpp delete mode 100644 src/modules/navigator/offboard.h diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index b501989961..5d680948de 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -46,7 +46,6 @@ SRCS = navigator_main.cpp \ loiter.cpp \ rtl.cpp \ rtl_params.c \ - offboard.cpp \ mission_feasibility_checker.cpp \ geofence.cpp \ geofence_params.c diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 8edbb63b35..742cd59ca5 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -56,14 +56,13 @@ #include "mission.h" #include "loiter.h" #include "rtl.h" -#include "offboard.h" #include "geofence.h" /** * Number of navigation modes that need on_active/on_inactive calls * Currently: mission, loiter, and rtl */ -#define NAVIGATOR_MODE_ARRAY_SIZE 4 +#define NAVIGATOR_MODE_ARRAY_SIZE 3 class Navigator : public control::SuperBlock { @@ -116,7 +115,6 @@ public: struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; } int get_onboard_mission_sub() { return _onboard_mission_sub; } int get_offboard_mission_sub() { return _offboard_mission_sub; } - int get_offboard_control_sp_sub() { return _offboard_control_sp_sub; } Geofence& get_geofence() { return _geofence; } bool get_can_loiter_at_sp() { return _can_loiter_at_sp; } float get_loiter_radius() { return _param_loiter_radius.get(); } @@ -134,7 +132,6 @@ private: int _home_pos_sub; /**< home position subscription */ int _vstatus_sub; /**< vehicle status subscription */ int _capabilities_sub; /**< notification of vehicle capabilities updates */ - int _offboard_control_sp_sub; /*** offboard control subscription */ int _control_mode_sub; /**< vehicle control mode subscription */ int _onboard_mission_sub; /**< onboard mission subscription */ int _offboard_mission_sub; /**< offboard mission subscription */ @@ -164,7 +161,6 @@ private: Mission _mission; /**< class that handles the missions */ Loiter _loiter; /**< class that handles loiter */ RTL _rtl; /**< class that handles RTL */ - Offboard _offboard; /**< class that handles offboard */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE]; /**< array of navigation modes */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 331a9a728f..f11ae72c5d 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -67,7 +67,6 @@ #include #include #include -#include #include #include @@ -101,7 +100,6 @@ Navigator::Navigator() : _home_pos_sub(-1), _vstatus_sub(-1), _capabilities_sub(-1), - _offboard_control_sp_sub(-1), _control_mode_sub(-1), _onboard_mission_sub(-1), _offboard_mission_sub(-1), @@ -124,7 +122,6 @@ Navigator::Navigator() : _mission(this, "MIS"), _loiter(this, "LOI"), _rtl(this, "RTL"), - _offboard(this, "OFF"), _can_loiter_at_sp(false), _pos_sp_triplet_updated(false), _param_loiter_radius(this, "LOITER_RAD"), @@ -134,7 +131,6 @@ Navigator::Navigator() : _navigation_mode_array[0] = &_mission; _navigation_mode_array[1] = &_loiter; _navigation_mode_array[2] = &_rtl; - _navigation_mode_array[3] = &_offboard; updateParams(); } @@ -247,7 +243,6 @@ Navigator::task_main() _onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission)); _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission)); _param_update_sub = orb_subscribe(ORB_ID(parameter_update)); - _offboard_control_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); /* copy all topics first time */ vehicle_status_update(); @@ -353,6 +348,9 @@ Navigator::task_main() case NAVIGATION_STATE_ACRO: case NAVIGATION_STATE_ALTCTL: case NAVIGATION_STATE_POSCTL: + case NAVIGATION_STATE_LAND: + case NAVIGATION_STATE_TERMINATION: + case NAVIGATION_STATE_OFFBOARD: _navigation_mode = nullptr; _can_loiter_at_sp = false; break; @@ -368,11 +366,6 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTGS: _navigation_mode = &_rtl; /* TODO: change this to something else */ break; - case NAVIGATION_STATE_LAND: - case NAVIGATION_STATE_TERMINATION: - case NAVIGATION_STATE_OFFBOARD: - _navigation_mode = &_offboard; - break; default: _navigation_mode = nullptr; _can_loiter_at_sp = false; diff --git a/src/modules/navigator/offboard.cpp b/src/modules/navigator/offboard.cpp deleted file mode 100644 index fc4d183cdf..0000000000 --- a/src/modules/navigator/offboard.cpp +++ /dev/null @@ -1,142 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 offboard.cpp - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include - -#include "navigator.h" -#include "offboard.h" - -Offboard::Offboard(Navigator *navigator, const char *name) : - NavigatorMode(navigator, name), - _offboard_control_sp({0}) -{ - /* load initial params */ - updateParams(); - /* initial reset */ - on_inactive(); -} - -Offboard::~Offboard() -{ -} - -void -Offboard::on_activation() -{ -} - -void -Offboard::on_active() -{ - struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); - - bool updated; - orb_check(_navigator->get_offboard_control_sp_sub(), &updated); - if (updated) { - update_offboard_control_setpoint(); - } - - /* copy offboard setpoints to the corresponding topics */ - if (_offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED) { - /* We accept position control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_position_enabled && - (!offboard_control_sp_ignore_position(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_position(_offboard_control_sp, 2))) { - /* position control */ - pos_sp_triplet->current.x = _offboard_control_sp.position[0]; - pos_sp_triplet->current.y = _offboard_control_sp.position[1]; - //pos_sp_triplet->current.yaw = _offboard_control_sp.position[2]; - //XXX: copy yaw - pos_sp_triplet->current.z = -_offboard_control_sp.position[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.position_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - /* We accept velocity control only if none of the directions is ignored (as pos_sp_triplet does not - * support deactivation of individual directions) */ - if (_navigator->get_control_mode()->flag_control_velocity_enabled && - (!offboard_control_sp_ignore_velocity(_offboard_control_sp, 0) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 1) && - !offboard_control_sp_ignore_velocity(_offboard_control_sp, 2))) { - /* velocity control */ - pos_sp_triplet->current.vx = _offboard_control_sp.velocity[0]; - pos_sp_triplet->current.vy = _offboard_control_sp.velocity[1]; -// pos_sp_triplet->current.yawspeed = _offboard_control_sp.velocity[; -// //XXX: copy yaw speed - pos_sp_triplet->current.vz = _offboard_control_sp.velocity[2]; - - pos_sp_triplet->current.type = SETPOINT_TYPE_OFFBOARD; - pos_sp_triplet->current.valid = true; - pos_sp_triplet->current.velocity_valid = true; - - _navigator->set_position_setpoint_triplet_updated(); - } - - //XXX: map acceleration setpoint once supported in setpoint triplet - } - -} - -void -Offboard::on_inactive() -{ -} - -void -Offboard::update_offboard_control_setpoint() -{ - orb_copy(ORB_ID(offboard_control_setpoint), _navigator->get_offboard_control_sp_sub(), &_offboard_control_sp); - -} diff --git a/src/modules/navigator/offboard.h b/src/modules/navigator/offboard.h deleted file mode 100644 index 66b923bdb5..0000000000 --- a/src/modules/navigator/offboard.h +++ /dev/null @@ -1,72 +0,0 @@ -/*************************************************************************** - * - * Copyright (c) 2014 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 offboard.h - * - * Helper class for offboard commands - * - * @author Julian Oes - */ - -#ifndef NAVIGATOR_OFFBOARD_H -#define NAVIGATOR_OFFBOARD_H - -#include -#include - -#include -#include - -#include "navigator_mode.h" - -class Navigator; - -class Offboard : public NavigatorMode -{ -public: - Offboard(Navigator *navigator, const char *name); - - ~Offboard(); - - virtual void on_inactive(); - - virtual void on_activation(); - - virtual void on_active(); -private: - void update_offboard_control_setpoint(); - - struct offboard_control_setpoint_s _offboard_control_sp; -}; - -#endif From 1e7dee439111f58fa9c0f737b93132a57dbf644d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 21 Aug 2014 16:24:00 +0200 Subject: [PATCH 028/138] set_attitude_target msg: convert quaternion to R --- src/modules/mavlink/mavlink_receiver.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index da594412e7..60fc2937a9 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -689,9 +689,11 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) att_sp.timestamp = hrt_absolute_time(); mavlink_quaternion_to_euler(set_attitude_target.q, &att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body); + mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body); + att_sp.R_valid = true; att_sp.thrust = set_attitude_target.thrust; - att_sp.q_d_valid = true; memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d)); + att_sp.q_d_valid = true; if (_att_sp_pub < 0) { _att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); } else { From 6d80ebfc8933091c1f2cf70a42107bf80d3db74f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 22 Aug 2014 16:09:20 +0200 Subject: [PATCH 029/138] update mavlink version --- mavlink/include/mavlink/v1.0 | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0 index 4d7487c2bc..c8a6e736e2 160000 --- a/mavlink/include/mavlink/v1.0 +++ b/mavlink/include/mavlink/v1.0 @@ -1 +1 @@ -Subproject commit 4d7487c2bc5f5ccf87bca82970fb2c08d6d8bd50 +Subproject commit c8a6e736e2bf71e14b023cf26733c3499d6d515b From 55fde23233de3d311d6c8d0b2cd368e45af3caac Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 09:19:36 +0200 Subject: [PATCH 030/138] support new yaw and yawrate fields in mavlnk position_target message --- src/modules/mavlink/mavlink_receiver.cpp | 35 ++++++++++++++++--- .../uORB/topics/offboard_control_setpoint.h | 22 +++++++++++- .../uORB/topics/position_setpoint_triplet.h | 2 ++ 3 files changed, 54 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 60fc2937a9..3ac4d3b03d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -474,6 +474,8 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx; offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy; offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz; + offboard_control_sp.yaw = set_position_target_local_ned.yaw; + offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate; offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9)); /* If we are in force control mode, for now set offboard mode to force control */ @@ -486,7 +488,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t offboard_control_sp.ignore &= ~(1 << i); offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << i)); } - + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAW); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 10)) << + OFB_IGN_BIT_YAW; + offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_YAWRATE); + offboard_control_sp.ignore |= (set_position_target_local_ned.type_mask & (1 << 11)) << + OFB_IGN_BIT_YAWRATE; offboard_control_sp.timestamp = hrt_absolute_time(); @@ -527,12 +534,12 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t pos_sp_triplet.previous.valid = false; pos_sp_triplet.next.valid = false; pos_sp_triplet.current.valid = true; + pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others /* set the local pos values if the setpoint type is 'local pos' and none * of the local pos fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_position_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.position_valid = true; pos_sp_triplet.current.x = offboard_control_sp.position[0]; pos_sp_triplet.current.y = offboard_control_sp.position[1]; @@ -545,7 +552,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the local vel fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_velocity_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.velocity_valid = true; pos_sp_triplet.current.vx = offboard_control_sp.velocity[0]; pos_sp_triplet.current.vy = offboard_control_sp.velocity[1]; @@ -558,7 +564,6 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t * of the accelerations fields is set to 'ignore' */ if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && !offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) { - pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others pos_sp_triplet.current.acceleration_valid = true; pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0]; pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1]; @@ -569,6 +574,28 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } else { pos_sp_triplet.current.acceleration_valid = false; } + + /* set the yaw sp value if the setpoint type is 'local pos' and the yaw + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yaw(offboard_control_sp)) { + pos_sp_triplet.current.yaw_valid = true; + pos_sp_triplet.current.yaw = offboard_control_sp.yaw; + + } else { + pos_sp_triplet.current.yaw_valid = false; + } + + /* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate + * field is not set to 'ignore' */ + if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED && + !offboard_control_sp_ignore_yawrate(offboard_control_sp)) { + pos_sp_triplet.current.yawspeed_valid = true; + pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate; + + } else { + pos_sp_triplet.current.yawspeed_valid = false; + } //XXX handle global pos setpoints (different MAV frames) diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h index 6e37896afe..72a28e5017 100644 --- a/src/modules/uORB/topics/offboard_control_setpoint.h +++ b/src/modules/uORB/topics/offboard_control_setpoint.h @@ -86,7 +86,9 @@ enum {OFB_IGN_BIT_POS_X, OFB_IGN_BIT_BODYRATE_Y, OFB_IGN_BIT_BODYRATE_Z, OFB_IGN_BIT_ATT, - OFB_IGN_BIT_THRUST + OFB_IGN_BIT_THRUST, + OFB_IGN_BIT_YAW, + OFB_IGN_BIT_YAWRATE, }; /** @@ -105,6 +107,10 @@ struct offboard_control_setpoint_s { float attitude[4]; /**< attitude of vehicle (quaternion) */ float attitude_rate[3]; /**< body angular rates (x, y, z) */ float thrust; /**< thrust */ + float yaw; /**< yaw: this is the yaw from the position_target message + (not from the full attitude_target message) */ + float yaw_rate; /**< yaw rate: this is the yaw from the position_target message + (not from the full attitude_target message) */ uint16_t ignore; /**< if field i is set to true, the value should be ignored, see definition at top of file for mapping */ @@ -249,6 +255,20 @@ inline bool offboard_control_sp_ignore_thrust(const struct offboard_control_setp return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_THRUST)); } +/** + * Returns true if the yaw setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yaw(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAW)); +} + +/** + * Returns true if the yaw rate setpoint should be ignored + */ +inline bool offboard_control_sp_ignore_yawrate(const struct offboard_control_setpoint_s &offboard_control_sp) { + return (bool)(offboard_control_sp.ignore & (1 << OFB_IGN_BIT_YAWRATE)); +} + /* register this as object request broker structure */ ORB_DECLARE(offboard_control_setpoint); diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h index 3ea3f671e0..cb2262534d 100644 --- a/src/modules/uORB/topics/position_setpoint_triplet.h +++ b/src/modules/uORB/topics/position_setpoint_triplet.h @@ -79,7 +79,9 @@ struct position_setpoint_s double lon; /**< longitude, in deg */ float alt; /**< altitude AMSL, in m */ float yaw; /**< yaw (only for multirotors), in rad [-PI..PI), NaN = hold current yaw */ + bool yaw_valid; /**< true if yaw setpoint valid */ float yawspeed; /**< yawspeed (only for multirotors, in rad/s) */ + bool yawspeed_valid; /**< true if yawspeed setpoint valid */ float loiter_radius; /**< loiter radius (only for fixed wing), in m */ int8_t loiter_direction; /**< loiter direction: 1 = CW, -1 = CCW */ float pitch_min; /**< minimal pitch angle for fixed wing takeoff waypoints */ From 9bda573151ae1b5fa87686ee58596ea14e052941 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 25 Aug 2014 13:15:30 +0200 Subject: [PATCH 031/138] mc pos control: offboard: set yaw and yawspeed depending on valid flags --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 ecc40428de..4f5c62a592 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -614,7 +614,6 @@ MulticopterPositionControl::control_offboard(float dt) _pos_sp(0) = _pos_sp_triplet.current.x; _pos_sp(1) = _pos_sp_triplet.current.y; _pos_sp(2) = _pos_sp_triplet.current.z; - _att_sp.yaw_body = _pos_sp_triplet.current.yaw; } else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) { /* control velocity */ @@ -624,6 +623,11 @@ MulticopterPositionControl::control_offboard(float dt) /* set position setpoint move rate */ _sp_move_rate(0) = _pos_sp_triplet.current.vx; _sp_move_rate(1) = _pos_sp_triplet.current.vy; + } + + if (_pos_sp_triplet.current.yaw_valid) { + _att_sp.yaw_body = _pos_sp_triplet.current.yaw; + } else if (_pos_sp_triplet.current.yawspeed_valid) { _att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt; } From ccdfab245bac02565521f2d76e604f54ba3f5bd5 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:26:47 +0200 Subject: [PATCH 032/138] mixer_multirotor: topic for motor limit notification --- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/multirotor_motor_limits.h | 69 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 src/modules/uORB/topics/multirotor_motor_limits.h diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index f7d5f87374..b921865ebe 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -192,6 +192,9 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +#include "topics/multirotor_motor_limits.h" +ORB_DEFINE(multirotor_motor_limits, struct multirotor_motor_limits_s); + #include "topics/telemetry_status.h" ORB_DEFINE(telemetry_status_0, struct telemetry_status_s); ORB_DEFINE(telemetry_status_1, struct telemetry_status_s); diff --git a/src/modules/uORB/topics/multirotor_motor_limits.h b/src/modules/uORB/topics/multirotor_motor_limits.h new file mode 100644 index 0000000000..44c51e4d98 --- /dev/null +++ b/src/modules/uORB/topics/multirotor_motor_limits.h @@ -0,0 +1,69 @@ +/**************************************************************************** + * + * Copyright (C) 2012-2013 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 multirotor_motor_limits.h + * + * Definition of multirotor_motor_limits topic + */ + +#ifndef MULTIROTOR_MOTOR_LIMITS_H_ +#define MULTIROTOR_MOTOR_LIMITS_H_ + +#include "../uORB.h" +#include + +/** + * @addtogroup topics + * @{ + */ + +/** + * Motor limits + */ +struct multirotor_motor_limits_s { + uint8_t roll_pitch : 1; // roll/pitch limit reached + uint8_t yaw : 1; // yaw limit reached + uint8_t throttle_lower : 1; // lower throttle limit reached + uint8_t throttle_upper : 1; // upper throttle limit reached + uint8_t reserved : 4; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(multirotor_motor_limits); + +#endif From 5aafa1b021a80868a3b5aebde13cd54d407e6e41 Mon Sep 17 00:00:00 2001 From: Holger Steinhaus Date: Tue, 26 Aug 2014 20:36:02 +0200 Subject: [PATCH 033/138] mixer_multirotor: motor limit notification for PX4FMU --- src/modules/systemlib/mixer/mixer.h | 6 +++++ .../systemlib/mixer/mixer_multirotor.cpp | 22 ++++++++++++++++++- 2 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h index cdf60febc7..17989558e6 100644 --- a/src/modules/systemlib/mixer/mixer.h +++ b/src/modules/systemlib/mixer/mixer.h @@ -130,6 +130,9 @@ #include #include "drivers/drv_mixer.h" + +#include + #include "mixer_load.h" /** @@ -531,6 +534,9 @@ private: float _yaw_scale; float _idle_speed; + orb_advert_t _limits_pub; + multirotor_motor_limits_s _limits; + unsigned _rotor_count; const Rotor *_rotors; diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp index 4b22a46d0b..57e17b67d2 100644 --- a/src/modules/systemlib/mixer/mixer_multirotor.cpp +++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp @@ -36,7 +36,8 @@ * * Multi-rotor mixers. */ - +#include +#include #include #include @@ -302,6 +303,11 @@ MultirotorMixer::mix(float *outputs, unsigned space) float min_out = 0.0f; float max_out = 0.0f; + _limits.roll_pitch = false; + _limits.yaw = false; + _limits.throttle_upper = false; + _limits.throttle_lower = false; + /* perform initial mix pass yielding unbounded outputs, ignore yaw */ for (unsigned i = 0; i < _rotor_count; i++) { float out = roll * _rotors[i].roll_scale + @@ -311,6 +317,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* limit yaw if it causes outputs clipping */ if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) { yaw = -out / _rotors[i].yaw_scale; + _limits.yaw = true; } /* calculate min and max output values */ @@ -332,6 +339,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) for (unsigned i = 0; i < _rotor_count; i++) { outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust; } + _limits.roll_pitch = true; } else { /* roll/pitch mixed without limiting, add yaw control */ @@ -344,6 +352,7 @@ MultirotorMixer::mix(float *outputs, unsigned space) float scale_out; if (max_out > 1.0f) { scale_out = 1.0f / max_out; + _limits.throttle_upper = true; } else { scale_out = 1.0f; @@ -351,9 +360,20 @@ MultirotorMixer::mix(float *outputs, unsigned space) /* scale outputs to range _idle_speed..1, and do final limiting */ for (unsigned i = 0; i < _rotor_count; i++) { + if (outputs[i] < _idle_speed) { + _limits.throttle_lower = true; + } outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f); } +#if defined(CONFIG_ARCH_BOARD_PX4FMU_V1) || defined(CONFIG_ARCH_BOARD_PX4FMU_V2) + /* publish/advertise motor limits if running on FMU */ + if (_limits_pub > 0) { + orb_publish(ORB_ID(multirotor_motor_limits), _limits_pub, &_limits); + } else { + _limits_pub = orb_advertise(ORB_ID(multirotor_motor_limits), &_limits); + } +#endif return _rotor_count; } From 63e741824cfde42ac563c457e615cf58f10a2529 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 1 Sep 2014 11:27:28 +0200 Subject: [PATCH 034/138] fw landing: use terrain estimate from global pos --- .../fw_pos_control_l1_main.cpp | 82 ++++++------------- 1 file changed, 24 insertions(+), 58 deletions(-) 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 deccab4820..064b8a6fae 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 @@ -88,7 +88,6 @@ #include #include #include -#include #include "landingslope.h" #include "mtecs/mTecs.h" @@ -143,7 +142,6 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ - int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _tecs_status_pub; /**< TECS status publication */ @@ -158,7 +156,6 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ - struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -239,7 +236,6 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; - float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -284,7 +280,6 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; - param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -310,12 +305,6 @@ private: */ bool vehicle_airspeed_poll(); - /** - * Check for range finder updates. - */ - bool range_finder_poll(); - - /** * Check for position updates. */ @@ -337,9 +326,9 @@ private: void navigation_capabilities_publish(); /** - * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available */ - float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos); /** * Control position. @@ -411,7 +400,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _params_sub(-1), _manual_control_sub(-1), _sensor_combined_sub(-1), - _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -428,7 +416,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _global_pos(), _pos_sp_triplet(), _sensor_combined(), - _range_finder(), /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")), @@ -477,7 +464,6 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); - _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -577,8 +563,6 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); - param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); - _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -669,20 +653,6 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } -bool -FixedwingPositionControl::range_finder_poll() -{ - /* check if there is a range finder measurement */ - bool range_finder_updated; - orb_check(_range_finder_sub, &range_finder_updated); - - if (range_finder_updated) { - orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); - } - - return range_finder_updated; -} - void FixedwingPositionControl::vehicle_attitude_poll() { @@ -820,21 +790,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } -float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - float rel_alt_estimated = current_alt - land_setpoint_alt; - - /* only use range finder if: - * parameter (range_finder_use_relative_alt) > 0 - * the measurement is valid - * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt - */ - if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) { - return rel_alt_estimated; + if (global_pos.terrain_alt_valid && + isfinite(global_pos.terrain_alt)) { + return global_pos.terrain_alt; + } else { + return land_setpoint_alt; } - - return range_finder.distance; - } bool @@ -991,15 +954,19 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; + /* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be + * equal to _pos_sp_triplet.current.alt */ + float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos); + /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ - float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f; + float L_altitude_rel = _pos_sp_triplet.previous.valid ? + _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -1009,7 +976,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -1027,12 +994,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel, + tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, flare_pitch_angle_rad, math::radians(15.0f), 0.0f, throttle_max, throttle_land, false, flare_pitch_angle_rad, - _pos_sp_triplet.current.alt + relative_alt, ground_speed, + _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); if (!land_noreturn_vertical) { @@ -1053,8 +1020,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is below the slope continue at previous wp altitude * until the intersection with slope * */ - float altitude_desired_rel = relative_alt; - if (relative_alt > landing_slope_alt_rel_desired || land_onslope) { + float altitude_desired_rel; + if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) { /* stay on slope */ altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { @@ -1063,10 +1030,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } } else { /* continue horizontally */ - altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt; + altitude_desired_rel = _pos_sp_triplet.previous.valid ? L_altitude_rel : + _global_pos.alt - terrain_alt; } - tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel, + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max), @@ -1075,7 +1043,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _parameters.throttle_cruise, false, math::radians(_parameters.pitch_limit_min), - _pos_sp_triplet.current.alt + relative_alt, + _global_pos.alt, ground_speed); } @@ -1221,7 +1189,6 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); - _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1295,7 +1262,6 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); - range_finder_poll(); // vehicle_baro_poll(); math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e, _global_pos.vel_d); From 8ad1aa789b6f10769dd9977151c9a39971b1ebaf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:13:29 +0200 Subject: [PATCH 035/138] mc_pos_control: reset position setpoint on entering to AUTO mode --- .../mc_pos_control/mc_pos_control_main.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 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 847a92151c..01fdb4c653 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,6 +180,7 @@ private: bool _reset_pos_sp; bool _reset_alt_sp; + bool _mode_auto; math::Vector<3> _pos; math::Vector<3> _pos_sp; @@ -292,7 +293,8 @@ MulticopterPositionControl::MulticopterPositionControl() : _ref_timestamp(0), _reset_pos_sp(true), - _reset_alt_sp(true) + _reset_alt_sp(true), + _mode_auto(false) { memset(&_att, 0, sizeof(_att)); memset(&_att_sp, 0, sizeof(_att_sp)); @@ -684,6 +686,13 @@ MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, f void MulticopterPositionControl::control_auto(float dt) { + if (!_mode_auto) { + _mode_auto = true; + /* reset position setpoint on AUTO mode activation */ + reset_pos_sp(); + reset_alt_sp(); + } + bool updated; orb_check(_pos_sp_triplet_sub, &updated); @@ -807,9 +816,7 @@ MulticopterPositionControl::control_auto(float dt) } } else { - /* no waypoint, loiter, reset position setpoint if needed */ - reset_pos_sp(); - reset_alt_sp(); + /* no waypoint, do nothing, setpoint was already reset */ } } @@ -916,10 +923,12 @@ MulticopterPositionControl::task_main() if (_control_mode.flag_control_manual_enabled) { /* manual control */ control_manual(dt); + _mode_auto = false; } else if (_control_mode.flag_control_offboard_enabled) { /* offboard control */ control_offboard(dt); + _mode_auto = false; } else { /* AUTO */ @@ -1271,9 +1280,9 @@ MulticopterPositionControl::task_main() /* position controller disabled, reset setpoints */ _reset_alt_sp = true; _reset_pos_sp = true; + _mode_auto = false; reset_int_z = true; reset_int_xy = true; - } /* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */ From 132c9180ead0c64c2edcc36dec2bf8896679a040 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 1 Sep 2014 18:47:56 +0200 Subject: [PATCH 036/138] mc_pos_control: move position offset limiting to separate method --- .../mc_pos_control/mc_pos_control_main.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) 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 01fdb4c653..ce26f7dc25 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -221,6 +221,11 @@ private: */ void reset_alt_sp(); + /** + * Check if position setpoint is too far from current position and adjust it if needed. + */ + void limit_pos_sp_offset(); + /** * Set position setpoint using manual control */ @@ -544,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp() } } +void +MulticopterPositionControl::limit_pos_sp_offset() +{ + math::Vector<3> pos_sp_offs; + pos_sp_offs.zero(); + + if (_control_mode.flag_control_position_enabled) { + pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0); + pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1); + } + + if (_control_mode.flag_control_altitude_enabled) { + pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2); + } + + float pos_sp_offs_norm = pos_sp_offs.length(); + + if (pos_sp_offs_norm > 1.0f) { + pos_sp_offs /= pos_sp_offs_norm; + _pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max); + } +} + void MulticopterPositionControl::control_manual(float dt) { From 7db57310d5cdd7295da6c919dafa92d3831296db Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:16 -0700 Subject: [PATCH 037/138] New mavlink_ftp unit test data --- .../mavlink_tests/empty_dir/.gitignore | 0 .../unit_test_data/mavlink_tests/test_234.data | Bin 0 -> 234 bytes .../unit_test_data/mavlink_tests/test_235.data | Bin 0 -> 235 bytes .../unit_test_data/mavlink_tests/test_236.data | Bin 0 -> 236 bytes 4 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data create mode 100644 ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/empty_dir/.gitignore new file mode 100644 index 0000000000..e69de29bb2 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data new file mode 100644 index 0000000000000000000000000000000000000000..3e075aa4f5df1d00f40e8d3b25549fdbe89e5212 GIT binary patch literal 234 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T k$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>6l+?egFUf literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data new file mode 100644 index 0000000000000000000000000000000000000000..61372f7699dab331a855c15bdbfec31e06982c9b GIT binary patch literal 235 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T l$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>FNX4YiIxf literal 0 HcmV?d00001 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data new file mode 100644 index 0000000000000000000000000000000000000000..eb7fcb11a5216bd7c3c7e553279d52a945eeb8d1 GIT binary patch literal 236 zcmVRaRG6Sz23MU0z>c zVPa!sWoBn+X=-b1ZEkOHadLBXb#`}nd3t+%eSUv{fr5jCg@%WSiHeJijgF6yk&=^? zm6n&7nVOrNot~edp`xRtrKYE-sj922t*)=Iv9hzYwYImoxw^Z&y}rM|!NSAD#m2|T m$;!*j&Cbuz(bCh@)z;V8+1lIO-QM5e;o{@u<>u$;>FVonDQr^! literal 0 HcmV?d00001 From 81275e624e87d5cbb10535ffa2fcc2adbccc793f Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:33 -0700 Subject: [PATCH 038/138] Added mavlink_ftp unit test --- makefiles/config_px4fmu-v2_test.mk | 3 +++ 1 file changed, 3 insertions(+) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 932f922617..a41670a77b 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -54,6 +54,9 @@ MODULES += lib/conversion # LIBRARIES += lib/mathlib/CMSIS +MODULES += modules/unit_test +MODULES += modules/mavlink/mavlink_tests + # # Transitional support - add commands from the NuttX export archive. # From 6f0160bb5db552c921773f278e93d33862811a2a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:32:50 -0700 Subject: [PATCH 039/138] New mavlink_ftp unit test --- .../mavlink_tests/mavlink_ftp_test.cpp | 788 ++++++++++++++++++ .../mavlink/mavlink_tests/mavlink_ftp_test.h | 109 +++ .../mavlink/mavlink_tests/mavlink_tests.cpp | 52 ++ src/modules/mavlink/mavlink_tests/module.mk | 48 ++ 4 files changed, 997 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_tests.cpp create mode 100644 src/modules/mavlink/mavlink_tests/module.mk diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp new file mode 100644 index 0000000000..ebfce6d749 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -0,0 +1,788 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_test.cpp +/// @author Don Gagne + +#include +#include +#include +#include + +#include "mavlink_ftp_test.h" +#include "../mavlink_ftp.h" + +/// @brief Test case file name for Read command. File are generated using mavlink_ftp_test_data.py +const MavlinkFtpTest::ReadTestCase MavlinkFtpTest::_rgReadTestCases[] = { + { "/etc/unit_test_data/mavlink_tests/test_234.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) - 1}, // Read takes less than single packet + { "/etc/unit_test_data/mavlink_tests/test_235.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) }, // Read completely fills single packet + { "/etc/unit_test_data/mavlink_tests/test_236.data", MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader) + 1 }, // Read take two packets +}; + +const char MavlinkFtpTest::_unittest_microsd_dir[] = "/fs/microsd/ftp_unit_test_dir"; +const char MavlinkFtpTest::_unittest_microsd_file[] = "/fs/microsd/ftp_unit_test_dir/file"; + +MavlinkFtpTest::MavlinkFtpTest() : + _ftp_server{}, + _reply_msg{}, + _lastOutgoingSeqNumber{} +{ +} + +MavlinkFtpTest::~MavlinkFtpTest() +{ + +} + +/// @brief Called before every test to initialize the FTP Server. +void MavlinkFtpTest::init(void) +{ + _ftp_server = new MavlinkFTP;; + _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); + + _cleanup_microsd(); +} + +/// @brief Called after every test to take down the FTP Server. +void MavlinkFtpTest::cleanup(void) +{ + delete _ftp_server; + + _cleanup_microsd(); +} + +/// @brief Tests for correct behavior of an Ack response. +bool MavlinkFtpTest::_ack_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + + return true; +} + +/// @brief Tests for correct response to a message sent with an invalid CRC. +bool MavlinkFtpTest::_bad_crc_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdNone; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->crc32++; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); + + return true; +} + +/// @brief Tests for correct response to an invalid opcpde. +bool MavlinkFtpTest::_bad_opcode_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = 0xFF; // bogus opcode + + bool success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrUnknownCommand); + + return true; +} + +/// @brief Tests for correct reponse to a payload which an invalid data size field. +bool MavlinkFtpTest::_bad_datasize_test(void) +{ + mavlink_message_t msg; + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + + _setup_ftp_msg(&payload, 0, nullptr, &msg); + + // Set the data size to be one larger than is legal + ((MavlinkFTP::PayloadHeader*)((mavlink_file_transfer_protocol_t*)msg.payload64)->payload)->size = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN + 1; + + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + + if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidDataSize); + + return true; +} + +bool MavlinkFtpTest::_list_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response2[] = "Ddev|Detc|Dfs|Dobj"; + + struct _testCase { + const char *dir; + char *response; + bool success; + }; + struct _testCase rgTestCases[] = { + { "/bogus", nullptr, false }, + { "/etc/unit_test_data/mavlink_tests", response1, true }, + { "/", response2, true }, + }; + + for (size_t i=0; iresponse) + 1; + + char *ptr = strtok (test->response, "|"); + while (ptr != nullptr) + { + ptr = strtok (nullptr, "|"); + } + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, expected_data_size); + ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Tests for correct reponse to a List command on a valid directory, but with an offset that +/// is beyond the last directory entry. +bool MavlinkFtpTest::_list_eof_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/"; + + payload.opcode = MavlinkFTP::kCmdListDirectory; + payload.offset = 4; // offset past top level dirs + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrEOF); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file which does not exist. +bool MavlinkFtpTest::_open_badfile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *dir = "/foo"; // non-existent file + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(dir)+1, // size in bytes of data + (uint8_t*)dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + + return true; +} + +/// @brief Tests for correct reponse to an Open command on a file, followed by Terminate +bool MavlinkFtpTest::_open_terminate_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("stat failed", stat(test->file, &st), 0); + + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, sizeof(uint32_t)); + ut_compare("File size incorrect", *((uint32_t*)&reply->data[0]), (uint32_t)st.st_size); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Terminate command on an invalid session. +bool MavlinkFtpTest::_terminate_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session + 1; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an open session. +bool MavlinkFtpTest::_read_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + for (size_t i=0; ifile, &st), 0); + uint8_t *bytes = new uint8_t[st.st_size]; + ut_assert("new failed", bytes != nullptr); + int fd = ::open(test->file, O_RDONLY); + ut_assert("open failed", fd != -1); + int bytes_read = ::read(fd, bytes, st.st_size); + ut_compare("read failed", bytes_read, st.st_size); + ::close(fd); + + // Test case data files are created for specific boundary conditions + ut_compare("Test case data files are out of date", test->length, st.st_size); + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->file)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session; + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Offset incorrect", reply->offset, 0); + + if (test->length <= MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(MavlinkFTP::PayloadHeader)) { + ut_compare("Payload size incorrect", reply->size, (uint32_t)st.st_size); + ut_compare("File contents differ", memcmp(reply->data, bytes, st.st_size), 0); + } + + payload.opcode = MavlinkFTP::kCmdTerminateSession; + payload.session = reply->session; + payload.size = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } + + return true; +} + +/// @brief Tests for correct reponse to a Read command on an invalid session. +bool MavlinkFtpTest::_read_badsession_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + const char *file = _rgReadTestCases[0].file; + + payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(file)+1, // size in bytes of data + (uint8_t*)file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + + payload.opcode = MavlinkFTP::kCmdReadFile; + payload.session = reply->session + 1; // Invalid session + payload.offset = 0; + + success = _send_receive_msg(&payload, // FTP payload header + 0, // size in bytes of data + nullptr, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 1); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrInvalidSession); + + return true; +} + +bool MavlinkFtpTest::_removedirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *dir; + bool success; + bool deleteFile; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false, false }, + { "/etc/unit_test_data/mavlink_tests/empty_dir", false, false }, + { _unittest_microsd_dir, false, false }, + { _unittest_microsd_file, false, false }, + { _unittest_microsd_dir, true, true }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ideleteFile) { + ut_compare("unlink failed", ::unlink(_unittest_microsd_file), 0); + } + + payload.opcode = MavlinkFTP::kCmdRemoveDirectory; + payload.offset = 0; + + bool success = _send_receive_msg(&payload, // FTP payload header + strlen(test->dir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_createdirectory_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + + struct _testCase { + const char *dir; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/etc/bogus", false }, + { _unittest_microsd_dir, true }, + { _unittest_microsd_dir, false }, + { "/fs/microsd/bogus/bogus", false }, + }; + + for (size_t i=0; idir)+1, // size in bytes of data + (uint8_t*)test->dir, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +bool MavlinkFtpTest::_removefile_test(void) +{ + MavlinkFTP::PayloadHeader payload; + mavlink_file_transfer_protocol_t ftp_msg; + MavlinkFTP::PayloadHeader *reply; + int fd; + + struct _testCase { + const char *file; + bool success; + }; + static const struct _testCase rgTestCases[] = { + { "/bogus", false }, + { _rgReadTestCases[0].file, false }, + { _unittest_microsd_dir, false }, + { _unittest_microsd_file, true }, + { _unittest_microsd_file, false }, + }; + + ut_compare("mkdir failed", ::mkdir(_unittest_microsd_dir, S_IRWXU | S_IRWXG | S_IRWXO), 0); + ut_assert("open failed", (fd = ::open(_unittest_microsd_file, O_CREAT | O_EXCL)) != -1); + ::close(fd); + + for (size_t i=0; ifile)+1, // size in bytes of data + (uint8_t*)test->file, // Data to start into FTP message payload + &ftp_msg, // Response from server + &reply); // Payload inside FTP message response + if (!success) { + return false; + } + + if (test->success) { + ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); + ut_compare("Incorrect payload size", reply->size, 0); + } else { + ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); + ut_compare("Incorrect payload size", reply->size, 2); + ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrFailErrno); + } + } + + return true; +} + +/// @brief Static method used as callback from MavlinkFTP. This method will be called by MavlinkFTP when +/// it needs to send a message out on Mavlink. +void MavlinkFtpTest::receive_message(const mavlink_message_t *msg, MavlinkFtpTest *ftp_test) +{ + ftp_test->_receive_message(msg); +} + +/// @brief Non-Static version of receive_message +void MavlinkFtpTest::_receive_message(const mavlink_message_t *msg) +{ + // Move the message into our own member variable + memcpy(&_reply_msg, msg, sizeof(mavlink_message_t)); +} + +/// @brief Decode and validate the incoming message +bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlink message to decode + mavlink_file_transfer_protocol_t *ftp_msg, ///< Decoded FTP message + MavlinkFTP::PayloadHeader **payload) ///< Payload inside FTP message response +{ + mavlink_msg_file_transfer_protocol_decode(msg, ftp_msg); + + // Make sure the targets are correct + ut_compare("Target network non-zero", ftp_msg->target_network, 0); + ut_compare("Target system id mismatch", ftp_msg->target_system, clientSystemId); + ut_compare("Target component id mismatch", ftp_msg->target_component, clientComponentId); + + *payload = reinterpret_cast(ftp_msg->payload); + + // Make sure we have a good CRC + ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); + + // Make sure we have a good sequence number + ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); + + // Bump sequence number for next outgoing message + _lastOutgoingSeqNumber++; + + return true; +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. +uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; +} + +/// @brief Initializes an FTP message into a mavlink message +void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_message_t *msg) ///< Returned mavlink message +{ + uint8_t payload_bytes[MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN]; + MavlinkFTP::PayloadHeader *payload = reinterpret_cast(payload_bytes); + + memcpy(payload, payload_header, sizeof(MavlinkFTP::PayloadHeader)); + + payload->seqNumber = _lastOutgoingSeqNumber; + payload->size = size; + if (size != 0) { + memcpy(payload->data, data, size); + } + + payload->crc32 = _payload_crc32(payload); + + msg->checksum = 0; + mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id + clientComponentId, // Sender component id + msg, // Message to pack payload into + 0, // Target network + serverSystemId, // Target system id + serverComponentId, // Target component id + payload_bytes); // Payload to pack into message +} + +/// @brief Sends the specified FTP message to the server and returns response +bool MavlinkFtpTest::_send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header + uint8_t size, ///< size in bytes of data + const uint8_t *data, ///< Data to start into FTP message payload + mavlink_file_transfer_protocol_t *ftp_msg_reply, ///< Response from server + MavlinkFTP::PayloadHeader **payload_reply) ///< Payload inside FTP message response +{ + mavlink_message_t msg; + + _setup_ftp_msg(payload_header, size, data, &msg); + _ftp_server->handle_message(nullptr /* mavlink */, &msg); + return _decode_message(&_reply_msg, ftp_msg_reply, payload_reply); +} + +/// @brief Cleans up an files created on microsd during testing +void MavlinkFtpTest::_cleanup_microsd(void) +{ + ::unlink(_unittest_microsd_file); + ::rmdir(_unittest_microsd_dir); +} + +/// @brief Runs all the unit tests +void MavlinkFtpTest::runTests(void) +{ + ut_run_test(_ack_test); + ut_run_test(_bad_crc_test); + ut_run_test(_bad_opcode_test); + ut_run_test(_bad_datasize_test); + ut_run_test(_list_test); + ut_run_test(_list_eof_test); + ut_run_test(_open_badfile_test); + ut_run_test(_open_terminate_test); + ut_run_test(_terminate_badsession_test); + ut_run_test(_read_test); + ut_run_test(_read_badsession_test); + ut_run_test(_removedirectory_test); + ut_run_test(_createdirectory_test); + ut_run_test(_removefile_test); +} + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h new file mode 100644 index 0000000000..bbb095a08a --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -0,0 +1,109 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_test.h +/// @author Don Gagne + +#pragma once + +#include +#include "../mavlink_bridge_header.h" +#include "../mavlink_ftp.h" + +class MavlinkFtpTest : public UnitTest +{ +public: + MavlinkFtpTest(); + virtual ~MavlinkFtpTest(); + + virtual void init(void); + virtual void cleanup(void); + + virtual void runTests(void); + + static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + static const uint8_t serverSystemId = 50; ///< System ID for server + static const uint8_t serverComponentId = 1; ///< Component ID for server + static const uint8_t serverChannel = 0; ///< Channel to send to + + static const uint8_t clientSystemId = 1; ///< System ID for client + static const uint8_t clientComponentId = 0; ///< Component ID for client + + // We don't want any of these + MavlinkFtpTest(const MavlinkFtpTest&); + MavlinkFtpTest& operator=(const MavlinkFtpTest&); + +private: + bool _ack_test(void); + bool _bad_crc_test(void); + bool _bad_opcode_test(void); + bool _bad_datasize_test(void); + bool _list_test(void); + bool _list_eof_test(void); + bool _open_badfile_test(void); + bool _open_terminate_test(void); + bool _terminate_badsession_test(void); + bool _read_test(void); + bool _read_badsession_test(void); + bool _removedirectory_test(void); + bool _createdirectory_test(void); + bool _removefile_test(void); + + void _receive_message(const mavlink_message_t *msg); + uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); + void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); + bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); + bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, + uint8_t size, + const uint8_t *data, + mavlink_file_transfer_protocol_t *ftp_msg_reply, + MavlinkFTP::PayloadHeader **payload_reply); + void _cleanup_microsd(void); + + MavlinkFTP *_ftp_server; + + mavlink_message_t _reply_msg; + + uint16_t _lastOutgoingSeqNumber; + + struct ReadTestCase { + const char *file; + const uint16_t length; + }; + static const ReadTestCase _rgReadTestCases[]; + + static const char _unittest_microsd_dir[]; + static const char _unittest_microsd_file[]; +}; + diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp new file mode 100644 index 0000000000..d9c1e413a5 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (C) 2014 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 mavlink_ftp_tests.cpp + */ + +#include + +#include "mavlink_ftp_test.h" + +extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); + +int mavlink_tests_main(int argc, char *argv[]) +{ + MavlinkFtpTest* test = new MavlinkFtpTest; + + test->runTests(); + test->printResults(); + + return 0; +} diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk new file mode 100644 index 0000000000..07cfce1dc9 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -0,0 +1,48 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# System state machine tests. +# + +MODULE_COMMAND = mavlink_tests +SRCS = mavlink_tests.cpp \ + mavlink_ftp_test.cpp \ + ../mavlink_ftp.cpp \ + ../mavlink.c + +INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink + +MODULE_STACKSIZE = 5000 + +EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST From 1969c9ccf6797c4d86f32b9f57332a9e59071251 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:33:43 -0700 Subject: [PATCH 040/138] Improved unit test framework - Added init/cleanup calls before after test - Added ut_compare macro with better failure reporting --- src/modules/unit_test/unit_test.cpp | 5 +++++ src/modules/unit_test/unit_test.h | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index 02d1af4818..be3b9461a8 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -56,3 +56,8 @@ void UnitTest::printAssert(const char* msg, const char* test, const char* file, { warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } + +void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +{ + warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); +} diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 32eb8c308c..1a54897094 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -52,11 +52,15 @@ INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); virtual ~UnitTest(); + + virtual void init(void) { }; + virtual void cleanup(void) { }; virtual void runTests(void) = 0; void printResults(void); void printAssert(const char* msg, const char* test, const char* file, int line); + void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); #define ut_assert(message, test) \ do { \ @@ -68,10 +72,23 @@ INLINE_GLOBAL(const char*, mu_last_test) } \ } while (0) +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + mu_assertion()++; \ + } \ + } while (0) + #define ut_run_test(test) \ do { \ warnx("RUNNING TEST: %s", #test); \ mu_tests_run()++; \ + init(); \ if (!test()) { \ warnx("TEST FAILED: %s", #test); \ mu_tests_failed()++; \ @@ -79,6 +96,7 @@ INLINE_GLOBAL(const char*, mu_last_test) warnx("TEST PASSED: %s", #test); \ mu_tests_passed()++; \ } \ + cleanup(); \ } while (0) }; From e7165d6cbf364cb95e45dad998daa7c494de409e Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:02 -0700 Subject: [PATCH 041/138] Generator for mavlink_ftp test files --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py | 7 +++++++ 1 file changed, 7 insertions(+) create mode 100644 src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py new file mode 100644 index 0000000000..a7e68f2a32 --- /dev/null +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test_data.py @@ -0,0 +1,7 @@ +import sys +print 'Arguments: file - ' + sys.argv[1] + ', length - ' + sys.argv[2] +file = open(sys.argv[1], 'w') +for i in range(int(sys.argv[2])): + b = bytearray([i & 0xFF]) + file.write(b) +file.close() \ No newline at end of file From 37ea85968da111f7dc63cdfa8ca02ad6e601a8c3 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:34:20 -0700 Subject: [PATCH 042/138] Tweak for mavlink_ftp api change --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bed1bd7898..823de5172f 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -121,7 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : { // make sure the FTP server is started - (void)MavlinkFTP::getServer(); + (void)MavlinkFTP::get_server(); } MavlinkReceiver::~MavlinkReceiver() @@ -173,7 +173,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) break; case MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL: - MavlinkFTP::getServer()->handle_message(_mavlink, msg); + MavlinkFTP::get_server()->handle_message(_mavlink, msg); break; default: From 828fb5efd10355d0d33b05fcf597bc4c05cd4db4 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Tue, 2 Sep 2014 15:36:32 -0700 Subject: [PATCH 043/138] Additional FTP command support plus unit test - Restructured to a flatter class implementation - Added support for create/remove directory, remove file - Refactored error codes and command opcodes - Added unit test support - Fixed bugs found by unit test --- src/modules/mavlink/mavlink_ftp.cpp | 445 +++++++++++++++++++--------- src/modules/mavlink/mavlink_ftp.h | 285 +++++++----------- 2 files changed, 423 insertions(+), 307 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 5b65dc369d..72ede87976 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -31,38 +31,39 @@ * ****************************************************************************/ +/// @file mavlink_ftp.cpp +/// @author px4dev, Don Gagne + #include #include #include #include #include +#include #include "mavlink_ftp.h" +#include "mavlink_tests/mavlink_ftp_test.h" // Uncomment the line below to get better debug output. Never commit with this left on. //#define MAVLINK_FTP_DEBUG -MavlinkFTP *MavlinkFTP::_server; - MavlinkFTP * -MavlinkFTP::getServer() +MavlinkFTP::get_server(void) { - // XXX this really cries out for some locking... - if (_server == nullptr) { - _server = new MavlinkFTP; - } - return _server; + static MavlinkFTP server; + return &server; } MavlinkFTP::MavlinkFTP() : - _session_fds{}, - _workBufs{}, - _workFree{}, - _lock{} + _request_bufs{}, + _request_queue{}, + _request_queue_sem{}, + _utRcvMsgFunc{}, + _ftp_test{} { // initialise the request freelist - dq_init(&_workFree); - sem_init(&_lock, 0, 1); + dq_init(&_request_queue); + sem_init(&_request_queue_sem, 0, 1); // initialize session list for (size_t i=0; idecode(mavlink, msg)) { - - // and queue it for the worker - work_queue(LPWORK, &req->work, &MavlinkFTP::_workerTrampoline, req, 0); - } else { - _qFree(req); + if (msg->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { + mavlink_msg_file_transfer_protocol_decode(msg, &req->message); + +#ifdef MAVLINK_FTP_UNIT_TEST + if (!_utRcvMsgFunc) { + warnx("Incorrectly written unit test\n"); + return; + } + // We use fake ids when unit testing + req->serverSystemId = MavlinkFtpTest::serverSystemId; + req->serverComponentId = MavlinkFtpTest::serverComponentId; + req->serverChannel = MavlinkFtpTest::serverChannel; +#else + // Not unit testing, use the real thing + req->serverSystemId = mavlink->get_system_id(); + req->serverComponentId = mavlink->get_component_id(); + req->serverChannel = mavlink->get_channel(); +#endif + + // This is the system id we want to target when sending + req->targetSystemId = msg->sysid; + + if (req->message.target_system == req->serverSystemId) { + req->mavlink = mavlink; +#ifdef MAVLINK_FTP_UNIT_TEST + // We are running in Unit Test mode. Don't queue, just call _worket directly. + _process_request(req); +#else + // We are running in normal mode. Queue the request to the worker + work_queue(LPWORK, &req->work, &MavlinkFTP::_worker_trampoline, req, 0); +#endif + return; } } + + _return_request(req); } +/// @brief Queued static work queue routine to handle mavlink messages void -MavlinkFTP::_workerTrampoline(void *arg) +MavlinkFTP::_worker_trampoline(void *arg) { - auto req = reinterpret_cast(arg); - auto server = MavlinkFTP::getServer(); + Request* req = reinterpret_cast(arg); + MavlinkFTP* server = MavlinkFTP::get_server(); // call the server worker with the work item - server->_worker(req); + server->_process_request(req); } +/// @brief Processes an FTP message void -MavlinkFTP::_worker(Request *req) +MavlinkFTP::_process_request(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); + ErrorCode errorCode = kErrNone; - uint32_t messageCRC; // basic sanity checks; must validate length before use - if (hdr->size > kMaxDataLength) { - errorCode = kErrNoRequest; + if (payload->size > kMaxDataLength) { + errorCode = kErrInvalidDataSize; goto out; } // check request CRC to make sure this is one of ours - messageCRC = hdr->crc32; - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - if (crc32(req->rawData(), req->dataSize()) != messageCRC) { - errorCode = kErrNoRequest; + if (_payload_crc32(payload) != payload->crc32) { + errorCode = kErrCrc; goto out; warnx("ftp: bad crc"); } #ifdef MAVLINK_FTP_DEBUG - printf("ftp: channel %u opc %u size %u offset %u\n", req->channel(), hdr->opcode, hdr->size, hdr->offset); + printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif - switch (hdr->opcode) { + switch (payload->opcode) { case kCmdNone: break; - case kCmdTerminate: - errorCode = _workTerminate(req); + case kCmdTerminateSession: + errorCode = _workTerminate(payload); break; - case kCmdReset: - errorCode = _workReset(); + case kCmdResetSessions: + errorCode = _workReset(payload); break; - case kCmdList: - errorCode = _workList(req); + case kCmdListDirectory: + errorCode = _workList(payload); break; - case kCmdOpen: - errorCode = _workOpen(req, false); + case kCmdOpenFile: + errorCode = _workOpen(payload, false); break; - case kCmdCreate: - errorCode = _workOpen(req, true); + case kCmdCreateFile: + errorCode = _workOpen(payload, true); break; - case kCmdRead: - errorCode = _workRead(req); + case kCmdReadFile: + errorCode = _workRead(payload); break; - case kCmdWrite: - errorCode = _workWrite(req); + case kCmdWriteFile: + errorCode = _workWrite(payload); break; - case kCmdRemove: - errorCode = _workRemove(req); + case kCmdRemoveFile: + errorCode = _workRemoveFile(payload); break; + case kCmdCreateDirectory: + errorCode = _workCreateDirectory(payload); + break; + + case kCmdRemoveDirectory: + errorCode = _workRemoveDirectory(payload); + break; + default: - errorCode = kErrNoRequest; + errorCode = kErrUnknownCommand; break; } out: // handle success vs. error if (errorCode == kErrNone) { - hdr->opcode = kRspAck; + payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); - hdr->opcode = kRspNak; - hdr->size = 1; - hdr->data[0] = errorCode; + payload->opcode = kRspNak; + payload->size = 1; + payload->data[0] = errorCode; + if (errorCode == kErrFailErrno) { + payload->size = 2; + payload->data[1] = errno; + } } + // respond to the request _reply(req); - // free the request buffer back to the freelist - _qFree(req); + _return_request(req); } +/// @brief Sends the specified FTP reponse message out through mavlink void MavlinkFTP::_reply(Request *req) { - auto hdr = req->header(); + PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); - hdr->seqNumber = req->header()->seqNumber + 1; + payload->seqNumber = payload->seqNumber + 1; - // message is assumed to be already constructed in the request buffer, so generate the CRC - hdr->crc32 = 0; - hdr->padding[0] = 0; - hdr->padding[1] = 0; - hdr->padding[2] = 0; - hdr->crc32 = crc32(req->rawData(), req->dataSize()); + payload->crc32 = _payload_crc32(payload); - // then pack and send the reply back to the request source - req->reply(); + mavlink_message_t msg; + msg.checksum = 0; +#ifndef MAVLINK_FTP_UNIT_TEST + uint16_t len = +#endif + mavlink_msg_file_transfer_protocol_pack_chan(req->serverSystemId, // Sender system id + req->serverComponentId, // Sender component id + req->serverChannel, // Channel to send on + &msg, // Message to pack payload into + 0, // Target network + req->targetSystemId, // Target system id + 0, // Target component id + (const uint8_t*)payload); // Payload to pack into message + + bool success = true; +#ifdef MAVLINK_FTP_UNIT_TEST + // Unit test hook is set, call that instead + _utRcvMsgFunc(&msg, _ftp_test); +#else + Mavlink *mavlink = req->mavlink; + + mavlink->lockMessageBufferMutex(); + success = mavlink->message_buffer_write(&msg, len); + mavlink->unlockMessageBufferMutex(); + +#endif + + if (!success) { + warnx("FTP TX ERR"); + } +#ifdef MAVLINK_FTP_DEBUG + else { + warnx("wrote: sys: %d, comp: %d, chan: %d, checksum: %d", + req->serverSystemId, + req->serverComponentId, + req->serverChannel, + msg.checksum); + } +#endif } +/// @brief Responds to a List command MavlinkFTP::ErrorCode -MavlinkFTP::_workList(Request *req) +MavlinkFTP::_workList(PayloadHeader* payload) { - auto hdr = req->header(); - char dirPath[kMaxDataLength]; - strncpy(dirPath, req->dataAsCString(), kMaxDataLength); + strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength); DIR *dp = opendir(dirPath); if (dp == nullptr) { warnx("FTP: can't open path '%s'", dirPath); - return kErrNotDir; + return kErrFailErrno; } #ifdef MAVLINK_FTP_DEBUG - warnx("FTP: list %s offset %d", dirPath, hdr->offset); + warnx("FTP: list %s offset %d", dirPath, payload->offset); #endif ErrorCode errorCode = kErrNone; @@ -239,19 +320,19 @@ MavlinkFTP::_workList(Request *req) unsigned offset = 0; // move to the requested offset - seekdir(dp, hdr->offset); + seekdir(dp, payload->offset); for (;;) { // read the directory entry if (readdir_r(dp, &entry, &result)) { warnx("FTP: list %s readdir_r failure\n", dirPath); - errorCode = kErrIO; + errorCode = kErrFailErrno; break; } // no more entries? if (result == nullptr) { - if (hdr->offset != 0 && offset == 0) { + if (payload->offset != 0 && offset == 0) { // User is requesting subsequent dir entries but there were none. This means the user asked // to seek past EOF. errorCode = kErrEOF; @@ -299,94 +380,95 @@ MavlinkFTP::_workList(Request *req) } // Move the data into the buffer - hdr->data[offset++] = direntType; - strcpy((char *)&hdr->data[offset], buf); + payload->data[offset++] = direntType; + strcpy((char *)&payload->data[offset], buf); #ifdef MAVLINK_FTP_DEBUG - printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]); + printf("FTP: list %s %s\n", dirPath, (char *)&payload->data[offset-1]); #endif offset += nameLen + 1; } closedir(dp); - hdr->size = offset; + payload->size = offset; return errorCode; } +/// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(Request *req, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) { - auto hdr = req->header(); - - int session_index = _findUnusedSession(); + int session_index = _find_unused_session(); if (session_index < 0) { - return kErrNoSession; + warnx("FTP: Open failed - out of sessions\n"); + return kErrNoSessionsAvailable; } - + + char *filename = _data_as_cstring(payload); uint32_t fileSize = 0; if (!create) { struct stat st; - if (stat(req->dataAsCString(), &st) != 0) { - return kErrNotFile; + if (stat(filename, &st) != 0) { + return kErrFailErrno; } fileSize = st.st_size; } int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - int fd = ::open(req->dataAsCString(), oflag); + int fd = ::open(filename, oflag); if (fd < 0) { - return create ? kErrPerm : kErrNotFile; + return kErrFailErrno; } _session_fds[session_index] = fd; - hdr->session = session_index; + payload->session = session_index; if (create) { - hdr->size = 0; + payload->size = 0; } else { - hdr->size = sizeof(uint32_t); - *((uint32_t*)hdr->data) = fileSize; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; } return kErrNone; } +/// @brief Responds to a Read command MavlinkFTP::ErrorCode -MavlinkFTP::_workRead(Request *req) +MavlinkFTP::_workRead(PayloadHeader* payload) { - auto hdr = req->header(); + int session_index = payload->session; - int session_index = hdr->session; - - if (!_validSession(session_index)) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } // Seek to the specified position #ifdef MAVLINK_FTP_DEBUG - warnx("seek %d", hdr->offset); + warnx("seek %d", payload->offset); #endif - if (lseek(_session_fds[session_index], hdr->offset, SEEK_SET) < 0) { + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { // Unable to see to the specified location warnx("seek fail"); return kErrEOF; } - int bytes_read = ::read(_session_fds[session_index], &hdr->data[0], kMaxDataLength); + int bytes_read = ::read(_session_fds[session_index], &payload->data[0], kMaxDataLength); if (bytes_read < 0) { // Negative return indicates error other than eof warnx("read fail %d", bytes_read); - return kErrIO; + return kErrFailErrno; } - hdr->size = bytes_read; + payload->size = bytes_read; return kErrNone; } +/// @brief Responds to a Write command MavlinkFTP::ErrorCode -MavlinkFTP::_workWrite(Request *req) +MavlinkFTP::_workWrite(PayloadHeader* payload) { #if 0 // NYI: Coming soon @@ -409,35 +491,44 @@ MavlinkFTP::_workWrite(Request *req) hdr->size = result; return kErrNone; #else - return kErrPerm; + return kErrUnknownCommand; #endif } +/// @brief Responds to a RemoveFile command MavlinkFTP::ErrorCode -MavlinkFTP::_workRemove(Request *req) +MavlinkFTP::_workRemoveFile(PayloadHeader* payload) { - //auto hdr = req->header(); - - // for now, send error reply - return kErrPerm; + char file[kMaxDataLength]; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + + if (unlink(file) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } } +/// @brief Responds to a Terminate command MavlinkFTP::ErrorCode -MavlinkFTP::_workTerminate(Request *req) +MavlinkFTP::_workTerminate(PayloadHeader* payload) { - auto hdr = req->header(); - - if (!_validSession(hdr->session)) { - return kErrNoSession; + if (!_valid_session(payload->session)) { + return kErrInvalidSession; } - ::close(_session_fds[hdr->session]); + ::close(_session_fds[payload->session]); + _session_fds[payload->session] = -1; + + payload->size = 0; return kErrNone; } +/// @brief Responds to a Reset command MavlinkFTP::ErrorCode -MavlinkFTP::_workReset(void) +MavlinkFTP::_workReset(PayloadHeader* payload) { for (size_t i=0; isize = 0; + return kErrNone; } +/// @brief Responds to a RemoveDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (rmdir(dir) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Responds to a CreateDirectory command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) +{ + char dir[kMaxDataLength]; + strncpy(dir, _data_as_cstring(payload), kMaxDataLength); + + if (mkdir(dir, S_IRWXU | S_IRWXG | S_IRWXO) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + +/// @brief Returns true if the specified session is a valid open session bool -MavlinkFTP::_validSession(unsigned index) +MavlinkFTP::_valid_session(unsigned index) { if ((index >= kMaxSession) || (_session_fds[index] < 0)) { return false; @@ -458,8 +582,9 @@ MavlinkFTP::_validSession(unsigned index) return true; } +/// @brief Returns an unused session index int -MavlinkFTP::_findUnusedSession(void) +MavlinkFTP::_find_unused_session(void) { for (size_t i=0; isize < kMaxDataLength) { - requestData()[header()->size] = '\0'; + if (payload->size < kMaxDataLength) { + payload->data[payload->size] = '\0'; } else { - requestData()[kMaxDataLength - 1] = '\0'; + payload->data[kMaxDataLength - 1] = '\0'; } // and return data - return (char *)&(header()->data[0]); + return (char *)&(payload->data[0]); +} + +/// @brief Returns a unused Request entry. NULL if none available. +MavlinkFTP::Request * +MavlinkFTP::_get_request(void) +{ + _lock_request_queue(); + Request* req = reinterpret_cast(dq_remfirst(&_request_queue)); + _unlock_request_queue(); + return req; +} + +/// @brief Locks a semaphore to provide exclusive access to the request queue +void +MavlinkFTP::_lock_request_queue(void) +{ + do {} + while (sem_wait(&_request_queue_sem) != 0); +} + +/// @brief Unlocks the semaphore providing exclusive access to the request queue +void +MavlinkFTP::_unlock_request_queue(void) +{ + sem_post(&_request_queue_sem); +} + +/// @brief Returns a no longer needed request to the queue +void +MavlinkFTP::_return_request(Request *req) +{ + _lock_request_queue(); + dq_addlast(&req->work.dq, &_request_queue); + _unlock_request_queue(); +} + +/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. +uint32_t +MavlinkFTP::_payload_crc32(PayloadHeader *payload) +{ + // We calculate CRC with crc and padding set to 0. + uint32_t saveCRC = payload->crc32; + payload->crc32 = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; + payload->padding[2] = 0; + uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); + payload->crc32 = saveCRC; + + return retCRC; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1dd8f102e7..b4cc154d11 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -33,17 +33,8 @@ #pragma once -/** - * @file mavlink_ftp.h - * - * MAVLink remote file server. - * - * A limited number of requests (currently 2) may be outstanding at a time. - * Additional messages will be discarded. - * - * Messages consist of a fixed header, followed by a data area. - * - */ +/// @file mavlink_ftp.h +/// @author px4dev, Don Gagne #include #include @@ -54,183 +45,131 @@ #include "mavlink_messages.h" #include "mavlink_main.h" +class MavlinkFtpTest; + +/// @brief MAVLink remote file server. Support FTP like commands using MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL message. +/// A limited number of requests (kRequestQueueSize) may be outstanding at a time. Additional messages will be discarded. class MavlinkFTP { public: + /// @brief Returns the one Mavlink FTP server in the system. + static MavlinkFTP* get_server(void); + + /// @brief Contructor is only public so unit test code can new objects. MavlinkFTP(); + + /// @brief Adds the specified message to the work queue. + void handle_message(Mavlink* mavlink, mavlink_message_t *msg); + + typedef void (*ReceiveMessageFunc_t)(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); + + /// @brief Sets up the server to run in unit test mode. + /// @param rcvmsgFunc Function which will be called to handle outgoing mavlink messages. + /// @param ftp_test MavlinkFtpTest object which the function is associated with + void set_unittest_worker(ReceiveMessageFunc_t rcvMsgFunc, MavlinkFtpTest *ftp_test); - static MavlinkFTP *getServer(); - - // static interface - void handle_message(Mavlink* mavlink, - mavlink_message_t *msg); - -private: - - static const unsigned kRequestQueueSize = 2; - - static MavlinkFTP *_server; - - /// @brief Trying to pack structures across differing compilers is questionable for Clients, so we pad the - /// structure ourselves to 32 bit alignment which should get us what we want. - struct RequestHeader + /// @brief This is the payload which is in mavlink_file_transfer_protocol_t.payload. We pad the structure ourselves to + /// 32 bit alignment to avoid usage of any pack pragmas. + struct PayloadHeader { - uint16_t seqNumber; ///< sequence number for message - uint8_t session; ///< Session id for read and write commands - uint8_t opcode; ///< Command opcode - uint8_t size; ///< Size of data - uint8_t padding[3]; - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 - uint32_t offset; ///< Offsets for List and Read commands - uint8_t data[]; + uint16_t seqNumber; ///< sequence number for message + uint8_t session; ///< Session id for read and write commands + uint8_t opcode; ///< Command opcode + uint8_t size; ///< Size of data + uint8_t padding[3]; ///< 32 bit aligment padding + uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint32_t offset; ///< Offsets for List and Read commands + uint8_t data[]; ///< command data, varies by Opcode }; - + + /// @brief Command opcodes enum Opcode : uint8_t { - kCmdNone, // ignored, always acked - kCmdTerminate, // releases sessionID, closes file - kCmdReset, // terminates all sessions - kCmdList, // list files in from - kCmdOpen, // opens for reading, returns - kCmdRead, // reads bytes from in - kCmdCreate, // creates for writing, returns - kCmdWrite, // appends bytes at in - kCmdRemove, // remove file (only if created by server?) - - kRspAck, - kRspNak + kCmdNone, ///< ignored, always acked + kCmdTerminateSession, ///< Terminates open Read session + kCmdResetSessions, ///< Terminates all open Read sessions + kCmdListDirectory, ///< List files in from + kCmdOpenFile, ///< Opens file at for reading, returns + kCmdReadFile, ///< Reads bytes from in + kCmdCreateFile, ///< Creates file at for writing, returns + kCmdWriteFile, ///< Appends bytes to file in + kCmdRemoveFile, ///< Remove file at + kCmdCreateDirectory, ///< Creates directory at + kCmdRemoveDirectory, ///< Removes Directory at , must be empty + + kRspAck, ///< Ack response + kRspNak ///< Nak response }; - + + /// @brief Error codes returned in Nak response PayloadHeader.data[0]. enum ErrorCode : uint8_t - { + { kErrNone, - kErrNoRequest, - kErrNoSession, - kErrSequence, - kErrNotDir, - kErrNotFile, - kErrEOF, - kErrNotAppend, - kErrTooBig, - kErrIO, - kErrPerm - }; - - int _findUnusedSession(void); - bool _validSession(unsigned index); - - static const unsigned kMaxSession = 2; - int _session_fds[kMaxSession]; - - class Request + kErrFail, ///< Unknown failure + kErrFailErrno, ///< Command failed, errno sent back in PayloadHeader.data[1] + kErrInvalidDataSize, ///< PayloadHeader.size is invalid + kErrInvalidSession, ///< Session is not currently open + kErrNoSessionsAvailable, ///< All available Sessions in use + kErrEOF, ///< Offset past end of file for List and Read commands + kErrUnknownCommand, ///< Unknown command opcode + kErrCrc ///< CRC on Payload is incorrect + }; + +private: + /// @brief Unit of work which is queued to work_queue + struct Request { - public: - union { - dq_entry_t entry; - work_s work; - }; - - bool decode(Mavlink *mavlink, mavlink_message_t *fromMessage) { - if (fromMessage->msgid == MAVLINK_MSG_ID_FILE_TRANSFER_PROTOCOL) { - _systemId = fromMessage->sysid; - _mavlink = mavlink; - mavlink_msg_file_transfer_protocol_decode(fromMessage, &_message); - return _message.target_system == _mavlink->get_system_id(); - } - return false; - } - - void reply() { - - // XXX the proper way would be an IOCTL / uORB call, rather than exploiting the - // flat memory architecture, as we're operating between threads here. - mavlink_message_t msg; - msg.checksum = 0; - unsigned len = mavlink_msg_file_transfer_protocol_pack_chan(_mavlink->get_system_id(), // Sender system id - _mavlink->get_component_id(), // Sender component id - _mavlink->get_channel(), // Channel to send on - &msg, // Message to pack payload into - 0, // Target network - _systemId, // Target system id - 0, // Target component id - rawData()); // Payload to pack into message - - _mavlink->lockMessageBufferMutex(); - bool success = _mavlink->message_buffer_write(&msg, len); - _mavlink->unlockMessageBufferMutex(); - - if (!success) { - warnx("FTP TX ERR"); - } -#ifdef MAVLINK_FTP_DEBUG - else { - warnx("wrote: sys: %d, comp: %d, chan: %d, len: %d, checksum: %d", - _mavlink->get_system_id(), - _mavlink->get_component_id(), - _mavlink->get_channel(), - len, - msg.checksum); - } -#endif - } - - uint8_t *rawData() { return &_message.payload[0]; } - RequestHeader *header() { return reinterpret_cast(&_message.payload[0]); } - uint8_t *requestData() { return &(header()->data[0]); } - unsigned dataSize() { return header()->size + sizeof(RequestHeader); } - mavlink_channel_t channel() { return _mavlink->get_channel(); } - - char *dataAsCString(); - - private: - Mavlink *_mavlink; - mavlink_file_transfer_protocol_t _message; - uint8_t _systemId; + work_s work; ///< work queue entry + Mavlink *mavlink; ///< Mavlink to reply to + uint8_t serverSystemId; ///< System ID to send from + uint8_t serverComponentId; ///< Component ID to send from + uint8_t serverChannel; ///< Channel to send to + uint8_t targetSystemId; ///< System ID to target reply to + mavlink_file_transfer_protocol_t message; ///< Protocol message }; + + Request *_get_request(void); + void _return_request(Request *req); + void _lock_request_queue(void); + void _unlock_request_queue(void); + + uint32_t _payload_crc32(PayloadHeader *hdr); + + char *_data_as_cstring(PayloadHeader* payload); + + static void _worker_trampoline(void *arg); + void _process_request(Request *req); + void _reply(Request *req); - static const char kDirentFile = 'F'; - static const char kDirentDir = 'D'; - static const char kDirentUnknown = 'U'; - static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(RequestHeader); + ErrorCode _workList(PayloadHeader *payload); + ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workRead(PayloadHeader *payload); + ErrorCode _workWrite(PayloadHeader *payload); + ErrorCode _workTerminate(PayloadHeader *payload); + ErrorCode _workReset(PayloadHeader* payload); + ErrorCode _workRemoveDirectory(PayloadHeader *payload); + ErrorCode _workCreateDirectory(PayloadHeader *payload); + ErrorCode _workRemoveFile(PayloadHeader *payload); - /// Request worker; runs on the low-priority work queue to service - /// remote requests. - /// - static void _workerTrampoline(void *arg); - void _worker(Request *req); - - /// Reply to a request (XXX should be a Request method) - /// - void _reply(Request *req); - - ErrorCode _workList(Request *req); - ErrorCode _workOpen(Request *req, bool create); - ErrorCode _workRead(Request *req); - ErrorCode _workWrite(Request *req); - ErrorCode _workRemove(Request *req); - ErrorCode _workTerminate(Request *req); - ErrorCode _workReset(); - - // work freelist - Request _workBufs[kRequestQueueSize]; - dq_queue_t _workFree; - sem_t _lock; - - void _qLock() { do {} while (sem_wait(&_lock) != 0); } - void _qUnlock() { sem_post(&_lock); } - - void _qFree(Request *req) { - _qLock(); - dq_addlast(&req->entry, &_workFree); - _qUnlock(); - } - - Request *_dqFree() { - _qLock(); - auto req = reinterpret_cast(dq_remfirst(&_workFree)); - _qUnlock(); - return req; - } - + static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests + Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work + dq_queue_t _request_queue; ///< Queue of available Request buffers + sem_t _request_queue_sem; ///< Semaphore for locking access to _request_queue + + int _find_unused_session(void); + bool _valid_session(unsigned index); + + static const char kDirentFile = 'F'; ///< Identifies File returned from List command + static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command + + /// @brief Maximum data size in RequestHeader::data + static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); + + static const unsigned kMaxSession = 2; ///< Max number of active sessions + int _session_fds[kMaxSession]; ///< Session file descriptors, 0 for empty slot + + ReceiveMessageFunc_t _utRcvMsgFunc; ///< Unit test override for mavlink message sending + MavlinkFtpTest *_ftp_test; ///< Additional parameter to _utRcvMsgFunc; }; From 7f0ba70b1e8e86334d1acb827d8d2d4bf7305870 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 08:39:26 +0200 Subject: [PATCH 044/138] fw landing: if using terrain estimate, don't switch back during landing --- .../fw_pos_control_l1_main.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) 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 064b8a6fae..0db857727a 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 @@ -160,12 +160,12 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ /* land states */ - /* not in non-abort mode for landing yet */ bool land_noreturn_horizontal; bool land_noreturn_vertical; bool land_stayonground; bool land_motor_lim; bool land_onslope; + bool land_useterrain; /* takeoff/launch states */ bool launch_detected; @@ -425,6 +425,7 @@ FixedwingPositionControl::FixedwingPositionControl() : land_stayonground(false), land_motor_lim(false), land_onslope(false), + land_useterrain(false), launch_detected(false), usePreTakeoffThrust(false), last_manual(false), @@ -792,8 +793,14 @@ void FixedwingPositionControl::navigation_capabilities_publish() float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos) { - if (global_pos.terrain_alt_valid && - isfinite(global_pos.terrain_alt)) { + if (!isfinite(global_pos.terrain_alt)) { + return land_setpoint_alt; + } + + /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it + * for the whole landing */ + if (global_pos.terrain_alt_valid || land_useterrain) { + land_useterrain = true; return global_pos.terrain_alt; } else { return land_setpoint_alt; @@ -941,11 +948,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Vertical landing control */ //xxx: using the tecs altitude controller for slope control for now - -// /* do not go down too early */ -// if (wp_distance > 50.0f) { -// altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt; -// } /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param @@ -967,7 +969,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi if ( (_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out - /* land with minimal speed */ // /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ @@ -993,6 +994,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } + warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1034,6 +1036,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } + warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); + tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), @@ -1326,6 +1330,7 @@ void FixedwingPositionControl::reset_landing_state() land_stayonground = false; land_motor_lim = false; land_onslope = false; + land_useterrain = false; } void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas, From 484f2864d188fa04cbbbd767278b547b7f9969af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 12:36:44 +0200 Subject: [PATCH 045/138] remove warnx --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 3 --- 1 file changed, 3 deletions(-) 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 0db857727a..6e06b2b788 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 @@ -994,7 +994,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - warnx("flare: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + flare_curve_alt_rel)); tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, @@ -1036,8 +1035,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _global_pos.alt - terrain_alt; } - warnx("approach: alt %4.3f, sp %4.3f", (double)_global_pos.alt, (double)(terrain_alt + altitude_desired_rel)); - tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), eas2tas, math::radians(_parameters.pitch_limit_min), From 139ca8c327a80b61213e2d75dd8819cf119d22d7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Wed, 3 Sep 2014 18:53:05 +0200 Subject: [PATCH 046/138] fw landing: impose horizontal limit for start of flare --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) 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 84c14be018..e71e7a2ac7 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 @@ -999,7 +999,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); - if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + /* Check if we should start flaring with a vertical and a + * horizontal limit (with some tolerance) */ + if ( ((relative_alt < landingslope.flare_relative_alt()) && + (wp_distance < landingslope.flare_length() + 5.0f)) || + land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ From 1d038eed047358eee30513673bbcb29bf93dd78e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Sep 2014 22:44:16 +0400 Subject: [PATCH 047/138] UAVCAN: GNSS fix message update --- src/modules/uavcan/sensors/gnss.cpp | 8 +++----- uavcan | 2 +- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 0d67aad476..24afe6aaf4 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -43,8 +43,6 @@ #include #include -#define MM_PER_CM 10 // Millimeters per centimeter - const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : @@ -95,9 +93,9 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure Date: Thu, 4 Sep 2014 11:11:03 +0200 Subject: [PATCH 048/138] fw landing: horiz flare check: fix wp distance better calculation of wp distance when behind wp --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) 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 409b57d63e..e8e316064d 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 @@ -909,10 +909,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) { float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); + float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); /* Horizontal landing control */ /* switch to heading hold for the last meters, continue heading hold after */ float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); + /* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */ + float wp_distance_save = wp_distance; + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) { + wp_distance_save = 0.0f; + } + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) { @@ -964,13 +971,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - terrain_alt : 0.0f; - float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* Check if we should start flaring with a vertical and a - * horizontal limit (with some tolerance) */ + * horizontal limit (with some tolerance) + * The horizontal limit is only applied when we are in front of the wp + */ if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) && - (wp_distance < landingslope.flare_length() + 5.0f)) || + (wp_distance_save < landingslope.flare_length() + 5.0f)) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ From 3684bf71d5068a03587005ba365c8705e327e936 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 18:12:20 +0200 Subject: [PATCH 049/138] make flare pitch angle a param --- .../fw_pos_control_l1_main.cpp | 14 +- .../fw_pos_control_l1_params.c | 134 ++++++++++-------- 2 files changed, 86 insertions(+), 62 deletions(-) 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 e8e316064d..c0c238ba9b 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 @@ -237,6 +237,8 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float land_flare_pitch_min_deg; + float land_flare_pitch_max_deg; } _parameters; /**< local copies of interesting parameters */ @@ -281,6 +283,8 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t land_flare_pitch_min_deg; + param_t land_flare_pitch_max_deg; } _parameter_handles; /**< handles for interesting parameters */ @@ -465,6 +469,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); + _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -563,6 +569,8 @@ FixedwingPositionControl::parameters_update() } param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); + param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -958,7 +966,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1) float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; float airspeed_land = 1.3f * _parameters.airspeed_min; float airspeed_approach = 1.3f * _parameters.airspeed_min; @@ -1008,9 +1015,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), eas2tas, - flare_pitch_angle_rad, math::radians(15.0f), + math::radians(_parameters.land_flare_pitch_min_deg), + math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, flare_pitch_angle_rad, + false, math::radians(_parameters.land_flare_pitch_min_deg), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 890ab3bad2..f2e8c422f3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * * @group L1 Control @@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide * some aerodynamic drag from a turning prop to improve the descent rate. * * For aircraft with internal combustion engine this parameter should be set @@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Throttle limit value before flare * - * This throttle value will be set as throttle limit at FW_LND_TLALT, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); /** * Maximum climb rate * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * * @group L1 Control @@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); /** * Minimum descent rate * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * * @group Fixed Wing TECS @@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); /** * Integrator gain * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and * increases overshoot. * * @group Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** * Complementary filter "omega" parameter for height * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); /** * Complementary filter "omega" parameter for speed * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** * Pitch damping factor * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned * properly. * * @group Fixed Wing TECS @@ -414,3 +414,19 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); + +/** + * Flare, minimum pitch + * + * Minimum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); + +/** + * Flare, maximum pitch + * + * Maximum pitch during flare, a positive sign means nose up + * + */ +PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); From 4c7d6707936718760639ee60b3c27698f0ca119c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:04:42 +0200 Subject: [PATCH 050/138] loiter mission items: better reached check for FW --- src/modules/navigator/mission_block.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 4adf77dce6..157a1cf712 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -113,6 +113,19 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (!_navigator->get_vstatus()->is_rotary_wing && + (_mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { + /* Loiter mission item on a non rotary wing: the aircraft is going to circle the + * coordinates with a radius equal to the loiter_radius field + * Therefore the item is marked as reached once the system reaches the loiter + * radius (+ some margin). Time inside and turn count is handled elsewhere. + */ + if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { + _waypoint_position_reached = true; + } + } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From 81837dcdde4bdef31fc153cd1f893e2fefc74c58 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 10:18:25 +0200 Subject: [PATCH 051/138] update comment --- src/modules/navigator/mission_block.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 157a1cf712..723caec7ce 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -118,14 +118,14 @@ MissionBlock::is_mission_item_reached() _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT)) { /* Loiter mission item on a non rotary wing: the aircraft is going to circle the - * coordinates with a radius equal to the loiter_radius field + * coordinates with a radius equal to the loiter_radius field. It is not flying + * through the waypoint center. * Therefore the item is marked as reached once the system reaches the loiter * radius (+ some margin). Time inside and turn count is handled elsewhere. */ if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) { _waypoint_position_reached = true; } - } else { /* for normal mission items used their acceptance radius */ if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { From e056fbb7038e4274fc54f4e19e66c8174ffff28f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Thu, 4 Sep 2014 10:59:35 +0200 Subject: [PATCH 052/138] add heightrate ff for tecs --- src/lib/external_lgpl/tecs/tecs.cpp | 2 +- src/lib/external_lgpl/tecs/tecs.h | 6 + .../fw_pos_control_l1_main.cpp | 5 + .../fw_pos_control_l1_params.c | 125 +++++++++--------- 4 files changed, 78 insertions(+), 60 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp index d27bf776fb..023bd71bfc 100644 --- a/src/lib/external_lgpl/tecs/tecs.cpp +++ b/src/lib/external_lgpl/tecs/tecs.cpp @@ -238,7 +238,7 @@ void TECS::_update_height_demand(float demand, float state) _hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last; _hgt_dem_adj_last = _hgt_dem_adj; - _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p; + _hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT; // Limit height rate of change if (_hgt_rate_dem > _maxClimbRate) { _hgt_rate_dem = _maxClimbRate; diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 36ae4ecaf2..8ac31de6f5 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -47,6 +47,7 @@ public: _rollComp(0.0f), _spdWeight(0.5f), _heightrate_p(0.0f), + _heightrate_ff(0.0f), _speedrate_p(0.0f), _throttle_dem(0.0f), _pitch_dem(0.0f), @@ -220,6 +221,10 @@ public: _heightrate_p = heightrate_p; } + void set_heightrate_ff(float heightrate_ff) { + _heightrate_ff = heightrate_ff; + } + void set_speedrate_p(float speedrate_p) { _speedrate_p = speedrate_p; } @@ -256,6 +261,7 @@ private: float _rollComp; float _spdWeight; float _heightrate_p; + float _heightrate_ff; float _speedrate_p; // throttle demand in the range from 0.0 to 1.0 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 84c14be018..d339b1c4d0 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 @@ -211,6 +211,7 @@ private: float max_climb_rate; float climbout_diff; float heightrate_p; + float heightrate_ff; float speedrate_p; float throttle_damp; float integrator_gain; @@ -256,6 +257,7 @@ private: param_t max_climb_rate; param_t climbout_diff; param_t heightrate_p; + param_t heightrate_ff; param_t speedrate_p; param_t throttle_damp; param_t integrator_gain; @@ -494,6 +496,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); + _parameter_handles.heightrate_ff = param_find("FW_T_HRATE_FF"); _parameter_handles.speedrate_p = param_find("FW_T_SRATE_P"); /* fetch initial parameter values */ @@ -563,6 +566,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff)); param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p)); + param_get(_parameter_handles.heightrate_ff, &(_parameters.heightrate_ff)); param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p)); param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); @@ -600,6 +604,7 @@ FixedwingPositionControl::parameters_update() _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); _tecs.set_heightrate_p(_parameters.heightrate_p); + _tecs.set_heightrate_ff(_parameters.heightrate_ff); _tecs.set_speedrate_p(_parameters.speedrate_p); /* sanity check parameters */ diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 890ab3bad2..847ecbb5cb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -131,8 +131,8 @@ PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f); /** * Throttle limit max * - * This is the maximum throttle % that can be used by the controller. - * For overpowered aircraft, this should be reduced to a value that + * This is the maximum throttle % that can be used by the controller. + * For overpowered aircraft, this should be reduced to a value that * provides sufficient thrust to climb at the maximum pitch angle PTCH_MAX. * * @group L1 Control @@ -142,10 +142,10 @@ PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); /** * Throttle limit min * - * This is the minimum throttle % that can be used by the controller. - * For electric aircraft this will normally be set to zero, but can be set - * to a small non-zero value if a folding prop is fitted to prevent the - * prop from folding and unfolding repeatedly in-flight or to provide + * This is the minimum throttle % that can be used by the controller. + * For electric aircraft this will normally be set to zero, but can be set + * to a small non-zero value if a folding prop is fitted to prevent the + * prop from folding and unfolding repeatedly in-flight or to provide * some aerodynamic drag from a turning prop to improve the descent rate. * * For aircraft with internal combustion engine this parameter should be set @@ -158,7 +158,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); /** * Throttle limit value before flare * - * This throttle value will be set as throttle limit at FW_LND_TLALT, + * This throttle value will be set as throttle limit at FW_LND_TLALT, * before arcraft will flare. * * @group L1 Control @@ -180,17 +180,17 @@ PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f); /** * Maximum climb rate * - * This is the best climb rate that the aircraft can achieve with - * the throttle set to THR_MAX and the airspeed set to the - * default value. For electric aircraft make sure this number can be - * achieved towards the end of flight when the battery voltage has reduced. - * The setting of this parameter can be checked by commanding a positive - * altitude change of 100m in loiter, RTL or guided mode. If the throttle - * required to climb is close to THR_MAX and the aircraft is maintaining - * airspeed, then this parameter is set correctly. If the airspeed starts - * to reduce, then the parameter is set to high, and if the throttle - * demand required to climb and maintain speed is noticeably less than - * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or + * This is the best climb rate that the aircraft can achieve with + * the throttle set to THR_MAX and the airspeed set to the + * default value. For electric aircraft make sure this number can be + * achieved towards the end of flight when the battery voltage has reduced. + * The setting of this parameter can be checked by commanding a positive + * altitude change of 100m in loiter, RTL or guided mode. If the throttle + * required to climb is close to THR_MAX and the aircraft is maintaining + * airspeed, then this parameter is set correctly. If the airspeed starts + * to reduce, then the parameter is set to high, and if the throttle + * demand required to climb and maintain speed is noticeably less than + * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or * FW_THR_MAX reduced. * * @group L1 Control @@ -200,8 +200,8 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); /** * Minimum descent rate * - * This is the sink rate of the aircraft with the throttle - * set to THR_MIN and flown at the same airspeed as used + * This is the sink rate of the aircraft with the throttle + * set to THR_MIN and flown at the same airspeed as used * to measure FW_T_CLMB_MAX. * * @group Fixed Wing TECS @@ -211,10 +211,10 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f); /** * Maximum descent rate * - * This sets the maximum descent rate that the controller will use. - * If this value is too large, the aircraft can over-speed on descent. - * This should be set to a value that can be achieved without - * exceeding the lower pitch angle limit and without over-speeding + * This sets the maximum descent rate that the controller will use. + * If this value is too large, the aircraft can over-speed on descent. + * This should be set to a value that can be achieved without + * exceeding the lower pitch angle limit and without over-speeding * the aircraft. * * @group Fixed Wing TECS @@ -224,7 +224,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f); /** * TECS time constant * - * This is the time constant of the TECS control algorithm (in seconds). + * This is the time constant of the TECS control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -235,7 +235,7 @@ PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f); /** * TECS Throttle time constant * - * This is the time constant of the TECS throttle control algorithm (in seconds). + * This is the time constant of the TECS throttle control algorithm (in seconds). * Smaller values make it faster to respond, larger values make it slower * to respond. * @@ -246,7 +246,7 @@ PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f); /** * Throttle damping factor * - * This is the damping gain for the throttle demand loop. + * This is the damping gain for the throttle demand loop. * Increase to add damping to correct for oscillations in speed and height. * * @group Fixed Wing TECS @@ -256,9 +256,9 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f); /** * Integrator gain * - * This is the integrator gain on the control loop. - * Increasing this gain increases the speed at which speed - * and height offsets are trimmed out, but reduces damping and + * This is the integrator gain on the control loop. + * Increasing this gain increases the speed at which speed + * and height offsets are trimmed out, but reduces damping and * increases overshoot. * * @group Fixed Wing TECS @@ -269,9 +269,9 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f); * Maximum vertical acceleration * * This is the maximum vertical acceleration (in metres/second square) - * either up or down that the controller will use to correct speed - * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) - * allows for reasonably aggressive pitch changes if required to recover + * either up or down that the controller will use to correct speed + * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) + * allows for reasonably aggressive pitch changes if required to recover * from under-speed conditions. * * @group Fixed Wing TECS @@ -281,10 +281,10 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f); /** * Complementary filter "omega" parameter for height * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse vertical acceleration and barometric height to obtain - * an estimate of height rate and height. Increasing this frequency weights - * the solution more towards use of the barometer, whilst reducing it weights + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse vertical acceleration and barometric height to obtain + * an estimate of height rate and height. Increasing this frequency weights + * the solution more towards use of the barometer, whilst reducing it weights * the solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -294,10 +294,10 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f); /** * Complementary filter "omega" parameter for speed * - * This is the cross-over frequency (in radians/second) of the complementary - * filter used to fuse longitudinal acceleration and airspeed to obtain an + * This is the cross-over frequency (in radians/second) of the complementary + * filter used to fuse longitudinal acceleration and airspeed to obtain an * improved airspeed estimate. Increasing this frequency weights the solution - * more towards use of the arispeed sensor, whilst reducing it weights the + * more towards use of the arispeed sensor, whilst reducing it weights the * solution more towards use of the accelerometer data. * * @group Fixed Wing TECS @@ -307,13 +307,13 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f); /** * Roll -> Throttle feedforward * - * Increasing this gain turn increases the amount of throttle that will - * be used to compensate for the additional drag created by turning. - * Ideally this should be set to approximately 10 x the extra sink rate - * in m/s created by a 45 degree bank turn. Increase this gain if - * the aircraft initially loses energy in turns and reduce if the - * aircraft initially gains energy in turns. Efficient high aspect-ratio - * aircraft (eg powered sailplanes) can use a lower value, whereas + * Increasing this gain turn increases the amount of throttle that will + * be used to compensate for the additional drag created by turning. + * Ideally this should be set to approximately 10 x the extra sink rate + * in m/s created by a 45 degree bank turn. Increase this gain if + * the aircraft initially loses energy in turns and reduce if the + * aircraft initially gains energy in turns. Efficient high aspect-ratio + * aircraft (eg powered sailplanes) can use a lower value, whereas * inefficient low aspect-ratio models (eg delta wings) can use a higher value. * * @group Fixed Wing TECS @@ -323,15 +323,15 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f); /** * Speed <--> Altitude priority * - * This parameter adjusts the amount of weighting that the pitch control - * applies to speed vs height errors. Setting it to 0.0 will cause the - * pitch control to control height and ignore speed errors. This will - * normally improve height accuracy but give larger airspeed errors. - * Setting it to 2.0 will cause the pitch control loop to control speed - * and ignore height errors. This will normally reduce airspeed errors, - * but give larger height errors. The default value of 1.0 allows the pitch - * control to simultaneously control height and speed. - * Note to Glider Pilots - set this parameter to 2.0 (The glider will + * This parameter adjusts the amount of weighting that the pitch control + * applies to speed vs height errors. Setting it to 0.0 will cause the + * pitch control to control height and ignore speed errors. This will + * normally improve height accuracy but give larger airspeed errors. + * Setting it to 2.0 will cause the pitch control loop to control speed + * and ignore height errors. This will normally reduce airspeed errors, + * but give larger height errors. The default value of 1.0 allows the pitch + * control to simultaneously control height and speed. + * Note to Glider Pilots - set this parameter to 2.0 (The glider will * adjust its pitch angle to maintain airspeed, ignoring changes in height). * * @group Fixed Wing TECS @@ -341,9 +341,9 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f); /** * Pitch damping factor * - * This is the damping gain for the pitch demand loop. Increase to add - * damping to correct for oscillations in height. The default value of 0.0 - * will work well provided the pitch to servo controller has been tuned + * This is the damping gain for the pitch demand loop. Increase to add + * damping to correct for oscillations in height. The default value of 0.0 + * will work well provided the pitch to servo controller has been tuned * properly. * * @group Fixed Wing TECS @@ -357,6 +357,13 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f); */ PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f); +/** + * Height rate FF factor + * + * @group Fixed Wing TECS + */ +PARAM_DEFINE_FLOAT(FW_T_HRATE_FF, 0.0f); + /** * Speed rate P factor * From dbe35d64ca4288de551ec11b4d5bff43650e8012 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 6 Sep 2014 15:32:12 +0200 Subject: [PATCH 053/138] Fixed delay calculation of laser --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 97abb76a9c..2c50e5c755 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 @@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main() if (newRangeData) { _ekf->fuseRngData = true; _ekf->useRangeFinder = true; - _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f)); + _ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f)); _ekf->GroundEKF(); } From f4664daa8ed80e369c8600f2f1d1ee8d5be328af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:08:56 +0200 Subject: [PATCH 054/138] Add license file --- LICENSE.md | 41 +++++++++++++++++++++++++++++++++++++++++ 1 file changed, 41 insertions(+) create mode 100644 LICENSE.md diff --git a/LICENSE.md b/LICENSE.md new file mode 100644 index 0000000000..2ad83eba48 --- /dev/null +++ b/LICENSE.md @@ -0,0 +1,41 @@ +The PX4 firmware is licensed generally under a permissive 3-clause BSD license. Contributions are required +to be made under the same license. Any exception to this general rule is listed below. + + /**************************************************************************** + * + * Copyright (c) 2012-2014 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. + * + ****************************************************************************/ + + + - PX4 middleware: BSD 3-clause + - PX4 flight control stack: BSD 3-clause + - NuttX operating system: BSD 3-clause + - Exceptions: Currently only this [400 LOC file](https://github.com/PX4/Firmware/blob/master/src/lib/external_lgpl/tecs/tecs.cpp) remains LGPL, but will be replaced with a BSD implementation. \ No newline at end of file From 35a6074419b3dcf567f23db74b8ea53eff20c9d6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Sep 2014 09:12:38 +0200 Subject: [PATCH 055/138] Added readme --- README.md | 10 ++++++++++ 1 file changed, 10 insertions(+) create mode 100644 README.md diff --git a/README.md b/README.md new file mode 100644 index 0000000000..84fb02e3b9 --- /dev/null +++ b/README.md @@ -0,0 +1,10 @@ +## PX4 Aerial Middleware and Flight Control Stack ## + +* Official Website: http://px4.io +* License: BSD 3-clause (see LICENSE.md) +* Supported airframes: + * Multicopters + * Fixed wing +* Binaries (always up-to-date from master): + * [Downloads](https://pixhawk.org/downloads) +* Mailing list: [Google Groups](http://groups.google.com/group/px4users) From 564c9b7b60aaa1187570e188fe7d1e19945d40b5 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:02:25 +0400 Subject: [PATCH 056/138] FTP: Add req_opcode field for return request opcode in response message. --- src/modules/mavlink/mavlink_ftp.cpp | 3 ++- src/modules/mavlink/mavlink_ftp.h | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 72ede87976..ae9246ece0 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -224,12 +224,14 @@ MavlinkFTP::_process_request(Request *req) out: // handle success vs. error if (errorCode == kErrNone) { + payload->req_opcode = payload->opcode; payload->opcode = kRspAck; #ifdef MAVLINK_FTP_DEBUG warnx("FTP: ack\n"); #endif } else { warnx("FTP: nak %u", errorCode); + payload->req_opcode = payload->opcode; payload->opcode = kRspNak; payload->size = 1; payload->data[0] = errorCode; @@ -654,7 +656,6 @@ MavlinkFTP::_payload_crc32(PayloadHeader *payload) payload->crc32 = 0; payload->padding[0] = 0; payload->padding[1] = 0; - payload->padding[2] = 0; uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); payload->crc32 = saveCRC; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b4cc154d11..1fb50a17e6 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -76,7 +76,8 @@ public: uint8_t session; ///< Session id for read and write commands uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data - uint8_t padding[3]; ///< 32 bit aligment padding + uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode From e7ae13a58e83263973feab3630f90f077786fcc3 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:17:11 +0400 Subject: [PATCH 057/138] FTP: Make responses start from opcode 128. --- src/modules/mavlink/mavlink_ftp.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1fb50a17e6..85e175d447 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,7 +98,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty - kRspAck, ///< Ack response + kRspAck = 128, ///< Ack response kRspNak ///< Nak response }; From 0d2e250d119a5a14c8757982c96b2afef09c4d0d Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Tue, 9 Sep 2014 17:36:41 +0400 Subject: [PATCH 058/138] FTP: Remove CRC32 from protocol. Extra crc not needed because mavlink already has crc16. --- src/modules/mavlink/mavlink_ftp.cpp | 27 +++------------------------ src/modules/mavlink/mavlink_ftp.h | 8 ++------ 2 files changed, 5 insertions(+), 30 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index ae9246ece0..b847fc6258 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,7 +34,6 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne -#include #include #include #include @@ -161,13 +160,6 @@ MavlinkFTP::_process_request(Request *req) goto out; } - // check request CRC to make sure this is one of ours - if (_payload_crc32(payload) != payload->crc32) { - errorCode = kErrCrc; - goto out; - warnx("ftp: bad crc"); - } - #ifdef MAVLINK_FTP_DEBUG printf("ftp: channel %u opc %u size %u offset %u\n", req->serverChannel, payload->opcode, payload->size, payload->offset); #endif @@ -255,8 +247,9 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - - payload->crc32 = _payload_crc32(payload); + payload->reserved[0] = 0; + payload->reserved[1] = 0; + payload->reserved[2] = 0; mavlink_message_t msg; msg.checksum = 0; @@ -647,17 +640,3 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding members are set to 0 for calculation. -uint32_t -MavlinkFTP::_payload_crc32(PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 85e175d447..ae7955ab73 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,8 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint8_t padding[2]; ///< 32 bit aligment padding - uint32_t crc32; ///< CRC for entire Request structure, with crc32 and padding set to 0 + uint16_t reserved[3]; ///< reserved area uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; @@ -112,8 +111,7 @@ public: kErrInvalidSession, ///< Session is not currently open kErrNoSessionsAvailable, ///< All available Sessions in use kErrEOF, ///< Offset past end of file for List and Read commands - kErrUnknownCommand, ///< Unknown command opcode - kErrCrc ///< CRC on Payload is incorrect + kErrUnknownCommand ///< Unknown command opcode }; private: @@ -135,8 +133,6 @@ private: void _lock_request_queue(void); void _unlock_request_queue(void); - uint32_t _payload_crc32(PayloadHeader *hdr); - char *_data_as_cstring(PayloadHeader* payload); static void _worker_trampoline(void *arg); From 745707d19377e2c650258ea77570a5f84a71c1b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:22:03 +0400 Subject: [PATCH 059/138] FTP: remove reserved uint32 from header. --- src/modules/mavlink/mavlink_ftp.cpp | 5 ++--- src/modules/mavlink/mavlink_ftp.h | 2 +- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b847fc6258..a5c96274be 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,9 +247,8 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->reserved[0] = 0; - payload->reserved[1] = 0; - payload->reserved[2] = 0; + payload->padding[0] = 0; + payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index ae7955ab73..4892e548c0 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -77,7 +77,7 @@ public: uint8_t opcode; ///< Command opcode uint8_t size; ///< Size of data uint8_t req_opcode; ///< Request opcode returned in kRspAck, kRspNak message - uint16_t reserved[3]; ///< reserved area + uint8_t padding[2]; ///< 32 bit aligment padding uint32_t offset; ///< Offsets for List and Read commands uint8_t data[]; ///< command data, varies by Opcode }; From ed66097ebc4808587301bd569b5bef6c312800c4 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 11:54:42 +0400 Subject: [PATCH 060/138] FTP: Update unit test for new header size. _list_test failed. --- .../{test_234.data => test_238.data} | Bin 234 -> 238 bytes .../{test_235.data => test_239.data} | Bin 235 -> 239 bytes .../{test_236.data => test_240.data} | Bin 236 -> 240 bytes .../mavlink_tests/mavlink_ftp_test.cpp | 57 ++---------------- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 2 - 5 files changed, 6 insertions(+), 53 deletions(-) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_234.data => test_238.data} (85%) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_235.data => test_239.data} (84%) rename ROMFS/px4fmu_test/unit_test_data/mavlink_tests/{test_236.data => test_240.data} (84%) diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data similarity index 85% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_234.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_238.data index 3e075aa4f5df1d00f40e8d3b25549fdbe89e5212..973de16e5965e6caeccd2b13cd41421766c1bf04 100644 GIT binary patch delta 11 TcmaFG_>OVHE0$NU-@F9?B{v5L delta 6 NcmaFI_=<7DD*y{r162S3 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data similarity index 84% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_235.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_239.data index 61372f7699dab331a855c15bdbfec31e06982c9b..7e8764c031918329d226d5e77f4649ce60c71a25 100644 GIT binary patch delta 11 TcmaFO_?~gXYnIn<-o66>C0hp$ delta 6 NcmaFQ_?mITYXA#h16Tk6 diff --git a/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data b/ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data similarity index 84% rename from ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_236.data rename to ROMFS/px4fmu_test/unit_test_data/mavlink_tests/test_240.data index eb7fcb11a5216bd7c3c7e553279d52a945eeb8d1..f3fe31687fc2daf01c80768dc521bd8f659f8db2 100644 GIT binary patch delta 11 TcmaFE_payload)->crc32++; - - _ftp_server->handle_message(nullptr /* mavlink */, &msg); - - if (!_decode_message(&_reply_msg, &ftp_msg, &reply)) { - return false; - } - - ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); - ut_compare("Incorrect payload size", reply->size, 1); - ut_compare("Incorrect error code", reply->data[0], MavlinkFTP::kErrCrc); - - return true; -} - /// @brief Tests for correct response to an invalid opcpde. bool MavlinkFtpTest::_bad_opcode_test(void) { @@ -191,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_234.data\t234|Ftest_235.data\t235|Ftest_236.data\t236"; + char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { @@ -690,9 +663,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin *payload = reinterpret_cast(ftp_msg->payload); - // Make sure we have a good CRC - ut_compare("Incoming CRC mismatch", (*payload)->crc32, _payload_crc32((*payload))); - // Make sure we have a good sequence number ut_compare("Sequence number mismatch", (*payload)->seqNumber, _lastOutgoingSeqNumber + 1); @@ -702,21 +672,6 @@ bool MavlinkFtpTest::_decode_message(const mavlink_message_t *msg, ///< Mavlin return true; } -/// @brief Returns the 32 bit CRC for the payload, crc32 and padding are set to 0 for calculation. -uint32_t MavlinkFtpTest::_payload_crc32(struct MavlinkFTP::PayloadHeader *payload) -{ - // We calculate CRC with crc and padding set to 0. - uint32_t saveCRC = payload->crc32; - payload->crc32 = 0; - payload->padding[0] = 0; - payload->padding[1] = 0; - payload->padding[2] = 0; - uint32_t retCRC = crc32((const uint8_t*)payload, payload->size + sizeof(MavlinkFTP::PayloadHeader)); - payload->crc32 = saveCRC; - - return retCRC; -} - /// @brief Initializes an FTP message into a mavlink message void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, ///< FTP payload header uint8_t size, ///< size in bytes of data @@ -734,7 +689,8 @@ void MavlinkFtpTest::_setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, / memcpy(payload->data, data, size); } - payload->crc32 = _payload_crc32(payload); + payload->padding[0] = 0; + payload->padding[1] = 0; msg->checksum = 0; mavlink_msg_file_transfer_protocol_pack(clientSystemId, // Sender system id @@ -771,7 +727,6 @@ void MavlinkFtpTest::_cleanup_microsd(void) void MavlinkFtpTest::runTests(void) { ut_run_test(_ack_test); - ut_run_test(_bad_crc_test); ut_run_test(_bad_opcode_test); ut_run_test(_bad_datasize_test); ut_run_test(_list_test); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index bbb095a08a..babd909dac 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -66,7 +66,6 @@ public: private: bool _ack_test(void); - bool _bad_crc_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); bool _list_test(void); @@ -81,7 +80,6 @@ private: bool _removefile_test(void); void _receive_message(const mavlink_message_t *msg); - uint32_t _payload_crc32(struct MavlinkFTP::PayloadHeader *payload); void _setup_ftp_msg(MavlinkFTP::PayloadHeader *payload_header, uint8_t size, const uint8_t *data, mavlink_message_t *msg); bool _decode_message(const mavlink_message_t *msg, mavlink_file_transfer_protocol_t *ftp_msg, MavlinkFTP::PayloadHeader **payload); bool _send_receive_msg(MavlinkFTP::PayloadHeader *payload_header, From 0e3c6060b6375fa08b6c0087f7aaf2905ea516d0 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 10 Sep 2014 21:36:08 +0400 Subject: [PATCH 061/138] FTP: remove padding zeroing. --- src/modules/mavlink/mavlink_ftp.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index a5c96274be..64206ac544 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -247,8 +247,6 @@ MavlinkFTP::_reply(Request *req) PayloadHeader *payload = reinterpret_cast(&req->message.payload[0]); payload->seqNumber = payload->seqNumber + 1; - payload->padding[0] = 0; - payload->padding[1] = 0; mavlink_message_t msg; msg.checksum = 0; From 981741f8cea95eebf34577935571260be917abc8 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:11:36 -0700 Subject: [PATCH 062/138] Don't send back U or ./.. entries from List command --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++++-- src/modules/mavlink/mavlink_ftp.h | 1 - 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 64206ac544..cf5fae3b3e 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -349,11 +349,15 @@ MavlinkFTP::_workList(PayloadHeader* payload) } break; case DTYPE_DIRECTORY: + if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { + // Don't bother sending these back + continue; + } direntType = kDirentDir; break; default: - direntType = kDirentUnknown; - break; + // We only send back file and diretory entries, skip everything else + continue; } if (entry.d_type == DTYPE_FILE) { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 4892e548c0..b52561e8ab 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,7 +159,6 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command - static const char kDirentUnknown = 'U'; ///< Identifies Unknown entry returned from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); From dbefa77943fb7f05f391933e82aa4ddf35755851 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Wed, 10 Sep 2014 13:12:04 -0700 Subject: [PATCH 063/138] Update for not getting back "." entries --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 3616db2377..c9566f7db4 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -164,7 +164,7 @@ bool MavlinkFtpTest::_list_test(void) mavlink_file_transfer_protocol_t ftp_msg; MavlinkFTP::PayloadHeader *reply; - char response1[] = "D.|Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; + char response1[] = "Dempty_dir|Ftest_238.data\t238|Ftest_239.data\t239|Ftest_240.data\t240"; char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { From 46a9f616eba368bb864f4faf8e920dad7fd4ecc5 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Fri, 12 Sep 2014 10:54:23 -0700 Subject: [PATCH 064/138] Fix List command test Return order from List command is not repeatable --- .../mavlink_tests/mavlink_ftp_test.cpp | 46 ++++++++++++------- 1 file changed, 30 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index c9566f7db4..022041c74b 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -168,27 +168,20 @@ bool MavlinkFtpTest::_list_test(void) char response2[] = "Ddev|Detc|Dfs|Dobj"; struct _testCase { - const char *dir; - char *response; - bool success; + const char *dir; ///< Directory to run List command on + char *response; ///< Expected response entries from List command + int response_count; ///< Number of directories that should be returned + bool success; ///< true: List command should succeed, false: List command should fail }; struct _testCase rgTestCases[] = { - { "/bogus", nullptr, false }, - { "/etc/unit_test_data/mavlink_tests", response1, true }, - { "/", response2, true }, + { "/bogus", nullptr, 0, false }, + { "/etc/unit_test_data/mavlink_tests", response1, 4, true }, + { "/", response2, 4, true }, }; for (size_t i=0; iresponse) + 1; - - char *ptr = strtok (test->response, "|"); - while (ptr != nullptr) - { - ptr = strtok (nullptr, "|"); - } - payload.opcode = MavlinkFTP::kCmdListDirectory; payload.offset = 0; @@ -203,8 +196,29 @@ bool MavlinkFtpTest::_list_test(void) if (test->success) { ut_compare("Didn't get Ack back", reply->opcode, MavlinkFTP::kRspAck); - ut_compare("Incorrect payload size", reply->size, expected_data_size); - ut_compare("Ack payload contents incorrect", memcmp(reply->data, test->response, expected_data_size), 0); + ut_compare("Incorrect payload size", reply->size, strlen(test->response) + 1); + + // The return order of directories from the List command is not repeatable. So we can't do a direct comparison + // to a hardcoded return result string. + + // Convert null terminators to seperator char so we can use strok to parse returned data + for (uint8_t j=0; jsize-1; j++) { + if (reply->data[j] == 0) { + reply->data[j] = '|'; + } + } + + // Loop over returned directory entries trying to find then in the response list + char *dir; + int response_count = 0; + dir = strtok((char *)&reply->data[0], "|"); + while (dir != nullptr) { + ut_assert("Returned directory not found in expected response", strstr(test->response, dir)); + response_count++; + dir = strtok(nullptr, "|"); + } + + ut_compare("Incorrect number of directory entires returned", test->response_count, response_count); } else { ut_compare("Didn't get Nak back", reply->opcode, MavlinkFTP::kRspNak); ut_compare("Incorrect payload size", reply->size, 2); From deda5d0a04300f2ee67194c4fc9a60da37de722a Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sat, 13 Sep 2014 19:59:44 -0700 Subject: [PATCH 065/138] Upgraded unit test framework --- ROMFS/px4fmu_test/init.d/rcS | 30 +++++ makefiles/config_px4fmu-v2_test.mk | 1 + .../commander_tests/commander_tests.cpp | 4 +- .../state_machine_helper_test.cpp | 13 +- .../state_machine_helper_test.h | 2 +- .../mavlink_tests/mavlink_ftp_test.cpp | 10 +- .../mavlink/mavlink_tests/mavlink_ftp_test.h | 9 +- .../mavlink/mavlink_tests/mavlink_tests.cpp | 7 +- src/modules/mavlink/mavlink_tests/module.mk | 2 + src/modules/unit_test/unit_test.cpp | 24 ++-- src/modules/unit_test/unit_test.h | 125 ++++++++++-------- 11 files changed, 139 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 56482d1407..bc248ac04c 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ] then tests mount fi + +# +# Run unit tests at board boot, reporting failure as needed. +# Add new unit tests using the same pattern as below. +# + +set unit_test_failure 0 + +if mavlink_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} mavlink_tests" +fi + +if commander_tests +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} commander_tests" +fi + +if [ $unit_test_failure == 0 ] +then + echo + echo "All Unit Tests PASSED" +else + echo + echo "Some Unit Tests FAILED:${unit_test_failure_list}" +fi \ No newline at end of file diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index a41670a77b..2f4d9d6a47 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -56,6 +56,7 @@ LIBRARIES += lib/mathlib/CMSIS MODULES += modules/unit_test MODULES += modules/mavlink/mavlink_tests +MODULES += modules/commander/commander_tests # # Transitional support - add commands from the NuttX export archive. diff --git a/src/modules/commander/commander_tests/commander_tests.cpp b/src/modules/commander/commander_tests/commander_tests.cpp index 0abb84a82c..2bfa5d0bb1 100644 --- a/src/modules/commander/commander_tests/commander_tests.cpp +++ b/src/modules/commander/commander_tests/commander_tests.cpp @@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]); int commander_tests_main(int argc, char *argv[]) { - stateMachineHelperTest(); - - return 0; + return stateMachineHelperTest() ? 0 : -1; } diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 08dda2fabf..874090e93e 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -49,7 +49,7 @@ public: StateMachineHelperTest(); virtual ~StateMachineHelperTest(); - virtual void runTests(void); + virtual bool run_tests(void); private: bool armingStateTransitionTest(); @@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void) return true; } -void StateMachineHelperTest::runTests(void) +bool StateMachineHelperTest::run_tests(void) { ut_run_test(armingStateTransitionTest); ut_run_test(mainStateTransitionTest); ut_run_test(isSafeTest); + + return (_tests_failed == 0); } -void stateMachineHelperTest(void) -{ - StateMachineHelperTest* test = new StateMachineHelperTest(); - test->runTests(); - test->printResults(); -} +ut_declare_test(stateMachineHelperTest, StateMachineHelperTest) \ No newline at end of file diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.h b/src/modules/commander/commander_tests/state_machine_helper_test.h index bbf66255ea..cf67190955 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.h +++ b/src/modules/commander/commander_tests/state_machine_helper_test.h @@ -39,6 +39,6 @@ #ifndef STATE_MACHINE_HELPER_TEST_H_ #define STATE_MACHINE_HELPER_TEST_ -void stateMachineHelperTest(void); +bool stateMachineHelperTest(void); #endif /* STATE_MACHINE_HELPER_TEST_H_ */ diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index 022041c74b..a5fb2117ea 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest() } /// @brief Called before every test to initialize the FTP Server. -void MavlinkFtpTest::init(void) +void MavlinkFtpTest::_init(void) { _ftp_server = new MavlinkFTP;; _ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this); @@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void) } /// @brief Called after every test to take down the FTP Server. -void MavlinkFtpTest::cleanup(void) +void MavlinkFtpTest::_cleanup(void) { delete _ftp_server; @@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void) } /// @brief Runs all the unit tests -void MavlinkFtpTest::runTests(void) +bool MavlinkFtpTest::run_tests(void) { ut_run_test(_ack_test); ut_run_test(_bad_opcode_test); @@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void) ut_run_test(_removedirectory_test); ut_run_test(_createdirectory_test); ut_run_test(_removefile_test); + + return (_tests_failed == 0); + } +ut_declare_test(mavlink_ftp_test, MavlinkFtpTest) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h index babd909dac..2696192cc6 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.h @@ -46,10 +46,7 @@ public: MavlinkFtpTest(); virtual ~MavlinkFtpTest(); - virtual void init(void); - virtual void cleanup(void); - - virtual void runTests(void); + virtual bool run_tests(void); static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest); @@ -65,6 +62,9 @@ public: MavlinkFtpTest& operator=(const MavlinkFtpTest&); private: + virtual void _init(void); + virtual void _cleanup(void); + bool _ack_test(void); bool _bad_opcode_test(void); bool _bad_datasize_test(void); @@ -105,3 +105,4 @@ private: static const char _unittest_microsd_file[]; }; +bool mavlink_ftp_test(void); diff --git a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp index d9c1e413a5..10cbcb0ecc 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_tests.cpp @@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]); int mavlink_tests_main(int argc, char *argv[]) { - MavlinkFtpTest* test = new MavlinkFtpTest; - - test->runTests(); - test->printResults(); - - return 0; + return mavlink_ftp_test() ? 0 : -1; } diff --git a/src/modules/mavlink/mavlink_tests/module.mk b/src/modules/mavlink/mavlink_tests/module.mk index 07cfce1dc9..1cc28cce15 100644 --- a/src/modules/mavlink/mavlink_tests/module.mk +++ b/src/modules/mavlink/mavlink_tests/module.mk @@ -45,4 +45,6 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 5000 +MAXOPTIMIZATION = -Os + EXTRACXXFLAGS = -Weffc++ -DMAVLINK_FTP_UNIT_TEST diff --git a/src/modules/unit_test/unit_test.cpp b/src/modules/unit_test/unit_test.cpp index be3b9461a8..b7568ce3a4 100644 --- a/src/modules/unit_test/unit_test.cpp +++ b/src/modules/unit_test/unit_test.cpp @@ -36,7 +36,11 @@ #include -UnitTest::UnitTest() +UnitTest::UnitTest() : + _tests_run(0), + _tests_failed(0), + _tests_passed(0), + _assertions(0) { } @@ -44,20 +48,22 @@ UnitTest::~UnitTest() { } -void UnitTest::printResults(void) +void UnitTest::print_results(void) { - warnx(mu_tests_failed() ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); - warnx(" Tests passed : %d", mu_tests_passed()); - warnx(" Tests failed : %d", mu_tests_failed()); - warnx(" Assertions : %d", mu_assertion()); + warnx(_tests_failed ? "SOME TESTS FAILED" : "ALL TESTS PASSED"); + warnx(" Tests passed : %d", _tests_passed); + warnx(" Tests failed : %d", _tests_failed); + warnx(" Assertions : %d", _assertions); } -void UnitTest::printAssert(const char* msg, const char* test, const char* file, int line) +/// @brief Used internally to the ut_assert macro to print assert failures. +void UnitTest::_print_assert(const char* msg, const char* test, const char* file, int line) { - warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); + warnx("Assertion failed: %s - %s (%s:%d)", msg, test, file, line); } -void UnitTest::printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) +/// @brief Used internally to the ut_compare macro to print assert failures. +void UnitTest::_print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line) { warnx("Compare failed: %s - (%s:%d) (%s:%d) (%s:%d)", msg, v1_text, v1, v2_text, v2, file, line); } diff --git a/src/modules/unit_test/unit_test.h b/src/modules/unit_test/unit_test.h index 1a54897094..8ed4efadf9 100644 --- a/src/modules/unit_test/unit_test.h +++ b/src/modules/unit_test/unit_test.h @@ -37,68 +37,85 @@ #include +/// @brief Base class to be used for unit tests. class __EXPORT UnitTest { - public: -#define INLINE_GLOBAL(type,func) inline type& func() { static type x; return x; } - -INLINE_GLOBAL(int, mu_tests_run) -INLINE_GLOBAL(int, mu_tests_failed) -INLINE_GLOBAL(int, mu_tests_passed) -INLINE_GLOBAL(int, mu_assertion) -INLINE_GLOBAL(int, mu_line) -INLINE_GLOBAL(const char*, mu_last_test) UnitTest(); - virtual ~UnitTest(); + virtual ~UnitTest(); - virtual void init(void) { }; - virtual void cleanup(void) { }; - - virtual void runTests(void) = 0; - void printResults(void); - - void printAssert(const char* msg, const char* test, const char* file, int line); - void printCompare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); - -#define ut_assert(message, test) \ - do { \ - if (!(test)) { \ - printAssert(message, #test, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ - } while (0) - -#define ut_compare(message, v1, v2) \ - do { \ - int _v1 = v1; \ - int _v2 = v2; \ - if (_v1 != _v2) { \ - printCompare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ - return false; \ - } else { \ - mu_assertion()++; \ - } \ + /// @brief Override to run your unit tests. Unit tests should be called using ut_run_test macro. + /// @return true: all unit tests succeeded, false: one or more unit tests failed + virtual bool run_tests(void) = 0; + + /// @brief Prints results from running of unit tests. + void print_results(void); + +/// @brief Macro to create a function which will run a unit test class and print results. +#define ut_declare_test(test_function, test_class) \ + bool test_function(void) \ + { \ + test_class* test = new test_class(); \ + bool success = test->run_tests(); \ + test->print_results(); \ + return success; \ + } + +protected: + +/// @brief Runs a single unit test. Unit tests must have the function signature of bool test(void). The unit +/// test should return true if it succeeded, false for fail. +#define ut_run_test(test) \ + do { \ + warnx("RUNNING TEST: %s", #test); \ + _tests_run++; \ + _init(); \ + if (!test()) { \ + warnx("TEST FAILED: %s", #test); \ + _tests_failed++; \ + } else { \ + warnx("TEST PASSED: %s", #test); \ + _tests_passed++; \ + } \ + _cleanup(); \ } while (0) - -#define ut_run_test(test) \ - do { \ - warnx("RUNNING TEST: %s", #test); \ - mu_tests_run()++; \ - init(); \ - if (!test()) { \ - warnx("TEST FAILED: %s", #test); \ - mu_tests_failed()++; \ - } else { \ - warnx("TEST PASSED: %s", #test); \ - mu_tests_passed()++; \ - } \ - cleanup(); \ - } while (0) + +/// @brief Used to assert a value within a unit test. +#define ut_assert(message, test) \ + do { \ + if (!(test)) { \ + _print_assert(message, #test, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + +/// @brief Used to compare two integer values within a unit test. If possible use ut_compare instead of ut_assert +/// since it will give you better error reporting of the actual values being compared. +#define ut_compare(message, v1, v2) \ + do { \ + int _v1 = v1; \ + int _v2 = v2; \ + if (_v1 != _v2) { \ + _print_compare(message, #v1, _v1, #v2, _v2, __FILE__, __LINE__); \ + return false; \ + } else { \ + _assertions++; \ + } \ + } while (0) + + virtual void _init(void) { }; ///< Run before each unit test. Override to provide custom behavior. + virtual void _cleanup(void) { }; ///< Run after each unit test. Override to provide custom behavior. + + void _print_assert(const char* msg, const char* test, const char* file, int line); + void _print_compare(const char* msg, const char *v1_text, int v1, const char *v2_text, int v2, const char* file, int line); + int _tests_run; ///< The number of individual unit tests run + int _tests_failed; ///< The number of unit tests which failed + int _tests_passed; ///< The number of unit tests which passed + int _assertions; ///< Total number of assertions tested by all unit tests }; #endif /* UNIT_TEST_H_ */ From 50f7e27d13e3dfc4b94f17a06f29d775e47627f9 Mon Sep 17 00:00:00 2001 From: Anthony Kenga Date: Mon, 15 Sep 2014 12:24:11 +0300 Subject: [PATCH 066/138] Fixed parameter storage to support struct parameters. --- src/modules/systemlib/param/param.c | 3 ++- src/modules/systemlib/param/param.h | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index e44e6cdb0e..6b8d0e6341 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -322,7 +322,8 @@ param_get_value_ptr(param_t param) v = ¶m_info_base[param].val; } - if (param_type(param) == PARAM_TYPE_STRUCT) { + if (param_type(param) >= PARAM_TYPE_STRUCT + && param_type(param) <= PARAM_TYPE_STRUCT_MAX) { result = v->p; } else { diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index 084cd931a3..dc73b37e91 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -307,7 +307,7 @@ __EXPORT int param_load_default(void); struct param_info_s __param__##_name = { \ #_name, \ PARAM_TYPE_STRUCT + sizeof(_default), \ - .val.p = &_default; \ + .val.p = &_default \ } /** From 006717734c351926e8daced9805767970c5e65b7 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 15 Sep 2014 21:29:19 +0400 Subject: [PATCH 067/138] FTP: Add skip entry information for proper offset calculation. --- src/modules/mavlink/mavlink_ftp.cpp | 12 ++++++++---- src/modules/mavlink/mavlink_ftp.h | 1 + 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index cf5fae3b3e..2a85c3702a 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -351,16 +351,20 @@ MavlinkFTP::_workList(PayloadHeader* payload) case DTYPE_DIRECTORY: if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) { // Don't bother sending these back - continue; + direntType = kDirentSkip; + } else { + direntType = kDirentDir; } - direntType = kDirentDir; break; default: // We only send back file and diretory entries, skip everything else - continue; + direntType = kDirentSkip; } - if (entry.d_type == DTYPE_FILE) { + if (direntType == kDirentSkip) { + // Skip send only dirent identifier + buf[0] = '\0'; + } else if (direntType == kDirentFile) { // Files send filename and file length snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize); } else { diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index b52561e8ab..657e2f855b 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -159,6 +159,7 @@ private: static const char kDirentFile = 'F'; ///< Identifies File returned from List command static const char kDirentDir = 'D'; ///< Identifies Directory returned from List command + static const char kDirentSkip = 'S'; ///< Identifies Skipped entry from List command /// @brief Maximum data size in RequestHeader::data static const uint8_t kMaxDataLength = MAVLINK_MSG_FILE_TRANSFER_PROTOCOL_FIELD_PAYLOAD_LEN - sizeof(PayloadHeader); From 60c63f48729f028ec9f5b6de93da91873d152bfd Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 10:07:29 +0400 Subject: [PATCH 068/138] FTP: Add new open command for write. All open commands now return file size. --- src/modules/mavlink/mavlink_ftp.cpp | 46 ++++++++++++++--------------- src/modules/mavlink/mavlink_ftp.h | 7 +++-- 2 files changed, 27 insertions(+), 26 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 2a85c3702a..b17036c8ee 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -180,12 +180,16 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workList(payload); break; - case kCmdOpenFile: - errorCode = _workOpen(payload, false); + case kCmdOpenFileRO: + errorCode = _workOpen(payload, O_RDONLY); break; case kCmdCreateFile: - errorCode = _workOpen(payload, true); + errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY); + break; + + case kCmdOpenFileWO: + errorCode = _workOpen(payload, O_CREAT | O_WRONLY); break; case kCmdReadFile: @@ -396,27 +400,27 @@ MavlinkFTP::_workList(PayloadHeader* payload) /// @brief Responds to an Open command MavlinkFTP::ErrorCode -MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) +MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag) { int session_index = _find_unused_session(); if (session_index < 0) { warnx("FTP: Open failed - out of sessions\n"); return kErrNoSessionsAvailable; } - - char *filename = _data_as_cstring(payload); - - uint32_t fileSize = 0; - if (!create) { - struct stat st; - if (stat(filename, &st) != 0) { - return kErrFailErrno; - } - fileSize = st.st_size; - } - int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY; - + char *filename = _data_as_cstring(payload); + + uint32_t fileSize = 0; + struct stat st; + if (stat(filename, &st) != 0) { + // fail only if requested open for read + if (oflag & O_RDONLY) + return kErrFailErrno; + else + st.st_size = 0; + } + fileSize = st.st_size; + int fd = ::open(filename, oflag); if (fd < 0) { return kErrFailErrno; @@ -424,12 +428,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create) _session_fds[session_index] = fd; payload->session = session_index; - if (create) { - payload->size = 0; - } else { - payload->size = sizeof(uint32_t); - *((uint32_t*)payload->data) = fileSize; - } + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = fileSize; return kErrNone; } diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 657e2f855b..1c873fe9f0 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -89,13 +89,14 @@ public: kCmdTerminateSession, ///< Terminates open Read session kCmdResetSessions, ///< Terminates all open Read sessions kCmdListDirectory, ///< List files in from - kCmdOpenFile, ///< Opens file at for reading, returns + kCmdOpenFileRO, ///< Opens file at for reading, returns kCmdReadFile, ///< Reads bytes from in kCmdCreateFile, ///< Creates file at for writing, returns - kCmdWriteFile, ///< Appends bytes to file in + kCmdWriteFile, ///< Writes bytes to in kCmdRemoveFile, ///< Remove file at kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty + kCmdOpenFileWO, ///< Opens file at for writing, returns kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -140,7 +141,7 @@ private: void _reply(Request *req); ErrorCode _workList(PayloadHeader *payload); - ErrorCode _workOpen(PayloadHeader *payload, bool create); + ErrorCode _workOpen(PayloadHeader *payload, int oflag); ErrorCode _workRead(PayloadHeader *payload); ErrorCode _workWrite(PayloadHeader *payload); ErrorCode _workTerminate(PayloadHeader *payload); From 72887e14d9964cf12f8ca0a48d0d185f8d469ecb Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 13:04:21 +0400 Subject: [PATCH 069/138] FTP: Implement write command. --- src/modules/mavlink/mavlink_ftp.cpp | 45 +++++++++++++++++------------ 1 file changed, 26 insertions(+), 19 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index b17036c8ee..695dcb0dcc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -470,29 +470,36 @@ MavlinkFTP::_workRead(PayloadHeader* payload) MavlinkFTP::ErrorCode MavlinkFTP::_workWrite(PayloadHeader* payload) { -#if 0 - // NYI: Coming soon - auto hdr = req->header(); + int session_index = payload->session; - // look up session - auto session = getSession(hdr->session); - if (session == nullptr) { - return kErrNoSession; + if (!_valid_session(session_index)) { + return kErrInvalidSession; } - // append to file - int result = session->append(hdr->offset, &hdr->data[0], hdr->size); - - if (result < 0) { - // XXX might also be no space, I/O, etc. - return kErrNotAppend; - } - - hdr->size = result; - return kErrNone; -#else - return kErrUnknownCommand; + // Seek to the specified position +#ifdef MAVLINK_FTP_DEBUG + warnx("seek %d", payload->offset); #endif + if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) { + // Unable to see to the specified location + warnx("seek fail"); + return kErrFailErrno; + } + + int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size); + if (bytes_written < 0) { + // Negative return indicates error other than eof + warnx("write fail %d", bytes_written); + return kErrFailErrno; + } + + // need to decide how to indicate how much data was written + // current: old code set hdr->size + payload->size = bytes_written; + //payload->size = sizeof(uint32_t); + //*((uint32_t*)payload->data) = bytes_written; + + return kErrNone; } /// @brief Responds to a RemoveFile command From f55d20a133397b2529826f015bec3452294ad4c2 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 18:42:00 +0400 Subject: [PATCH 070/138] FTP: Add truncate command. Unfortunately NuttX not provides truncate(), had to be emulated by copying with O_TRUNC flag. --- src/modules/mavlink/mavlink_ftp.cpp | 126 ++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 3 + 2 files changed, 129 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 695dcb0dcc..6865655da1 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdTruncateFile: + errorCode = _workTruncateFile(payload); + break; + case kCmdCreateDirectory: errorCode = _workCreateDirectory(payload); break; @@ -517,6 +521,82 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload) } } +/// @brief Responds to a TruncateFile command +MavlinkFTP::ErrorCode +MavlinkFTP::_workTruncateFile(PayloadHeader* payload) +{ + char file[kMaxDataLength]; + const char temp_file[] = "/fs/microsd/.trunc.tmp"; + strncpy(file, _data_as_cstring(payload), kMaxDataLength); + payload->size = 0; + + // emulate truncate(file, payload->offset) by + // copying to temp and overwrite with O_TRUNC flag. + + struct stat st; + if (stat(file, &st) != 0) { + return kErrFailErrno; + } + + if (!S_ISREG(st.st_mode)) { + errno = EISDIR; + return kErrFailErrno; + } + + // check perms allow us to write (not romfs) + int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); + if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + errno = EROFS; + return kErrFailErrno; + } + + if (payload->offset == st.st_size) { + // nothing to do + return kErrNone; + } + else if (payload->offset == 0) { + // 1: truncate all data + int fd = ::open(file, O_TRUNC | O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + ::close(fd); + return kErrNone; + } + else if (payload->offset > st.st_size) { + // 2: extend file + int fd = ::open(file, O_WRONLY); + if (fd < 0) { + return kErrFailErrno; + } + + if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + ::close(fd); + return kErrFailErrno; + } + + bool ok = 1 == ::write(fd, "", 1); + ::close(fd); + + return (ok)? kErrNone : kErrFailErrno; + } + else { + // 3: truncate + if (_copy_file(file, temp_file, payload->offset) != 0) { + return kErrFailErrno; + } + if (_copy_file(temp_file, file, payload->offset) != 0) { + return kErrFailErrno; + } + if (::unlink(temp_file) != 0) { + return kErrFailErrno; + } + + return kErrNone; + } +} + /// @brief Responds to a Terminate command MavlinkFTP::ErrorCode MavlinkFTP::_workTerminate(PayloadHeader* payload) @@ -652,3 +732,49 @@ MavlinkFTP::_return_request(Request *req) _unlock_request_queue(); } +/// @brief Copy file (with limited space) +int +MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length) +{ + char buff[512]; + int src_fd = -1, dst_fd = -1; + + src_fd = ::open(src_path, O_RDONLY); + if (src_fd < 0) { + return -1; + } + + dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); + if (dst_fd < 0) { + ::close(src_fd); + return -1; + } + + while (length > 0) { + ssize_t bytes_read, bytes_written; + size_t blen = (length > sizeof(buff))? sizeof(buff) : length; + + bytes_read = ::read(src_fd, buff, blen); + if (bytes_read == 0) { + // EOF + break; + } + else if (bytes_read < 0) { + warnx("cp: read"); + break; + } + + bytes_written = ::write(dst_fd, buff, bytes_read); + if (bytes_written != bytes_read) { + warnx("cp: short write"); + break; + } + + length -= bytes_written; + } + + ::close(src_fd); + ::close(dst_fd); + + return (length > 0)? -1 : 0; +} diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 1c873fe9f0..bde8d7ec57 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -97,6 +97,7 @@ public: kCmdCreateDirectory, ///< Creates directory at kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns + kCmdTruncateFile, ///< Truncate file at to length kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -139,6 +140,7 @@ private: static void _worker_trampoline(void *arg); void _process_request(Request *req); void _reply(Request *req); + int _copy_file(const char *src_path, const char *dst_path, ssize_t length); ErrorCode _workList(PayloadHeader *payload); ErrorCode _workOpen(PayloadHeader *payload, int oflag); @@ -149,6 +151,7 @@ private: ErrorCode _workRemoveDirectory(PayloadHeader *payload); ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); + ErrorCode _workTruncateFile(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From 407a4e0f0675f0b283eea0632208c8c3f8b9ca20 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Wed, 17 Sep 2014 20:02:32 +0400 Subject: [PATCH 071/138] FTP: fix truncate errors. Also correct errno reporting. Seems that warnx() everytime changes originak errno to EINVAL. --- src/modules/mavlink/mavlink_ftp.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 6865655da1..14a42ed724 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -230,6 +230,7 @@ out: warnx("FTP: ack\n"); #endif } else { + int r_errno = errno; warnx("FTP: nak %u", errorCode); payload->req_opcode = payload->opcode; payload->opcode = kRspNak; @@ -237,7 +238,7 @@ out: payload->data[0] = errorCode; if (errorCode == kErrFailErrno) { payload->size = 2; - payload->data[1] = errno; + payload->data[1] = r_errno; } } @@ -544,8 +545,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) } // check perms allow us to write (not romfs) - int mode = st.st_mode & (S_IRWXU | S_IRWXG | S_IRWXO); - if (!(mode & ~(S_IRUSR | S_IXUSR | S_IRGRP | S_IXGRP | S_IROTH | S_IXOTH))) { + if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) { errno = EROFS; return kErrFailErrno; } @@ -571,7 +571,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (lseek(fd, payload->offset - 1, SEEK_SET) != 0) { + if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) { ::close(fd); return kErrFailErrno; } From 772c5f745cdb2213e9d89d2ab3f373ac5ea2009a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 17 Sep 2014 16:37:10 -0500 Subject: [PATCH 072/138] Make space on FMUv1 --- makefiles/config_px4fmu-v1_default.mk | 3 --- 1 file changed, 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 97eddfdd2b..6f39b56a13 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -27,8 +27,6 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott/hott_telemetry -MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl @@ -42,7 +40,6 @@ MODULES += modules/sensors # System commands # MODULES += systemcmds/mtd -MODULES += systemcmds/bl_update MODULES += systemcmds/mixer MODULES += systemcmds/param MODULES += systemcmds/perf From ec39427d79249575b1bd8ca7af0929d08c084060 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 19 Sep 2014 09:51:56 +0200 Subject: [PATCH 073/138] geofence: lat/lon is double This is the same change as 5832948371866aec8f0c7f16b13869f270d36aad but against the master branch. Some time ago the type of lat and lon in the global_pos uorb topic was changed but this use here was missed. --- src/modules/navigator/geofence.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 2662153084..49aec21e01 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -76,11 +76,7 @@ Geofence::~Geofence() bool Geofence::inside(const struct vehicle_global_position_s *vehicle) { - double lat = vehicle->lat / 1e7d; - double lon = vehicle->lon / 1e7d; - //float alt = vehicle->alt; - - return inside(lat, lon, vehicle->alt); + return inside(vehicle->lat, vehicle->lon, vehicle->alt); } bool Geofence::inside(double lat, double lon, float altitude) From 3e41945cc6b43e91d08082b1ce976d5382e0405a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 10:46:46 +0200 Subject: [PATCH 074/138] flare pitch limit: apply at throttle lim altitude --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) 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 c0c238ba9b..77eb78097b 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 @@ -1018,7 +1018,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi math::radians(_parameters.land_flare_pitch_min_deg), math::radians(_parameters.land_flare_pitch_max_deg), 0.0f, throttle_max, throttle_land, - false, math::radians(_parameters.land_flare_pitch_min_deg), + false, land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed, land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index f2e8c422f3..859cef8bdb 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -419,6 +419,7 @@ PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); * Flare, minimum pitch * * Minimum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); @@ -427,6 +428,7 @@ PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f); * Flare, maximum pitch * * Maximum pitch during flare, a positive sign means nose up + * Applied once FW_LND_TLALT is reached * */ PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f); From 980175bb60ba40d29091f92fa8d494aed79b9ef6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: [PATCH 075/138] commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 ++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b143491..03b73447f5 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); From 4b8a3856583d77e3f4c22d0f2999ccd3275e2520 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: [PATCH 076/138] commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c53b143491..b43f08f673 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1449,8 +1449,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From 885d3e8ca4a0f3c5bfad9936860d549b5b2f0674 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 077/138] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From 808cc60cdad5c085d74e583d967cd86126ba1fca Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: [PATCH 078/138] navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 0e7f116ea4..b633945445 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -387,9 +387,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ From d6b669f4971d8d7329f0bfd5ade64bed2bd3120f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:31 +0200 Subject: [PATCH 079/138] commander: improve output on gf violation --- src/modules/commander/commander.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 07fcb5d40e..0ada8e978b 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1442,8 +1442,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of navigator request or geofence"); + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); flight_termination_printed = true; } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "GF violation: flight termination"); + } } // no reset is done here on purpose, on geofence violation we want to stay in flighttermination } From 31a17ce29a76a2f58e6feb92abe1de01eed5a8eb Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 080/138] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From e3cac1999a0b4398e669f90cd28279ec2a0784a7 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:04:11 +0200 Subject: [PATCH 081/138] navigator: geofence with global pos: reduce update rate --- src/modules/navigator/navigator_main.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 9a8c54e7e6..fc9120f137 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -386,9 +386,13 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { + static int gposcounter = 0; + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && + gposcounter % 10 == 0) { + /* Geofence is checked only every 10th gpos update */ have_geofence_position_data = true; } + gposcounter++; } /* Check geofence violation */ From 25b2d4b823d339c69365baecc7b9eada37e0a529 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 10:57:17 +1000 Subject: [PATCH 082/138] change rates for mavlink streams --- src/modules/mavlink/mavlink_main.cpp | 45 +++++++++++++++++----------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index dd830071d9..69565e3ab3 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1374,39 +1374,50 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(2.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 15.0f); - configure_stream("VFR_HUD", 8.0f); + configure_stream("ATTITUDE", 4.0f); + configure_stream("VFR_HUD", 4.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 3.0f); - configure_stream("LOCAL_POSITION_NED", 3.0f); + configure_stream("GLOBAL_POSITION_INT", 1.0f); + configure_stream("LOCAL_POSITION_NED", 1.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); - configure_stream("ATTITUDE_TARGET", 15.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); + configure_stream("ATTITUDE_TARGET", 1.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - configure_stream("OPTICAL_FLOW", 20.0f); + //configure_stream("OPTICAL_FLOW", 20.0f); break; case MAVLINK_MODE_ONBOARD: + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f); - configure_stream("ATTITUDE_TARGET", 50.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 20.0f); + configure_stream("ATTITUDE_TARGET", 10.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f); + configure_stream("VFR_HUD", 10.0f); break; default: From 98bc5ece10987c177be5035cfca6ecea841b8843 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:36 +1000 Subject: [PATCH 083/138] switch mission manager back to what is was before --- src/modules/mavlink/mavlink_main.cpp | 24 ++++++++---------------- 1 file changed, 8 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 69565e3ab3..114b4678b2 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1374,18 +1374,18 @@ Mavlink::task_main(int argc, char *argv[]) _parameters_manager->set_interval(interval_from_rate(30.0f)); LL_APPEND(_streams, _parameters_manager); + /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on + * remote requests rate. Rate specified here controls how much bandwidth we will reserve for + * mission messages. */ + _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); + _mission_manager->set_interval(interval_from_rate(10.0f)); + _mission_manager->set_verbose(_verbose); + LL_APPEND(_streams, _mission_manager); + switch (_mode) { case MAVLINK_MODE_NORMAL: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(2.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); @@ -1402,14 +1402,6 @@ Mavlink::task_main(int argc, char *argv[]) break; case MAVLINK_MODE_ONBOARD: - /* MISSION_STREAM stream, actually sends all MISSION_XXX messages at some rate depending on - * remote requests rate. Rate specified here controls how much bandwidth we will reserve for - * mission messages. */ - _mission_manager = (MavlinkMissionManager *) MavlinkMissionManager::new_instance(this); - _mission_manager->set_interval(interval_from_rate(10.0f)); - _mission_manager->set_verbose(_verbose); - LL_APPEND(_streams, _mission_manager); - configure_stream("SYS_STATUS", 1.0f); // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); From bc23b6239c50527aa550eed3bc6f17dec15c5c97 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 22 Sep 2014 11:03:49 +1000 Subject: [PATCH 084/138] increase ram --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 114b4678b2..b9f486c95e 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2700, + 5000, (main_t)&Mavlink::start_helper, (const char **)argv); From 973c034d6ec502009a8fd92c14ef02c4d1769a64 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 08:59:00 +0200 Subject: [PATCH 085/138] engine failure detection --- src/modules/commander/commander.cpp | 52 ++++++++++++++++++++++-- src/modules/commander/commander_params.c | 32 +++++++++++++++ src/modules/systemlib/circuit_breaker.c | 14 +++++++ src/modules/systemlib/circuit_breaker.h | 1 + 4 files changed, 95 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 0ada8e978b..b9a0bd2cb7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -686,6 +686,9 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); + param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); + param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); + param_t _param_ef_time_thres = param_find("COM_EF_TIME"); /* welcome user */ warnx("starting"); @@ -974,8 +977,16 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; int32_t datalink_regain_timeout = 0; - uint8_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid - is not validated for the datalink loss check */ + int32_t onboard_sysid = 42; /**< systemid of the onboard computer, + telemetry from this sysid is not + validated for the datalink loss check */ + + /* Thresholds for engine failure detection */ + int32_t ef_throttle_thres = 1.0f; + int32_t ef_current2throttle_thres = 0.0f; + int32_t ef_time_thres = 1000.0f; + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine + was healty*/ /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; @@ -1039,6 +1050,9 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); + param_get(_param_ef_throttle_thres, &ef_throttle_thres); + param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); + param_get(_param_ef_time_thres, &ef_time_thres); } orb_check(sp_man_sub, &updated); @@ -1576,10 +1590,11 @@ int commander_thread_main(int argc, char *argv[]) /* If this is not an onboard link/onboard computer: * set flag that we have a valid link */ - if (telemetry_sysid[i] != onboard_sysid) { + if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { have_link = true; } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != onboard_sysid) { + } else if (!telemetry_lost[i] && telemetry_sysid[i] != + (uint8_t)onboard_sysid) { /* telemetry was healthy also in last iteration * we don't have to check a timeout, * telemetry from onboard computers is not accepted as a valid datalink @@ -1612,6 +1627,35 @@ int commander_thread_main(int argc, char *argv[]) } } + /* Check engine failure + * only for fixed wing for now + */ + if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + status.is_rotary_wing == false && + armed.armed && + ((actuator_controls.control[3] > ef_throttle_thres && + battery.current_a/actuator_controls.control[3] < + ef_current2throttle_thres) || + (status.engine_failure))) { + /* potential failure, measure time */ + if (timestamp_engine_healthy > 0 && + hrt_elapsed_time(×tamp_engine_healthy) > + ef_time_thres * 1e6 && + !status.engine_failure) { + status.engine_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "Engine Failure"); + } + } else { + /* no failure reset flag */ + timestamp_engine_healthy = hrt_absolute_time(); + if (status.engine_failure) { + status.engine_failure = false; + status_changed = true; + } + } + + /* handle commands last, as the system needs to be updated to handle them */ orb_check(cmd_sub, &updated); diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 98c0982b21..15c299a8bd 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -138,3 +138,35 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); * @max 255 */ PARAM_DEFINE_INT32(COM_ONBSYSID, 42); + +/** Engine Failure Throttle Threshold + * + * Engine failure triggers only above this throttle value + * + * @group commander + * @min 0.0f + * @max 1.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f); + +/** Engine Failure Current/Throttle Threshold + * + * Engine failure triggers only below this current/throttle value + * + * @group commander + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); + +/** Engine Failure Time Threshold + * + * Engine failure triggers only if the throttle threshold and the + * current to throttle threshold are violated for this time + * + * @group commander + * @unit second + * @min 0.0f + * @max 7.0f + */ +PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index b0f95aedf2..9e5429988b 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -108,6 +108,20 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); */ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +/** + * Circuit breaker for engine failure detection + * + * Setting this parameter to 284953 will disable the engine failure detection. + * If the aircraft is in engine failure mode the enine failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 284953 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 445a89d3a7..6a55e4948d 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -54,6 +54,7 @@ #define CBRK_IO_SAFETY_KEY 22027 #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 +#define CBRK_ENGINEFAIL_KEY 284953 #include From d18f3ee70d5fbeb150c6b37ccafa4f622494ec19 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 5 Sep 2014 12:06:05 +0200 Subject: [PATCH 086/138] make rc loss timeout a param --- src/modules/commander/commander.cpp | 7 +++++-- src/modules/commander/commander_params.c | 11 +++++++++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b9a0bd2cb7..b7a16e4e63 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -128,7 +128,6 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT (2 * 1000 * 1000) /**< consider the local or global position estimate invalid after 600ms */ #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ -#define RC_TIMEOUT 500000 #define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 @@ -684,6 +683,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN"); param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN"); param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); + param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); @@ -976,6 +976,7 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_enabled = false; int32_t datalink_loss_timeout = 10; + float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; int32_t onboard_sysid = 42; /**< systemid of the onboard computer, telemetry from this sysid is not @@ -1048,6 +1049,7 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_enable_parachute, ¶chute_enabled); param_get(_param_enable_datalink_loss, &datalink_loss_enabled); param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); + param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); @@ -1466,7 +1468,8 @@ int commander_thread_main(int argc, char *argv[]) } /* RC input check */ - if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && + hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 15c299a8bd..7d06003c91 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -170,3 +170,14 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @max 7.0f */ PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); + +/** RC loss time threshold + * + * After this amount of seconds without RC connection the rc lost flag is set to true + * + * @group commander + * @unit second + * @min 0 + * @max 35 + */ +PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5); From 21009e89a4c748d8a61174058bb378c1d6306b8d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:29:53 +0200 Subject: [PATCH 087/138] flight termination mavlink outtput: limit rate --- src/modules/commander/commander.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b7a16e4e63..134f23c0b7 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1688,9 +1688,12 @@ int commander_thread_main(int argc, char *argv[]) static bool flight_termination_printed = false; if (!flight_termination_printed) { warnx("Flight termination because of data link loss && gps failure"); + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "DL and GPS lost: flight termination"); + } } /* At this point the rc signal and the gps system have been checked @@ -1709,7 +1712,9 @@ int commander_thread_main(int argc, char *argv[]) warnx("Flight termination because of RC signal loss && gps failure"); flight_termination_printed = true; } - mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 ) { + mavlink_log_critical(mavlink_fd, "RC and GPS lost: flight termination"); + } } } From a8239b2c4516c36d30767c0ae61b30f1e2dde096 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:31:58 +0200 Subject: [PATCH 088/138] if V_RCL_LT < 0 go directly to termination --- src/modules/navigator/rcloss.cpp | 15 +++++++++++---- src/modules/navigator/rcloss_params.c | 3 ++- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp index 651e311840..5564a1c42e 100644 --- a/src/modules/navigator/rcloss.cpp +++ b/src/modules/navigator/rcloss.cpp @@ -153,10 +153,17 @@ RCLoss::advance_rcl() { switch (_rcl_state) { case RCL_STATE_NONE: - /* Check the number of data link losses. If above home fly home directly */ - warnx("RC loss, OBC mode, loiter"); - mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); - _rcl_state = RCL_STATE_LOITER; + if (_param_loitertime.get() > 0.0f) { + warnx("RC loss, OBC mode, loiter"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering"); + _rcl_state = RCL_STATE_LOITER; + } else { + warnx("RC loss, OBC mode, slip loiter, terminate"); + mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating"); + _rcl_state = RCL_STATE_TERMINATE; + _navigator->get_mission_result()->stay_in_failsafe = true; + _navigator->publish_mission_result(); + } break; case RCL_STATE_LOITER: _rcl_state = RCL_STATE_TERMINATE; diff --git a/src/modules/navigator/rcloss_params.c b/src/modules/navigator/rcloss_params.c index 83d23cf499..f1a01c73b5 100644 --- a/src/modules/navigator/rcloss_params.c +++ b/src/modules/navigator/rcloss_params.c @@ -51,9 +51,10 @@ * Loiter Time * * The amount of time in seconds the system should loiter at current position before termination + * Set to -1 to make the system skip loitering * * @unit seconds - * @min 0.0 + * @min -1.0 * @group RCL */ PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f); From 1fb8e76f0a4508cadd63b47e6f94a6638b699bcc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:32:28 +0200 Subject: [PATCH 089/138] fix typo in comment --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 ee08f3e981..f44985d505 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 @@ -1218,7 +1218,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } if (_vehicle_status.engine_failure || _vehicle_status.engine_failure_cmd) { - /* Set thrrust to 0 to minimize damage */ + /* Set thrust to 0 to minimize damage */ _att_sp.thrust = 0.0f; } else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF && launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) { From e17411769847f8681dc05da72ed4a06ff27a7a32 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 7 Sep 2014 15:33:11 +0200 Subject: [PATCH 090/138] gps failure has priority over engine falure, in case both fail make sure that the gps failure mode does not turn on the engine --- src/modules/commander/state_machine_helper.cpp | 4 ++-- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++++-- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index e3b5d30e49..9568752aed 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -511,10 +511,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en } else if (status->rc_signal_lost_cmd) { status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX /* Finished handling commands which have priority , now handle failures */ - } else if (status->engine_failure) { - status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (status->gps_failure) { status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL; + } else if (status->engine_failure) { + status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL; } else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) || (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) { status->failsafe = true; diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 517333c80a..7cf721a356 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -860,8 +860,12 @@ FixedwingAttitudeControl::task_main() } } - /* throttle passed through */ - _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; + /* throttle passed through if it is finite and if no engine failure was + * detected */ + _actuators.control[3] = (isfinite(throttle_sp) && + !(_vehicle_status.engine_failure || + _vehicle_status.engine_failure_cmd)) ? + throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { if (_debug && loop_counter % 10 == 0) { warnx("throttle_sp %.4f", (double)throttle_sp); From b5ffcfe3d12a0e2e98cb5319e0286c8215150672 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 13 Sep 2014 16:23:28 +0200 Subject: [PATCH 091/138] Revert "datalink check: ignore onboard computer" This reverts commit 3f8793210b47bd8e09ed2adaabc2fab966db5df6. Conflicts: src/modules/commander/commander.cpp src/modules/commander/commander_params.c --- src/modules/commander/commander.cpp | 23 +++-------------------- src/modules/commander/commander_params.c | 10 ---------- 2 files changed, 3 insertions(+), 30 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 134f23c0b7..b43f08f673 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -685,7 +685,6 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T"); param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T"); param_t _param_datalink_regain_timeout = param_find("COM_DL_REG_T"); - param_t _param_onboard_sysid = param_find("COM_ONBSYSID"); param_t _param_ef_throttle_thres = param_find("COM_EF_THROT"); param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T"); param_t _param_ef_time_thres = param_find("COM_EF_TIME"); @@ -883,14 +882,12 @@ int commander_thread_main(int argc, char *argv[]) uint64_t telemetry_last_heartbeat[TELEMETRY_STATUS_ORB_ID_NUM]; uint64_t telemetry_last_dl_loss[TELEMETRY_STATUS_ORB_ID_NUM]; bool telemetry_lost[TELEMETRY_STATUS_ORB_ID_NUM]; - uint8_t telemetry_sysid[TELEMETRY_STATUS_ORB_ID_NUM]; for (int i = 0; i < TELEMETRY_STATUS_ORB_ID_NUM; i++) { telemetry_subs[i] = orb_subscribe(telemetry_status_orb_id[i]); telemetry_last_heartbeat[i] = 0; telemetry_last_dl_loss[i] = 0; telemetry_lost[i] = true; - telemetry_sysid[i] = 0; } /* Subscribe to global position */ @@ -978,9 +975,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t datalink_loss_timeout = 10; float rc_loss_timeout = 0.5; int32_t datalink_regain_timeout = 0; - int32_t onboard_sysid = 42; /**< systemid of the onboard computer, - telemetry from this sysid is not - validated for the datalink loss check */ /* Thresholds for engine failure detection */ int32_t ef_throttle_thres = 1.0f; @@ -988,7 +982,6 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_time_thres = 1000.0f; uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty*/ - /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1051,7 +1044,6 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_datalink_loss_timeout, &datalink_loss_timeout); param_get(_param_rc_loss_timeout, &rc_loss_timeout); param_get(_param_datalink_regain_timeout, &datalink_regain_timeout); - param_get(_param_onboard_sysid, &onboard_sysid); param_get(_param_ef_throttle_thres, &ef_throttle_thres); param_get(_param_ef_current2throttle_thres, &ef_current2throttle_thres); param_get(_param_ef_time_thres, &ef_time_thres); @@ -1101,7 +1093,6 @@ int commander_thread_main(int argc, char *argv[]) } telemetry_last_heartbeat[i] = telemetry.heartbeat_time; - telemetry_sysid[i] = telemetry.system_id; } } @@ -1590,18 +1581,10 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "data link %i regained", i); telemetry_lost[i] = false; - - /* If this is not an onboard link/onboard computer: - * set flag that we have a valid link */ - if (telemetry_sysid[i] != (uint8_t)onboard_sysid) { - have_link = true; - } - } else if (!telemetry_lost[i] && telemetry_sysid[i] != - (uint8_t)onboard_sysid) { + have_link = true; + } else if (!telemetry_lost[i]) { /* telemetry was healthy also in last iteration - * we don't have to check a timeout, - * telemetry from onboard computers is not accepted as a valid datalink - */ + * we don't have to check a timeout */ have_link = true; } diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 7d06003c91..f4bc7fd7fb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -129,16 +129,6 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10); */ PARAM_DEFINE_INT32(COM_DL_REG_T, 0); -/** Onboard computer system id - * - * The system id of the onboard computer. Heartbeats from this system are ignored during the datalink check - * - * @group commander - * @min 0 - * @max 255 - */ -PARAM_DEFINE_INT32(COM_ONBSYSID, 42); - /** Engine Failure Throttle Threshold * * Engine failure triggers only above this throttle value From c7966d56f5457be2eab4d6ee9cfadb7c3f22674b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 08:44:30 +0200 Subject: [PATCH 092/138] geofence: better usefeedback if loaded --- src/modules/navigator/geofence.cpp | 6 +++++- src/modules/navigator/geofence.h | 4 ++++ src/modules/navigator/navigator_main.cpp | 2 ++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/geofence.cpp b/src/modules/navigator/geofence.cpp index 5504239c5e..0f431ded26 100644 --- a/src/modules/navigator/geofence.cpp +++ b/src/modules/navigator/geofence.cpp @@ -48,6 +48,7 @@ #include #include #include +#include /* Oddly, ERROR is not defined for C++ */ @@ -66,7 +67,8 @@ Geofence::Geofence() : _param_altitude_mode(this, "ALTMODE"), _param_source(this, "SOURCE"), _param_counter_threshold(this, "COUNT"), - _outside_counter(0) + _outside_counter(0), + _mavlinkFd(-1) { /* Load initial params */ updateParams(); @@ -330,8 +332,10 @@ Geofence::loadFromFile(const char *filename) { _verticesCount = pointCounter; warnx("Geofence: imported successfully"); + mavlink_log_info(_mavlinkFd, "Geofence imported"); } else { warnx("Geofence: import error"); + mavlink_log_critical(_mavlinkFd, "#audio: Geofence import error"); } return ERROR; diff --git a/src/modules/navigator/geofence.h b/src/modules/navigator/geofence.h index 65e5b4042b..9d647cb68a 100644 --- a/src/modules/navigator/geofence.h +++ b/src/modules/navigator/geofence.h @@ -98,6 +98,8 @@ public: int getSource() { return _param_source.get(); } + void setMavlinkFd(int value) { _mavlinkFd = value; } + private: orb_advert_t _fence_pub; /**< publish fence topic */ @@ -114,6 +116,8 @@ private: uint8_t _outside_counter; + int _mavlinkFd; + bool inside(double lat, double lon, float altitude); bool inside(const struct vehicle_global_position_s &global_position); bool inside(const struct vehicle_global_position_s &global_position, float baro_altitude_amsl); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fc9120f137..4f92954fd9 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -252,6 +252,7 @@ Navigator::task_main() warnx("Initializing.."); _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + _geofence.setMavlinkFd(_mavlink_fd); /* Try to load the geofence: * if /fs/microsd/etc/geofence.txt load from this file @@ -263,6 +264,7 @@ Navigator::task_main() _geofence.loadFromFile(GEOFENCE_FILENAME); } else { + mavlink_log_critical(_mavlink_fd, "#audio: No geofence file"); if (_geofence.clearDm() > 0) warnx("Geofence cleared"); else From 5a0e0d041229700d2ae9c14e4ab2798d34fff14c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 20 Sep 2014 09:45:22 +0200 Subject: [PATCH 093/138] navigator: fix status information, remove fence_valid flag (this is handled by the geofence class) --- src/modules/navigator/navigator.h | 1 - src/modules/navigator/navigator_main.cpp | 5 ++--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index 840b43f1b8..bf42acff98 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -191,7 +191,6 @@ private: Geofence _geofence; /**< class that handles the geofence */ bool _geofence_violation_warning_sent; /**< prevents spaming to mavlink */ - bool _fence_valid; /**< flag if fence is valid */ bool _inside_fence; /**< vehicle is inside fence */ NavigatorMode *_navigation_mode; /**< abstract pointer to current navigation mode class */ diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 4f92954fd9..b633945445 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -127,7 +127,6 @@ Navigator::Navigator() : _loop_perf(perf_alloc(PC_ELAPSED, "navigator")), _geofence{}, _geofence_violation_warning_sent(false), - _fence_valid(false), _inside_fence(true), _navigation_mode(nullptr), _mission(this, "MIS"), @@ -536,7 +535,7 @@ Navigator::status() // warnx("Compass heading in degrees %5.5f", (double)(_global_pos.yaw * M_RAD_TO_DEG_F)); // } - if (_fence_valid) { + if (_geofence.valid()) { warnx("Geofence is valid"); /* TODO: needed? */ // warnx("Vertex longitude latitude"); @@ -544,7 +543,7 @@ Navigator::status() // warnx("%6u %9.5f %8.5f", i, (double)_fence.vertices[i].lon, (double)_fence.vertices[i].lat); } else { - warnx("Geofence not set"); + warnx("Geofence not set (no /etc/geofence.txt on microsd) or not valid"); } } From d113fcfc54c246f3d5ac22ad5485f7103aecab41 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 13:32:47 +0200 Subject: [PATCH 094/138] commander: move position of gps failure check --- src/modules/commander/commander.cpp | 35 ++++++++++++++--------------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b43f08f673..bf15bbeb60 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1205,24 +1205,6 @@ int commander_thread_main(int argc, char *argv[]) check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed); - /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { - /* handle the case where gps was regained */ - if (status.gps_failure) { - status.gps_failure = false; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps regained"); - } - } else { - if (!status.gps_failure) { - status.gps_failure = true; - status_changed = true; - mavlink_log_critical(mavlink_fd, "gps fix lost"); - } - } - - /* update home position */ if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && (global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) { @@ -1434,6 +1416,23 @@ int commander_thread_main(int argc, char *argv[]) globallocalconverter_init((double)gps_position.lat * 1.0e-7, (double)gps_position.lon * 1.0e-7, (float)gps_position.alt * 1.0e-3f, hrt_absolute_time()); } + /* check if GPS fix is ok */ + if (gps_position.fix_type >= 3 && //XXX check eph and epv ? + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + /* handle the case where gps was regained */ + if (status.gps_failure) { + status.gps_failure = false; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps regained"); + } + } else { + if (!status.gps_failure) { + status.gps_failure = true; + status_changed = true; + mavlink_log_critical(mavlink_fd, "gps fix lost"); + } + } + /* start mission result check */ orb_check(mission_result_sub, &updated); From 8eb310061693c86a9c8b3c7f6e2bab1ec680c46a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 13:48:43 +0400 Subject: [PATCH 095/138] FTP: Rename command. Payload: `\0` See: man 2 rename --- src/modules/mavlink/mavlink_ftp.cpp | 31 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 33 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 14a42ed724..117cc4ec16 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -204,6 +204,10 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveFile(payload); break; + case kCmdRename: + errorCode = _workRename(payload); + break; + case kCmdTruncateFile: errorCode = _workTruncateFile(payload); break; @@ -629,6 +633,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload) return kErrNone; } +/// @brief Responds to a Rename command +MavlinkFTP::ErrorCode +MavlinkFTP::_workRename(PayloadHeader* payload) +{ + char oldpath[kMaxDataLength]; + char newpath[kMaxDataLength]; + + char *ptr = _data_as_cstring(payload); + size_t oldpath_sz = strlen(ptr); + + if (oldpath_sz == payload->size) { + // no newpath + errno = EINVAL; + return kErrFailErrno; + } + + strncpy(oldpath, ptr, kMaxDataLength); + strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength); + + if (rename(oldpath, newpath) == 0) { + payload->size = 0; + return kErrNone; + } else { + return kErrFailErrno; + } +} + /// @brief Responds to a RemoveDirectory command MavlinkFTP::ErrorCode MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index bde8d7ec57..0fbd010be2 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -98,6 +98,7 @@ public: kCmdRemoveDirectory, ///< Removes Directory at , must be empty kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length + kCmdRename, ///< Rename to kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -152,6 +153,7 @@ private: ErrorCode _workCreateDirectory(PayloadHeader *payload); ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); + ErrorCode _workRename(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From ef5a93c09ce4bd08729cf3fbf87a6c1d453c646f Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Mon, 22 Sep 2014 15:19:38 +0400 Subject: [PATCH 096/138] FTP: Add file checksum calculation command. --- src/modules/mavlink/mavlink_ftp.cpp | 39 +++++++++++++++++++++++++++++ src/modules/mavlink/mavlink_ftp.h | 2 ++ 2 files changed, 41 insertions(+) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 117cc4ec16..c26fe6b4bc 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -34,6 +34,7 @@ /// @file mavlink_ftp.cpp /// @author px4dev, Don Gagne +#include #include #include #include @@ -220,6 +221,11 @@ MavlinkFTP::_process_request(Request *req) errorCode = _workRemoveDirectory(payload); break; + + case kCmdCalcFileCRC32: + errorCode = _workCalcFileCRC32(payload); + break; + default: errorCode = kErrUnknownCommand; break; @@ -690,6 +696,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload) } } +/// @brief Responds to a CalcFileCRC32 command +MavlinkFTP::ErrorCode +MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload) +{ + char file_buf[256]; + uint32_t checksum = 0; + ssize_t bytes_read; + strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength); + + int fd = ::open(file_buf, O_RDONLY); + if (fd < 0) { + return kErrFailErrno; + } + + do { + bytes_read = ::read(fd, file_buf, sizeof(file_buf)); + if (bytes_read < 0) { + int r_errno = errno; + ::close(fd); + errno = r_errno; + return kErrFailErrno; + } + + checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum); + } while (bytes_read == sizeof(file_buf)); + + ::close(fd); + + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = checksum; + return kErrNone; +} + /// @brief Returns true if the specified session is a valid open session bool MavlinkFTP::_valid_session(unsigned index) diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h index 0fbd010be2..bef6775a94 100644 --- a/src/modules/mavlink/mavlink_ftp.h +++ b/src/modules/mavlink/mavlink_ftp.h @@ -99,6 +99,7 @@ public: kCmdOpenFileWO, ///< Opens file at for writing, returns kCmdTruncateFile, ///< Truncate file at to length kCmdRename, ///< Rename to + kCmdCalcFileCRC32, ///< Calculate CRC32 for file at kRspAck = 128, ///< Ack response kRspNak ///< Nak response @@ -154,6 +155,7 @@ private: ErrorCode _workRemoveFile(PayloadHeader *payload); ErrorCode _workTruncateFile(PayloadHeader *payload); ErrorCode _workRename(PayloadHeader *payload); + ErrorCode _workCalcFileCRC32(PayloadHeader *payload); static const unsigned kRequestQueueSize = 2; ///< Max number of queued requests Request _request_bufs[kRequestQueueSize]; ///< Request buffers which hold work From 73aa7d81e31e18a6d1405697856e389a7a7b89af Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: [PATCH 097/138] mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 937e4fa5ad..389cdd0d2d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() From ec09f080083fc297228857397eef0651085887d9 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 17:20:50 +0200 Subject: [PATCH 098/138] mf checker: fix landing check, ensure feedback from all checks is sent --- .../navigator/mission_feasibility_checker.cpp | 20 ++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 606521f200..7ae7f60cb2 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -84,7 +84,12 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) @@ -93,7 +98,13 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); + /* Perform checks and issue feedback to the user for all checks */ + bool resLanding = checkFixedWingLanding(dm_current, nMissionItems); + bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence); + bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt); + + /* Mission is only marked as feasible if all checks return true */ + return (resLanding && resGeofence && resHomeAltitude); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -216,9 +227,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size } } - -// float slope_alt = wp_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_distance)/_flare_constant) - _H1_virt; - return false; + /* No landing waypoints or no waypoints */ + return true; } void MissionFeasibilityChecker::updateNavigationCapabilities() From 69eb222d4e2cf3d0894992a50a7a5822489f757f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 19:04:24 +0200 Subject: [PATCH 099/138] ubx: add sbas config, default to disable --- src/drivers/gps/ubx.cpp | 12 ++++++++++++ src/drivers/gps/ubx.h | 17 +++++++++++++++++ 2 files changed, 29 insertions(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index d0854f5e9a..c54c474b3d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,16 @@ UBX::configure(unsigned &baudrate) return 1; } + /* send a SBAS message to set the SBAS options */ + memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); + _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; + + send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas)); + + if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { + return 1; + } + /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ @@ -265,6 +275,8 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } + warnx("msg %u ret %d", msg, ret); + _ack_state = UBX_ACK_IDLE; return ret; } diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 219a5762aa..c74eb9168f 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -73,6 +73,7 @@ #define UBX_ID_CFG_MSG 0x01 #define UBX_ID_CFG_RATE 0x08 #define UBX_ID_CFG_NAV5 0x24 +#define UBX_ID_CFG_SBAS 0x16 #define UBX_ID_MON_VER 0x04 #define UBX_ID_MON_HW 0x09 @@ -89,6 +90,7 @@ #define UBX_MSG_CFG_MSG ((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8) #define UBX_MSG_CFG_RATE ((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8) #define UBX_MSG_CFG_NAV5 ((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8) +#define UBX_MSG_CFG_SBAS ((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8) #define UBX_MSG_MON_HW ((UBX_CLASS_MON) | UBX_ID_MON_HW << 8) #define UBX_MSG_MON_VER ((UBX_CLASS_MON) | UBX_ID_MON_VER << 8) @@ -128,6 +130,11 @@ #define UBX_TX_CFG_NAV5_DYNMODEL 7 /**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */ #define UBX_TX_CFG_NAV5_FIXMODE 2 /**< 1 2D only, 2 3D only, 3 Auto 2D/3D */ +/* TX CFG-SBAS message contents */ +#define UBX_TX_CFG_SBAS_MODE_ENABLED 1 /**< SBAS enabled */ +#define UBX_TX_CFG_SBAS_MODE_DISABLED 0 /**< SBAS disabled */ +#define UBX_TX_CFG_SBAS_MODE UBX_TX_CFG_SBAS_MODE_DISABLED /**< SBAS enabled or disabled */ + /* TX CFG-MSG message contents */ #define UBX_TX_CFG_MSG_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_TX_CFG_MSG_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ @@ -383,6 +390,15 @@ typedef struct { uint32_t reserved4; } ubx_payload_tx_cfg_nav5_t; +/* tx cfg-sbas */ +typedef struct { + uint8_t mode; + uint8_t usage; + uint8_t maxSBAS; + uint8_t scanmode2; + uint32_t scanmode1; +} ubx_payload_tx_cfg_sbas_t; + /* Tx CFG-MSG */ typedef struct { union { @@ -413,6 +429,7 @@ typedef union { ubx_payload_tx_cfg_prt_t payload_tx_cfg_prt; ubx_payload_tx_cfg_rate_t payload_tx_cfg_rate; ubx_payload_tx_cfg_nav5_t payload_tx_cfg_nav5; + ubx_payload_tx_cfg_sbas_t payload_tx_cfg_sbas; ubx_payload_tx_cfg_msg_t payload_tx_cfg_msg; uint8_t raw[]; } ubx_buf_t; From d9bb3a51124cb3006f7548ac6ee629b48a939336 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 23 Sep 2014 06:08:24 +1000 Subject: [PATCH 100/138] viper: removed wrong failsafe values --- ROMFS/px4fmu_common/init.d/3035_viper | 2 -- 1 file changed, 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper index ac4d919ce7..f3b0e84183 100644 --- a/ROMFS/px4fmu_common/init.d/3035_viper +++ b/ROMFS/px4fmu_common/init.d/3035_viper @@ -9,5 +9,3 @@ sh /etc/init.d/rc.fw_defaults set MIXER Viper -set FAILSAFE "-c56 -p 1000" -set FAILSAFE "-c7 -p 1200" From 32131a069eeb77637a67e10011b4b756bf4d718a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 09:44:38 +0200 Subject: [PATCH 101/138] inform in GCS when switching to laser --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) 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 eb4ffdabb3..ddfbc233b0 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 @@ -814,7 +814,10 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ if (global_pos.terrain_alt_valid || land_useterrain) { - land_useterrain = true; + if(!land_useterrain) { + mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); + land_useterrain = true; + } return global_pos.terrain_alt; } else { return land_setpoint_alt; From 852159d9edaa85c3abee3059359264f3329841e8 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 22 Sep 2014 10:03:23 +0200 Subject: [PATCH 102/138] fw pos control: add param to enable/disable usage of terrain estimate during landing --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 6 +++++- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 8 +++----- 2 files changed, 8 insertions(+), 6 deletions(-) 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 ddfbc233b0..a94311ccb3 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 @@ -240,6 +240,7 @@ private: float land_heading_hold_horizontal_distance; float land_flare_pitch_min_deg; float land_flare_pitch_max_deg; + int land_use_terrain_estimate; } _parameters; /**< local copies of interesting parameters */ @@ -287,6 +288,7 @@ private: param_t land_heading_hold_horizontal_distance; param_t land_flare_pitch_min_deg; param_t land_flare_pitch_max_deg; + param_t land_use_terrain_estimate; } _parameter_handles; /**< handles for interesting parameters */ @@ -474,6 +476,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); _parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN"); _parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX"); + _parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.time_const_throt = param_find("FW_T_THRO_CONST"); @@ -576,6 +579,7 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg)); param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg)); + param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate)); _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); @@ -813,7 +817,7 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint /* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it * for the whole landing */ - if (global_pos.terrain_alt_valid || land_useterrain) { + if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) { if(!land_useterrain) { mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate"); land_useterrain = true; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 41a3665381..c00d822327 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -412,15 +412,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Relative altitude threshold for range finder measurements for use during landing + * Enable or disable usage of terrain estimate during landing * - * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT - * set to < 0 to disable - * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location + * 0: disabled, 1: enabled * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); +PARAM_DEFINE_INT32(FW_LND_USETER, 0); /** * Flare, minimum pitch From 8a18cfa3869555389e7e9ff8f104d83f9c54cb43 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 21 Sep 2014 18:03:52 +0200 Subject: [PATCH 103/138] datman: reduce task priority --- src/modules/dataman/dataman.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c index ca1fe9bbbd..b2355d4d89 100644 --- a/src/modules/dataman/dataman.c +++ b/src/modules/dataman/dataman.c @@ -797,7 +797,7 @@ start(void) sem_init(&g_init_sema, 1, 0); /* start the worker thread */ - if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) { + if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, task_main, NULL)) <= 0) { warn("task start failed"); return -1; } From cf4604f5c3614bc5cd1b82dfee693cbac36181ab Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:34:44 +0400 Subject: [PATCH 104/138] navigator/mission.cpp: indentation fixed --- src/modules/navigator/mission.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 128dab7d77..719301a0ac 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -417,13 +417,13 @@ Mission::set_mission_items() if (_navigator->get_global_position()->alt < takeoff_alt - _navigator->get_acceptance_radius()) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; - _mission_item.lat = _navigator->get_global_position()->lat; - _mission_item.lon = _navigator->get_global_position()->lon; - _mission_item.altitude = takeoff_alt; - _mission_item.altitude_is_relative = false; - _mission_item.autocontinue = true; - _mission_item.time_inside = 0; + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = takeoff_alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); From 2d81c2cf466596b7c4f787a52836fd200af2a097 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Sep 2014 13:43:42 +0400 Subject: [PATCH 105/138] mc_pos_control: commented code block removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ---------- 1 file changed, 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 ce26f7dc25..99deb0d29a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1022,16 +1022,6 @@ MulticopterPositionControl::task_main() _vel_sp(2) = _params.land_speed; } - -// if (!_control_mode.flag_control_manual_enabled) { -// /* limit 3D speed only in non-manual modes */ -// float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length(); -// -// if (vel_sp_norm > 1.0f) { -// _vel_sp /= vel_sp_norm; -// } -// } - _global_vel_sp.vx = _vel_sp(0); _global_vel_sp.vy = _vel_sp(1); _global_vel_sp.vz = _vel_sp(2); From 964fddb387b8e635d2cd13fb2248643791304489 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:08:25 +0200 Subject: [PATCH 106/138] make geofence update rate independent from positon update rate --- src/modules/navigator/navigator_main.cpp | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index b633945445..fad46998e1 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -87,6 +87,7 @@ */ extern "C" __EXPORT int navigator_main(int argc, char *argv[]); +#define GEOFENCE_CHECK_INTERVAL 200000 namespace navigator { @@ -343,7 +344,7 @@ Navigator::task_main() _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); } - bool have_geofence_position_data = false; + static bool have_geofence_position_data = false; /* gps updated */ if (fds[7].revents & POLLIN) { @@ -388,17 +389,18 @@ Navigator::task_main() if (fds[0].revents & POLLIN) { global_position_update(); static int gposcounter = 0; - if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS && - gposcounter % 10 == 0) { - /* Geofence is checked only every 10th gpos update */ + if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } gposcounter++; } /* Check geofence violation */ - if (have_geofence_position_data) { + static hrt_abstime last_geofence_check = 0; + if (have_geofence_position_data && hrt_elapsed_time(&last_geofence_check) > GEOFENCE_CHECK_INTERVAL) { bool inside = _geofence.inside(_global_pos, _gps_pos, _sensor_combined.baro_alt_meter); + last_geofence_check = hrt_absolute_time(); + have_geofence_position_data = false; if (!inside) { /* inform other apps via the mission result */ _mission_result.geofence_violated = true; From 038e1cac03198259d6f7630c6bb7c65c35f44fae Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:17 +0200 Subject: [PATCH 107/138] increase default engine failure threshold --- src/modules/commander/commander_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index f4bc7fd7fb..1b0c4258b2 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -159,7 +159,7 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f); * @min 0.0f * @max 7.0f */ -PARAM_DEFINE_FLOAT(COM_EF_TIME, 5.0f); +PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f); /** RC loss time threshold * From ab400089bc2a42f1f0ace569d8f0ee58f4338e1d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 28 Sep 2014 16:17:40 +0200 Subject: [PATCH 108/138] disable flight termination as default for now --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 9e5429988b..64420be379 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -106,7 +106,7 @@ PARAM_DEFINE_INT32(CBRK_AIRSPD_CHK, 0); * @max 121212 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 0); +PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); /** * Circuit breaker for engine failure detection From 70e5d4027a3b1465d5128dbf9a04cbb6545e043d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 30 Sep 2014 09:08:31 +0400 Subject: [PATCH 109/138] navigator: autocontinue fix --- src/modules/navigator/mission.cpp | 26 +++++++++++++++++--------- 1 file changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 1040651320..8bacaa4254 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,8 +150,19 @@ Mission::on_active() /* lets check if we reached the current mission item */ if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { - advance_mission(); - set_mission_items(); + if (_mission_item.autocontinue) { + /* switch to next waypoint if 'autocontinue' flag set */ + advance_mission(); + set_mission_items(); + + } else { + /* else just report that item reached */ + if (_mission_type == MISSION_TYPE_OFFBOARD) { + if (!(_mission_result.seq_reached == _current_offboard_mission_index && _mission_result.reached)) { + report_mission_item_reached(); + } + } + } } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); @@ -694,10 +705,8 @@ Mission::save_offboard_mission_state() void Mission::report_mission_item_reached() { - if (_mission_type == MISSION_TYPE_OFFBOARD) { - _mission_result.reached = true; - _mission_result.seq_reached = _current_offboard_mission_index; - } + _mission_result.reached = true; + _mission_result.seq_reached = _current_offboard_mission_index; publish_mission_result(); } @@ -705,6 +714,8 @@ void Mission::report_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); + _mission_result.reached = false; + _mission_result.finished = false; _mission_result.seq_current = _current_offboard_mission_index; publish_mission_result(); @@ -730,7 +741,4 @@ Mission::publish_mission_result() /* advertise and publish */ _mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result); } - /* reset reached bool */ - _mission_result.reached = false; - _mission_result.finished = false; } From 3e1eec5906ea8df955bf6d0ac7ec182979f47c90 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 10 Sep 2014 14:42:32 +0900 Subject: [PATCH 110/138] mpu6k: set hardware filter during ACCELIOCLOWPASS also set from GYROIOCLOWPASS Conflicts: mavlink/include/mavlink/v1.0 --- src/drivers/mpu6000/mpu6000.cpp | 15 +++++---------- 1 file changed, 5 insertions(+), 10 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 6f5dae7adb..fb38c7e67b 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -968,11 +968,9 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) return _accel_filter_x.get_cutoff_freq(); case ACCELIOCSLOWPASS: - if (arg == 0) { - // allow disabling of on-chip filter using - // zero as desired filter frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); + // set software filtering _accel_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _accel_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); @@ -1056,11 +1054,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - if (arg == 0) { - // allow disabling of on-chip filter using 0 - // as desired frequency - _set_dlpf_filter(0); - } + // set hardware filtering + _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 8ced6bb49bf32128d2673e565cb112ca6d2f21eb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 10:05:39 +0200 Subject: [PATCH 111/138] Set filter frequency for hardware and software in parallel, always do so in the same order --- src/drivers/mpu6000/mpu6000.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index fb38c7e67b..b22bb2e070 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -910,12 +910,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) // adjust filters float cutoff_freq_hz = _accel_filter_x.get_cutoff_freq(); float sample_rate = 1.0e6f/ticks; + _set_dlpf_filter(cutoff_freq_hz); _accel_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz); _accel_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz); float cutoff_freq_hz_gyro = _gyro_filter_x.get_cutoff_freq(); + _set_dlpf_filter(cutoff_freq_hz_gyro); _gyro_filter_x.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_y.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); _gyro_filter_z.set_cutoff_frequency(sample_rate, cutoff_freq_hz_gyro); @@ -1051,11 +1053,11 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg) case GYROIOCGLOWPASS: return _gyro_filter_x.get_cutoff_freq(); case GYROIOCSLOWPASS: + // set hardware filtering + _set_dlpf_filter(arg); _gyro_filter_x.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_y.set_cutoff_frequency(1.0e6f / _call_interval, arg); _gyro_filter_z.set_cutoff_frequency(1.0e6f / _call_interval, arg); - // set hardware filtering - _set_dlpf_filter(arg); return OK; case GYROIOCSSCALE: From 70606d400bdceaa86331670938c1e0ae988ed97c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:30 +0200 Subject: [PATCH 112/138] remove wrong comments --- src/modules/navigator/navigator_main.cpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e1..bf5e36d39e 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -432,9 +432,6 @@ Navigator::task_main() _can_loiter_at_sp = false; break; case NAVIGATION_STATE_AUTO_MISSION: - /* Some failsafe modes prohibit the fallback to mission - * usually this is done after some time to make sure - * that the full failsafe operation is performed */ _navigation_mode = &_mission; break; case NAVIGATION_STATE_AUTO_LOITER: @@ -450,7 +447,7 @@ Navigator::task_main() case NAVIGATION_STATE_AUTO_RTL: _navigation_mode = &_rtl; break; - case NAVIGATION_STATE_AUTO_RTGS: //XXX OBC: differentiate between rc loss and dl loss here + case NAVIGATION_STATE_AUTO_RTGS: /* Use complex data link loss mode only when enabled via param * otherwise use rtl */ if (_param_datalinkloss_obc.get() != 0) { From 1072a3380c2d6bdea010bb5091c6d0b23fe6f224 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:11:46 +0200 Subject: [PATCH 113/138] enable engine failure circuit breaker --- src/modules/systemlib/circuit_breaker.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 64420be379..83c8344d54 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -120,7 +120,7 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); * @max 284953 * @group Circuit Breaker */ -PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 0); +PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); bool circuit_breaker_enabled(const char* breaker, int32_t magic) { From d4c0dc2ba0271f4d9c8044fd2a3a178cbb9987e3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 11:20:30 +0200 Subject: [PATCH 114/138] add and activate circuit breaker for gps failure detection --- src/modules/commander/commander.cpp | 5 +++-- src/modules/systemlib/circuit_breaker.c | 14 ++++++++++++++ src/modules/systemlib/circuit_breaker.h | 1 + 3 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index bf15bbeb60..b86f3678b0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1417,8 +1417,9 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (gps_position.fix_type >= 3 && //XXX check eph and epv ? - hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT) { + if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + (gps_position.fix_type >= 3 && + hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ if (status.gps_failure) { status.gps_failure = false; diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 83c8344d54..12187d70ea 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -122,6 +122,20 @@ PARAM_DEFINE_INT32(CBRK_FLIGHTTERM, 121212); */ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); +/** + * Circuit breaker for gps failure detection + * + * Setting this parameter to 240024 will disable the gps failure detection. + * If the aircraft is in gps failure mode the gps failure flag will be + * set to healthy + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 240024 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); + bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h index 6a55e4948d..b3431722fc 100644 --- a/src/modules/systemlib/circuit_breaker.h +++ b/src/modules/systemlib/circuit_breaker.h @@ -55,6 +55,7 @@ #define CBRK_AIRSPD_CHK_KEY 162128 #define CBRK_FLIGHTTERM_KEY 121212 #define CBRK_ENGINEFAIL_KEY 284953 +#define CBRK_GPSFAIL_KEY 240024 #include From 8b7c57a0d050d51f45f8c66536e5450e7a494d73 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:39:40 +0200 Subject: [PATCH 115/138] px4io driver: update cb only when changed --- src/drivers/px4io/px4io.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6f8d6f5f6b..fbb5d4f2e6 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -295,6 +295,7 @@ private: float _battery_amp_bias; ///< current sensor bias float _battery_mamphour_total;///< amp hours consumed so far uint64_t _battery_last_timestamp;///< last amp hour calculation timestamp + bool _cb_flighttermination; ///< true if the flight termination circuit breaker is enabled #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 bool _dsm_vcc_ctl; ///< true if relay 1 controls DSM satellite RX power @@ -515,7 +516,8 @@ PX4IO::PX4IO(device::Device *interface) : _battery_amp_per_volt(90.0f / 5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _cb_flighttermination(true) #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 , _dsm_vcc_ctl(false) #endif @@ -1051,6 +1053,9 @@ PX4IO::task_main() } } + /* Update Circuit breakers */ + _cb_flighttermination = circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY); + } } @@ -1170,7 +1175,7 @@ PX4IO::io_set_arming_state() } /* Do not set failsafe if circuit breaker is enabled */ - if (armed.force_failsafe && !circuit_breaker_enabled("CBRK_FLIGHTTERM", CBRK_FLIGHTTERM_KEY)) { + if (armed.force_failsafe && !_cb_flighttermination) { set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; } else { clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE; From d11f05b12c0b0c56703ab14303f91efd123b9dc6 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 13:40:03 +0200 Subject: [PATCH 116/138] commander: update gps and engine cb only when changed --- src/modules/commander/commander.cpp | 20 ++++++++++++++------ src/modules/uORB/topics/vehicle_status.h | 2 ++ 2 files changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b86f3678b0..9ebe006f0d 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -775,6 +775,8 @@ int commander_thread_main(int argc, char *argv[]) // CIRCUIT BREAKERS status.circuit_breaker_engaged_power_check = false; status.circuit_breaker_engaged_airspd_check = false; + status.circuit_breaker_engaged_enginefailure_check = false; + status.circuit_breaker_engaged_gpsfailure_check = false; /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -980,8 +982,8 @@ int commander_thread_main(int argc, char *argv[]) int32_t ef_throttle_thres = 1.0f; int32_t ef_current2throttle_thres = 0.0f; int32_t ef_time_thres = 1000.0f; - uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine - was healty*/ + uint64_t timestamp_engine_healthy = 0; /**< absolute time when engine was healty */ + /* check which state machines for changes, clear "changed" flag */ bool arming_state_changed = false; bool main_state_changed = false; @@ -1028,8 +1030,14 @@ int commander_thread_main(int argc, char *argv[]) param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); - status.circuit_breaker_engaged_airspd_check = circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_power_check = + circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status.circuit_breaker_engaged_airspd_check = + circuit_breaker_enabled("CBRK_AIRSPD_CHK", CBRK_AIRSPD_CHK_KEY); + status.circuit_breaker_engaged_enginefailure_check = + circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY); + status.circuit_breaker_engaged_gpsfailure_check = + circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY); status_changed = true; @@ -1417,7 +1425,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if GPS fix is ok */ - if (circuit_breaker_enabled("CBRK_GPSFAIL", CBRK_GPSFAIL_KEY) || + if (status.circuit_breaker_engaged_gpsfailure_check || (gps_position.fix_type >= 3 && hrt_elapsed_time(&gps_position.timestamp_position) < FAILSAFE_DEFAULT_TIMEOUT)) { /* handle the case where gps was regained */ @@ -1616,7 +1624,7 @@ int commander_thread_main(int argc, char *argv[]) /* Check engine failure * only for fixed wing for now */ - if (!circuit_breaker_enabled("CBRK_ENGINEFAIL", CBRK_ENGINEFAIL_KEY) && + if (!status.circuit_breaker_engaged_enginefailure_check && status.is_rotary_wing == false && armed.armed && ((actuator_controls.control[3] > ef_throttle_thres && diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9dccb2309d..505039d90d 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -239,6 +239,8 @@ struct vehicle_status_s { bool circuit_breaker_engaged_power_check; bool circuit_breaker_engaged_airspd_check; + bool circuit_breaker_engaged_enginefailure_check; + bool circuit_breaker_engaged_gpsfailure_check; }; /** From 47dcf88271b4b05f2007383e62a6dd239b7eb859 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:18:30 +0200 Subject: [PATCH 117/138] Flash optimization --- makefiles/config_px4fmu-v1_default.mk | 6 +++--- src/lib/conversion/module.mk | 2 ++ 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 6f39b56a13..cf631c2be5 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -24,14 +24,14 @@ MODULES += drivers/l3gd20 MODULES += drivers/mpu6000 MODULES += drivers/hmc5883 MODULES += drivers/ms5611 -MODULES += drivers/mb12xx +#MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/blinkm +#MODULES += drivers/blinkm MODULES += drivers/rgbled MODULES += drivers/mkblctrl MODULES += drivers/airspeed -MODULES += drivers/ets_airspeed +#MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed MODULES += drivers/frsky_telemetry MODULES += modules/sensors diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk index f5f59a2dca..4593c48871 100644 --- a/src/lib/conversion/module.mk +++ b/src/lib/conversion/module.mk @@ -36,3 +36,5 @@ # SRCS = rotation.cpp + +MAXOPTIMIZATION = -Os From 47f151d4cefa5d7a9da34d6139aa493259f8b2fd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 30 Sep 2014 15:19:06 +0200 Subject: [PATCH 118/138] Deactivate FrSky telem by default --- makefiles/config_px4fmu-v1_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index cf631c2be5..9fe16fbb6c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -33,7 +33,7 @@ MODULES += drivers/mkblctrl MODULES += drivers/airspeed #MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -MODULES += drivers/frsky_telemetry +#MODULES += drivers/frsky_telemetry MODULES += modules/sensors # From 6da52c5543012a1391c5e5c4384434c1292b919b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:39:01 +0200 Subject: [PATCH 119/138] remove unnecessary variable --- src/modules/navigator/navigator_main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index fad46998e1..b81155efe3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -388,11 +388,9 @@ Navigator::task_main() /* global position updated */ if (fds[0].revents & POLLIN) { global_position_update(); - static int gposcounter = 0; if (_geofence.getSource() == Geofence::GF_SOURCE_GLOBALPOS) { have_geofence_position_data = true; } - gposcounter++; } /* Check geofence violation */ From c5a1ddd8f2dd32005c2581886e96b3419009fef4 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:42:56 +0200 Subject: [PATCH 120/138] re-enable MC apps on FMUV1 --- makefiles/config_px4fmu-v1_default.mk | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 9aa8320ee8..9fe16fbb6c 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -64,17 +64,17 @@ MODULES += modules/gpio_led # # Estimation modules (EKF / other filters) # -#MODULES += modules/attitude_estimator_ekf +MODULES += modules/attitude_estimator_ekf MODULES += modules/ekf_att_pos_estimator -#MODULES += modules/position_estimator_inav +MODULES += modules/position_estimator_inav # # Vehicle Control # MODULES += modules/fw_pos_control_l1 MODULES += modules/fw_att_control -#MODULES += modules/mc_att_control -#MODULES += modules/mc_pos_control +MODULES += modules/mc_att_control +MODULES += modules/mc_pos_control # # Logging From 4fbeb73bda55f14d1ffd259d064e116047b20301 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:48:48 +0200 Subject: [PATCH 121/138] fix merge fail, remove double declaration of circuit breaker --- src/modules/systemlib/circuit_breaker.c | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c index 8ad79f94d6..12187d70ea 100644 --- a/src/modules/systemlib/circuit_breaker.c +++ b/src/modules/systemlib/circuit_breaker.c @@ -136,20 +136,6 @@ PARAM_DEFINE_INT32(CBRK_ENGINEFAIL, 284953); */ PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); -/** - * Circuit breaker for gps failure detection - * - * Setting this parameter to 240024 will disable the gps failure detection. - * If the aircraft is in gps failure mode the gps failure flag will be - * set to healthy - * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK - * - * @min 0 - * @max 240024 - * @group Circuit Breaker - */ -PARAM_DEFINE_INT32(CBRK_GPSFAIL, 240024); - bool circuit_breaker_enabled(const char* breaker, int32_t magic) { int32_t val; From f4851c1b148f21143c4a0446a843948af6793ed3 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:50:58 +0200 Subject: [PATCH 122/138] ubx: disable sbas configuration per default --- src/drivers/gps/ubx.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index c54c474b3d..45bd445c45 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -189,6 +189,7 @@ UBX::configure(unsigned &baudrate) return 1; } +#ifdef UBX_CONFIGURE_SBAS /* send a SBAS message to set the SBAS options */ memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas)); _buf.payload_tx_cfg_sbas.mode = UBX_TX_CFG_SBAS_MODE; @@ -198,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } +#endif /* configure message rates */ /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ From 5452732610bd1d5b795558fa8f9d9ffb2639dc0b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 15:54:27 +0200 Subject: [PATCH 123/138] switch back to common mavlink dialect --- src/modules/mavlink/mavlink_bridge_header.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h index 00d427636a..0cd08769e1 100644 --- a/src/modules/mavlink/mavlink_bridge_header.h +++ b/src/modules/mavlink/mavlink_bridge_header.h @@ -84,7 +84,7 @@ void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int leng extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan); extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan); -#include +#include __END_DECLS From 2766285d56cab6f9916efde535b979bc5475f6a5 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 30 Sep 2014 16:00:14 +0200 Subject: [PATCH 124/138] mavlink: change message buffer size to 4 --- src/modules/mavlink/mavlink_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index b9f486c95e..980362ce56 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(10 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } @@ -1382,7 +1382,7 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - + switch (_mode) { case MAVLINK_MODE_NORMAL: From 84908f8f3db5179ffff0d96d15756ab112535482 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 Oct 2014 15:45:02 +0400 Subject: [PATCH 125/138] mc_pos_control: AUTO speed limiting bug fixed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 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 99deb0d29a..ec7b2a78f5 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -829,10 +829,12 @@ MulticopterPositionControl::control_auto(float dt) /* move setpoint not faster than max allowed speed */ math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale); - math::Vector<3> d_pos_s = pos_sp_s - pos_sp_old_s; - float d_pos_s_len = d_pos_s.length(); - if (d_pos_s_len > dt) { - pos_sp_s = pos_sp_old_s + d_pos_s / d_pos_s_len * dt; + + /* difference between current and desired position setpoints, 1 = max speed */ + math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p); + float d_pos_m_len = d_pos_m.length(); + if (d_pos_m_len > dt) { + pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p); } /* scale result back to normal space */ From 5846f217d0e09ee4c1c64c0ebf0f2e621a14fa69 Mon Sep 17 00:00:00 2001 From: tstellanova Date: Fri, 3 Oct 2014 17:18:44 -0700 Subject: [PATCH 126/138] Use global position altitude to report HUD altitude, for consistency. Otherwise the HUD altitude jumps between two very different values. --- src/modules/mavlink/mavlink_messages.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a2f3828ffa..941b45d881 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,7 +820,11 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - msg.alt = sensor_combined.baro_alt_meter; + /* + Do not use sensor_combined.baro_alt_meter here because + it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. + */ + msg.alt = pos.alt; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 7bde4fa6342230ab37644f94d364cd6e9541773e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 4 Oct 2014 12:31:16 +0200 Subject: [PATCH 127/138] Revert "Use global position altitude to report HUD altitude, for consistency." --- src/modules/mavlink/mavlink_messages.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 941b45d881..a2f3828ffa 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -820,11 +820,7 @@ protected: msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); msg.heading = _wrap_2pi(att.yaw) * M_RAD_TO_DEG_F; msg.throttle = armed.armed ? act.control[3] * 100.0f : 0.0f; - /* - Do not use sensor_combined.baro_alt_meter here because - it is mismatched with WSG84 AMSL used elsewhere for reporting altitude. - */ - msg.alt = pos.alt; + msg.alt = sensor_combined.baro_alt_meter; msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); From 56a9d16fc476e99106bda0627cc20f69e987fc6a Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:19:13 +0400 Subject: [PATCH 128/138] FTP: Save errno in _copy_file(). --- src/modules/mavlink/mavlink_ftp.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index c26fe6b4bc..4ed06561d3 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -560,7 +560,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) return kErrFailErrno; } - if (payload->offset == st.st_size) { + if (payload->offset == (unsigned)st.st_size) { // nothing to do return kErrNone; } @@ -574,7 +574,7 @@ MavlinkFTP::_workTruncateFile(PayloadHeader* payload) ::close(fd); return kErrNone; } - else if (payload->offset > st.st_size) { + else if (payload->offset > (unsigned)st.st_size) { // 2: extend file int fd = ::open(file, O_WRONLY); if (fd < 0) { @@ -808,6 +808,7 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt { char buff[512]; int src_fd = -1, dst_fd = -1; + int op_errno = 0; src_fd = ::open(src_path, O_RDONLY); if (src_fd < 0) { @@ -816,7 +817,9 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY); if (dst_fd < 0) { + op_errno = errno; ::close(src_fd); + errno = op_errno; return -1; } @@ -831,12 +834,14 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt } else if (bytes_read < 0) { warnx("cp: read"); + op_errno = errno; break; } bytes_written = ::write(dst_fd, buff, bytes_read); if (bytes_written != bytes_read) { warnx("cp: short write"); + op_errno = errno; break; } @@ -846,5 +851,6 @@ MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t lengt ::close(src_fd); ::close(dst_fd); + errno = op_errno; return (length > 0)? -1 : 0; } From 0fbdb8d326e3a764fde840e426e093bae32bcba6 Mon Sep 17 00:00:00 2001 From: Vladimir Ermakov Date: Sun, 5 Oct 2014 13:23:57 +0400 Subject: [PATCH 129/138] FTP: Return bytes written in payload. --- src/modules/mavlink/mavlink_ftp.cpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp index 4ed06561d3..f17497aa8f 100644 --- a/src/modules/mavlink/mavlink_ftp.cpp +++ b/src/modules/mavlink/mavlink_ftp.cpp @@ -508,11 +508,8 @@ MavlinkFTP::_workWrite(PayloadHeader* payload) return kErrFailErrno; } - // need to decide how to indicate how much data was written - // current: old code set hdr->size - payload->size = bytes_written; - //payload->size = sizeof(uint32_t); - //*((uint32_t*)payload->data) = bytes_written; + payload->size = sizeof(uint32_t); + *((uint32_t*)payload->data) = bytes_written; return kErrNone; } From 2722921b30a48fd3fb67ba25efe8c4e3ca1338c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 12:30:29 +0200 Subject: [PATCH 130/138] Fixed function name of mission modification logic, attributed @DrTon --- src/modules/navigator/mission.cpp | 9 +++++---- src/modules/navigator/mission.h | 13 +++++++------ 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 87d16f1e67..b5926df815 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #include @@ -157,7 +158,7 @@ Mission::on_active() /* else just report that item reached */ if (_mission_type == MISSION_TYPE_OFFBOARD) { if (!(_navigator->get_mission_result()->seq_reached == _current_offboard_mission_index && _navigator->get_mission_result()->reached)) { - report_mission_item_reached(); + set_mission_item_reached(); } } } @@ -701,7 +702,7 @@ Mission::save_offboard_mission_state() } void -Mission::report_mission_item_reached() +Mission::set_mission_item_reached() { _navigator->get_mission_result()->reached = true; _navigator->get_mission_result()->seq_reached = _current_offboard_mission_index; @@ -709,7 +710,7 @@ Mission::report_mission_item_reached() } void -Mission::report_current_offboard_mission_item() +Mission::set_current_offboard_mission_item() { warnx("current offboard mission index: %d", _current_offboard_mission_index); _navigator->get_mission_result()->reached = false; @@ -721,7 +722,7 @@ Mission::report_current_offboard_mission_item() } void -Mission::report_mission_finished() +Mission::set_mission_finished() { _navigator->get_mission_result()->finished = true; _navigator->publish_mission_result(); diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 5164737f33..ea7cc0927c 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -37,6 +37,7 @@ * * @author Julian Oes * @author Thomas Gubler + * @author Anton Babushkin */ #ifndef NAVIGATOR_MISSION_H @@ -130,19 +131,19 @@ private: void save_offboard_mission_state(); /** - * Report that a mission item has been reached + * Set a mission item as reached */ - void report_mission_item_reached(); + void set_mission_item_reached(); /** - * Rport the current mission item + * Set the current offboard mission item */ - void report_current_offboard_mission_item(); + void set_current_offboard_mission_item(); /** - * Report that the mission is finished if one exists or that none exists + * Set that the mission is finished if one exists or that none exists */ - void report_mission_finished(); + void set_mission_finished(); control::BlockParamInt _param_onboard_enabled; control::BlockParamFloat _param_takeoff_alt; From 61352b8e1af8d076ea9f6edb9932f3b498a72e7a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 12:52:04 +0200 Subject: [PATCH 131/138] remove warnx --- src/drivers/gps/ubx.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 45bd445c45..b0eb4ab662 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -277,8 +277,6 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) } } - warnx("msg %u ret %d", msg, ret); - _ack_state = UBX_ACK_IDLE; return ret; } From ebc84b9f44dd3d26bc8d26b18f8c059f900e5e1a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 5 Oct 2014 13:02:13 +0200 Subject: [PATCH 132/138] reduce mavlink message buffer size --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 980362ce56..25dc018447 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1328,7 +1328,7 @@ Mavlink::task_main(int argc, char *argv[]) * make space for two messages plus off-by-one space as we use the empty element * marker ring buffer approach. */ - if (OK != message_buffer_init(4 * sizeof(mavlink_message_t) + 1)) { + if (OK != message_buffer_init(2 * sizeof(mavlink_message_t) + 1)) { errx(1, "can't allocate message buffer, exiting"); } From 2587074a07f67ed37c8d4f0b4d7f1b31bb2f6525 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:07:56 +0200 Subject: [PATCH 133/138] Compile fixes in navigator --- src/modules/navigator/mission.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index b5926df815..7fac69a611 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -234,7 +234,7 @@ Mission::update_offboard_mission() _current_offboard_mission_index = 0; } - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } @@ -396,7 +396,7 @@ Mission::set_mission_items() _navigator->set_can_loiter_at_sp(pos_sp_triplet->current.type == SETPOINT_TYPE_LOITER); reset_mission_item_reached(); - report_mission_finished(); + set_mission_finished(); _navigator->set_position_setpoint_triplet_updated(); return; @@ -476,7 +476,7 @@ Mission::set_mission_items() reset_mission_item_reached(); if (_mission_type == MISSION_TYPE_OFFBOARD) { - report_current_offboard_mission_item(); + set_current_offboard_mission_item(); } // TODO: report onboard mission item somehow From ff17f31ccedc99bb9151e2f905d336345f2ed7d0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:02 +0200 Subject: [PATCH 134/138] Dataman: Optimize for space --- src/modules/dataman/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/dataman/module.mk b/src/modules/dataman/module.mk index 234607b3de..7ebe09fb7e 100644 --- a/src/modules/dataman/module.mk +++ b/src/modules/dataman/module.mk @@ -40,3 +40,5 @@ MODULE_COMMAND = dataman SRCS = dataman.c MODULE_STACKSIZE = 1200 + +MAXOPTIMIZATION = -Os From 850c630e11dca24bc160d00b580c7ff7d8b6ff6d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:09:19 +0200 Subject: [PATCH 135/138] Navigator: Optimize for space --- src/modules/navigator/module.mk | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk index c075903b75..fb8d079010 100644 --- a/src/modules/navigator/module.mk +++ b/src/modules/navigator/module.mk @@ -63,3 +63,5 @@ INCLUDE_DIRS += $(MAVLINK_SRC)/include/mavlink MODULE_STACKSIZE = 1200 EXTRACXXFLAGS = -Weffc++ + +MAXOPTIMIZATION = -Os From c0eefc983a5bbe2faf05eb21daaf3514b767daa6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 5 Oct 2014 13:13:22 +0200 Subject: [PATCH 136/138] File test compile fixes --- src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp index a5fb2117ea..4caa820b6e 100644 --- a/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp +++ b/src/modules/mavlink/mavlink_tests/mavlink_ftp_test.cpp @@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void) MavlinkFTP::PayloadHeader *reply; const char *dir = "/foo"; // non-existent file - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void) struct stat st; const ReadTestCase *test = &_rgReadTestCases[i]; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void) // Test case data files are created for specific boundary conditions ut_compare("Test case data files are out of date", test->length, st.st_size); - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header @@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void) MavlinkFTP::PayloadHeader *reply; const char *file = _rgReadTestCases[0].file; - payload.opcode = MavlinkFTP::kCmdOpenFile; + payload.opcode = MavlinkFTP::kCmdOpenFileRO; payload.offset = 0; bool success = _send_receive_msg(&payload, // FTP payload header From ba2f55c3d7f5872aaf07e20b58b15df85417d43a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:09:03 +0200 Subject: [PATCH 137/138] Revert "increase ram" This reverts commit bc23b6239c50527aa550eed3bc6f17dec15c5c97. --- src/modules/mavlink/mavlink_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 25dc018447..34596320fe 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1630,7 +1630,7 @@ Mavlink::start(int argc, char *argv[]) task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 5000, + 2700, (main_t)&Mavlink::start_helper, (const char **)argv); From c7f7de352d7e0f2921526e33077c6da6a46b404a Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 7 Oct 2014 10:12:56 +0200 Subject: [PATCH 138/138] revert some of the OBC rate changes --- src/modules/mavlink/mavlink_main.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 34596320fe..4738d1ea71 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1382,28 +1382,25 @@ Mavlink::task_main(int argc, char *argv[]) _mission_manager->set_verbose(_verbose); LL_APPEND(_streams, _mission_manager); - - switch (_mode) { case MAVLINK_MODE_NORMAL: configure_stream("SYS_STATUS", 1.0f); configure_stream("GPS_GLOBAL_ORIGIN", 0.5f); configure_stream("HIGHRES_IMU", 1.0f); - configure_stream("ATTITUDE", 4.0f); - configure_stream("VFR_HUD", 4.0f); + configure_stream("ATTITUDE", 10.0f); + configure_stream("VFR_HUD", 8.0f); configure_stream("GPS_RAW_INT", 1.0f); - configure_stream("GLOBAL_POSITION_INT", 1.0f); - configure_stream("LOCAL_POSITION_NED", 1.0f); + configure_stream("GLOBAL_POSITION_INT", 3.0f); + configure_stream("LOCAL_POSITION_NED", 3.0f); configure_stream("RC_CHANNELS_RAW", 1.0f); - configure_stream("POSITION_TARGET_GLOBAL_INT", 1.0f); - configure_stream("ATTITUDE_TARGET", 1.0f); + configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f); + configure_stream("ATTITUDE_TARGET", 3.0f); configure_stream("DISTANCE_SENSOR", 0.5f); - //configure_stream("OPTICAL_FLOW", 20.0f); + configure_stream("OPTICAL_FLOW", 5.0f); break; case MAVLINK_MODE_ONBOARD: configure_stream("SYS_STATUS", 1.0f); - // XXX OBC change back: We need to be bandwidth-efficient here too configure_stream("ATTITUDE", 50.0f); configure_stream("GLOBAL_POSITION_INT", 50.0f); configure_stream("CAMERA_CAPTURE", 2.0f);