From 170f9aec498c776d84db7a739a0602cc4977e3b6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 16 Jan 2016 14:38:12 +0100 Subject: [PATCH 001/128] Update standard VTOL model --- Tools/sitl_gazebo | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index 57a54ab737..16f7fb3761 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit 57a54ab73740f45f7313cc8179e21cc3b0d53b41 +Subproject commit 16f7fb37619e4af772a543f7bac62bef5126347b From 07fafc491347e858ef90c5602e33a23b3bb5ede3 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 15 Jan 2016 12:44:00 +0100 Subject: [PATCH 002/128] multirotor mission feasibility checks: make sure that the relative altitude of the takeoff waypoint is at least one meter higher than the acceptance radius of the waypoint. This makes sure that the takeoff waypoint is not reached before the vehicle is at least one meter in the air. --- src/modules/navigator/mission.cpp | 4 +- .../navigator/mission_feasibility_checker.cpp | 41 +++++++++++++++++-- .../navigator/mission_feasibility_checker.h | 4 +- 3 files changed, 41 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 992a426bfe..242b0faedc 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -259,7 +259,7 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -843,7 +843,7 @@ Mission::check_mission_valid() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index fd9a206c86..2cf1c45f56 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -64,7 +64,8 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, - float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued) + float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, + float default_acceptance_rad) { bool failed = false; bool warned = false; @@ -88,7 +89,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); if (isRotarywing) { - failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid); + failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid, default_acceptance_rad); } else { failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid); } @@ -96,9 +97,41 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota return !failed; } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, + Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad) { - /* no custom rotary wing checks yet */ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + return false; + } + + // look for a takeoff waypoint + if (missionitem.nav_cmd == NAV_CMD_TAKEOFF) { + // make sure that the altitude of the waypoint is at least one meter larger than the acceptance radius + // this makes sure that the takeoff waypoint is not reached before we are at least one meter in the air + float takeoff_alt = missionitem.altitude_is_relative + ? missionitem.altitude + : missionitem.altitude - home_alt; + // check if we should use default acceptance radius + float acceptance_radius = default_acceptance_rad; + + if (missionitem.acceptance_radius > NAV_EPSILON_POSITION) { + acceptance_radius = missionitem.acceptance_radius; + } + + if (takeoff_alt - 1.0f < acceptance_radius) { + mavlink_log_critical(_mavlink_fd, "Mission rejected: Takeoff altitude too low!"); + return false; + } + } + } + + // all checks have passed return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 4586f75a47..a1831999ac 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -72,7 +72,7 @@ private: void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, float default_acceptance_rad); public: MissionFeasibilityChecker(); @@ -83,7 +83,7 @@ public: */ bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad); }; From 57b95916f5b0e4a493f74ec14772f2b66be0ba4d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Jan 2016 16:29:36 +0100 Subject: [PATCH 003/128] MAVLink: Add stack space for interface --- 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 4eda4edb6c..412ae33ff0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -2141,7 +2141,7 @@ Mavlink::start(int argc, char *argv[]) px4_task_spawn_cmd(buf, SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, - 2400, + 2700, (px4_main_t)&Mavlink::start_helper, (char *const *)argv); From bafa9bb6bb637c70cb170593cf06e5fecb936670 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Jan 2016 16:30:02 +0100 Subject: [PATCH 004/128] MAVLink: Fall back in altitude indication to baro if estimate is not available --- src/modules/mavlink/mavlink_messages.cpp | 50 +++++++++++++++++------- 1 file changed, 36 insertions(+), 14 deletions(-) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index a196b6eb8e..34507c7c59 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -885,6 +885,9 @@ private: MavlinkOrbSubscription *_airspeed_sub; uint64_t _airspeed_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + /* do not allow top copying this class */ MavlinkStreamVFRHUD(MavlinkStreamVFRHUD &); MavlinkStreamVFRHUD& operator = (const MavlinkStreamVFRHUD &); @@ -900,16 +903,18 @@ protected: _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_controls_0))), _act_time(0), _airspeed_sub(_mavlink->add_orb_subscription(ORB_ID(airspeed))), - _airspeed_time(0) + _airspeed_time(0), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0) {} void send(const hrt_abstime t) { - struct vehicle_attitude_s att; - struct vehicle_global_position_s pos; - struct actuator_armed_s armed; - struct actuator_controls_s act; - struct airspeed_s airspeed; + struct vehicle_attitude_s att = {}; + struct vehicle_global_position_s pos = {}; + struct actuator_armed_s armed = {}; + struct actuator_controls_s act = {}; + struct airspeed_s airspeed = {}; bool updated = _att_sub->update(&_att_time, &att); updated |= _pos_sub->update(&_pos_time, &pos); @@ -924,7 +929,15 @@ 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 = pos.alt; + if (_pos_time > 0) { + /* use global estimate */ + msg.alt = pos.alt; + } else { + /* fall back to baro altitude */ + sensor_combined_s sensor; + (void)_sensor_sub->update(&_sensor_time, &sensor); + msg.alt = sensor.baro_alt_meter[0]; + } msg.climb = -pos.vel_d; _mavlink->send_message(MAVLINK_MSG_ID_VFR_HUD, &msg); @@ -2668,6 +2681,9 @@ private: MavlinkOrbSubscription *_home_sub; uint64_t _home_time; + MavlinkOrbSubscription *_sensor_sub; + uint64_t _sensor_time; + /* do not allow top copying this class */ MavlinkStreamAltitude(MavlinkStreamAltitude &); MavlinkStreamAltitude& operator = (const MavlinkStreamAltitude &); @@ -2679,7 +2695,9 @@ protected: _local_pos_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_local_position))), _local_pos_time(0), _home_sub(_mavlink->add_orb_subscription(ORB_ID(home_position))), - _home_time(0) + _home_time(0), + _sensor_sub(_mavlink->add_orb_subscription(ORB_ID(sensor_combined))), + _sensor_time(0) {} void send(const hrt_abstime t) @@ -2693,21 +2711,25 @@ protected: updated |= _home_sub->update(&_home_time, &home); if (updated) { + + struct sensor_combined_s sensor; + (void)_sensor_sub->update(&_sensor_time, &sensor); + mavlink_altitude_t msg; msg.time_usec = hrt_absolute_time(); - msg.altitude_monotonic = global_pos.pressure_alt; - msg.altitude_amsl = global_pos.alt; - msg.altitude_local = -local_pos.z; - msg.altitude_relative = home.alt; + msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter[0] : 0.0f / 0.0f; + msg.altitude_amsl = (_global_pos_time > 0) ? global_pos.alt : 0.0f / 0.0f; + msg.altitude_local = (_local_pos_time > 0) ? -local_pos.z : 0.0f / 0.0f; + msg.altitude_relative = (_home_time > 0) ? home.alt : 0.0f / 0.0f; if (global_pos.terrain_alt_valid) { msg.altitude_terrain = global_pos.terrain_alt; msg.bottom_clearance = global_pos.alt - global_pos.terrain_alt; } else { - msg.altitude_terrain = -1001; - msg.bottom_clearance = -1; + msg.altitude_terrain = 0.0f / 0.0f; + msg.bottom_clearance = 0.0f / 0.0f; } _mavlink->send_message(MAVLINK_MSG_ID_ALTITUDE, &msg); From c802b86acccb17a7c5ea7cf5911475e251a372dc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Jan 2016 10:23:23 +0100 Subject: [PATCH 005/128] Sensors: Fix FMUv4 voltage --- src/modules/sensors/sensors.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 13a965cc2c..c161cc6052 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -850,12 +850,13 @@ Sensors::parameters_update() } else if (_parameters.battery_voltage_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ - -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V2) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + _parameters.battery_voltage_scaling = 0.011f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) _parameters.battery_voltage_scaling = 0.0082f; -#elif CONFIG_ARCH_BOARD_AEROCORE +#elif defined (CONFIG_ARCH_BOARD_AEROCORE) _parameters.battery_voltage_scaling = 0.0063f; -#elif CONFIG_ARCH_BOARD_PX4FMU_V1 +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) _parameters.battery_voltage_scaling = 0.00459340659f; #else /* ensure a missing default trips a low voltage lockdown */ From 8cb472af31e00c2fe89857bb8c71a445fad516b3 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 18 Dec 2015 21:21:51 -0700 Subject: [PATCH 006/128] add RC kill switch --- msg/manual_control_setpoint.msg | 5 +++-- msg/rc_channels.msg | 7 ++++--- src/modules/commander/commander.cpp | 12 ++++++++++++ src/modules/sensors/sensor_params.c | 28 ++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 17 +++++++++++++++++ 5 files changed, 64 insertions(+), 5 deletions(-) diff --git a/msg/manual_control_setpoint.msg b/msg/manual_control_setpoint.msg index 2ff6fca1f5..424453d699 100644 --- a/msg/manual_control_setpoint.msg +++ b/msg/manual_control_setpoint.msg @@ -38,8 +38,9 @@ float32 aux5 # default function: payload drop uint8 mode_switch # main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO uint8 return_switch # return to launch 2 position switch (mandatory): _NORMAL_, RTL -uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE +uint8 rattitude_switch # rattitude control 2 position switch (optional): _MANUAL, RATTITUDE uint8 posctl_switch # position control 2 position switch (optional): _ALTCTL_, POSCTL uint8 loiter_switch # loiter 2 position switch (optional): _MISSION_, LOITER uint8 acro_switch # acro 2 position switch (optional): _MANUAL_, ACRO -uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 offboard_switch # offboard 2 position switch (optional): _NORMAL_, OFFBOARD +uint8 kill_switch # throttle kill: _NORMAL_, KILL diff --git a/msg/rc_channels.msg b/msg/rc_channels.msg index 6b30794e00..3171fbc106 100644 --- a/msg/rc_channels.msg +++ b/msg/rc_channels.msg @@ -1,4 +1,4 @@ -int32 RC_CHANNELS_FUNCTION_MAX=20 +int32 RC_CHANNELS_FUNCTION_MAX=21 uint8 RC_CHANNELS_FUNCTION_THROTTLE=0 uint8 RC_CHANNELS_FUNCTION_ROLL=1 uint8 RC_CHANNELS_FUNCTION_PITCH=2 @@ -19,11 +19,12 @@ uint8 RC_CHANNELS_FUNCTION_PARAM_1=16 uint8 RC_CHANNELS_FUNCTION_PARAM_2=17 uint8 RC_CHANNELS_FUNCTION_PARAM_3_5=18 uint8 RC_CHANNELS_FUNCTION_RATTITUDE=19 +uint8 RC_CHANNELS_FUNCTION_KILLSWITCH=20 uint64 timestamp # Timestamp in microseconds since boot time uint64 timestamp_last_valid # Timestamp of last valid RC signal -float32[20] channels # Scaled to -1..1 (throttle: 0..1) +float32[18] channels # Scaled to -1..1 (throttle: 0..1) uint8 channel_count # Number of valid channels -int8[20] function # Functions mapping +int8[21] function # Functions mapping uint8 rssi # Receive signal strength index bool signal_lost # Control signal lost, should be checked together with topic timeout uint32 frame_drop_count # Number of dropped frames diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 6b5c29ab93..e2d202e9d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2284,6 +2284,18 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "main state transition denied"); } + /* check throttle kill switch */ + int prevLockdown = armed.lockdown; + if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { + /* set lockdown flag */ + armed.lockdown = TRUE; + } else { + armed.lockdown = FALSE; + } + if (prevLockdown != armed.lockdown) { + warnx("armed.lockdown: %d\n", armed.lockdown); + } + } else { if (!status.rc_input_blocked && !status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "MANUAL CONTROL LOST (at t=%llums)", hrt_absolute_time() / 1000); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index aa91fcf92a..fa1dfeb1b7 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2009,6 +2009,15 @@ PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0); */ PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +/** + * Kill switch channel mapping. + * + * @min 0 + * @max 18 + * @group Radio Switches + */ +PARAM_DEFINE_INT32(RC_MAP_KILL_SW, 0); + /** * Flaps channel mapping. * @@ -2243,6 +2252,25 @@ PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f); */ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); + +/** + * Threshold for selecting offboard mode + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channel Date: Sun, 17 Jan 2016 13:05:53 +0100 Subject: [PATCH 007/128] Fix list of files to check for code style --- Tools/check_code_style.sh | 1 - 1 file changed, 1 deletion(-) diff --git a/Tools/check_code_style.sh b/Tools/check_code_style.sh index cd42b65c80..54ce08c680 100755 --- a/Tools/check_code_style.sh +++ b/Tools/check_code_style.sh @@ -25,7 +25,6 @@ for fn in $(find src/examples \ src/modules/uORB \ src/modules/bottle_drop \ src/modules/dataman \ - src/modules/fixedwing_backside \ src/modules/segway \ src/modules/local_position_estimator \ src/modules/unit_test \ From f485b60f570bc80ba465339624c127a0eaf852b4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Jan 2016 13:44:18 +0100 Subject: [PATCH 008/128] Kill switch: Fix logic to only trigger on on/off state of switch, not on/undefined. Remove debug output. --- src/modules/commander/commander.cpp | 11 ++++------- 1 file changed, 4 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e2d202e9d0..19562ccb6e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2285,16 +2285,13 @@ int commander_thread_main(int argc, char *argv[]) } /* check throttle kill switch */ - int prevLockdown = armed.lockdown; if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) { /* set lockdown flag */ - armed.lockdown = TRUE; - } else { - armed.lockdown = FALSE; - } - if (prevLockdown != armed.lockdown) { - warnx("armed.lockdown: %d\n", armed.lockdown); + armed.lockdown = true; + } else if (sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) { + armed.lockdown = false; } + /* no else case: do not change lockdown flag in unconfigured case */ } else { if (!status.rc_input_blocked && !status.rc_signal_lost) { From ab65a55fbf374a881318921e982e1df158853dc7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Jan 2016 13:45:44 +0100 Subject: [PATCH 009/128] Change arming transfer to only set the register if the local configuration changed. Move its write operation to the fast rate so that arming / disarming is instantaneous --- src/drivers/px4io/px4io.cpp | 37 +++++++++++++++++++++++++------------ 1 file changed, 25 insertions(+), 12 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 9db91d28b8..1ff147d190 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -273,6 +273,7 @@ private: /* cached IO state */ uint16_t _status; ///< Various IO status flags uint16_t _alarms; ///< Various IO alarms + uint16_t _last_written_arming; ///< the last written arming state reg /* subscribed topics */ int _t_actuator_controls_0; ///< actuator controls group 0 topic @@ -509,6 +510,7 @@ PX4IO::PX4IO(device::Device *interface) : _perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")), _status(0), _alarms(0), + _last_written_arming(0), _t_actuator_controls_0(-1), _t_actuator_controls_1(-1), _t_actuator_controls_2(-1), @@ -977,7 +979,7 @@ PX4IO::task_main() } if (now >= poll_last + IO_POLL_INTERVAL) { - /* run at 50Hz */ + /* run at 50-250Hz */ poll_last = now; /* pull status and alarms from IO */ @@ -988,16 +990,6 @@ PX4IO::task_main() /* fetch PWM outputs from IO */ io_publish_pwm_outputs(); - } - - if (now >= orb_check_last + ORB_CHECK_INTERVAL) { - /* run at 5Hz */ - orb_check_last = now; - - /* try to claim the MAVLink log FD */ - if (_mavlink_fd < 0) { - _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); - } /* check updates on uORB topics and handle it */ bool updated = false; @@ -1012,6 +1004,19 @@ PX4IO::task_main() if (updated) { io_set_arming_state(); } + } + + if (now >= orb_check_last + ORB_CHECK_INTERVAL) { + /* run at 5Hz */ + orb_check_last = now; + + /* try to claim the MAVLink log FD */ + if (_mavlink_fd < 0) { + _mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); + } + + /* check updates on uORB topics and handle it */ + bool updated = false; /* vehicle command */ orb_check(_t_vehicle_command, &updated); @@ -1350,7 +1355,15 @@ PX4IO::io_set_arming_state() } } - return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); + uint16_t new_arming_state = set; + new_arming_state &= ~(clear); + + if (_last_written_arming != new_arming_state) { + _last_written_arming = new_arming_state; + return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set); + } + + return 0; } int From 803f2ce03584d96847100fb42ec8e2a02ad14b57 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 17 Jan 2016 15:59:36 +0100 Subject: [PATCH 010/128] Fix threshold param comment --- src/modules/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index fa1dfeb1b7..dc081f0644 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -2254,7 +2254,7 @@ PARAM_DEFINE_FLOAT(RC_OFFB_TH, 0.5f); /** - * Threshold for selecting offboard mode + * Threshold for the kill switch * * 0-1 indicate where in the full channel range the threshold sits * 0 : min From 7ead4050d6608c5c7a433ad46077ef5cfc58067d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Jan 2016 13:08:35 +0100 Subject: [PATCH 011/128] Hotfix for sock addr, but this is merely a workaround --- 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 231ea21c1d..4efeefb180 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1771,7 +1771,7 @@ MavlinkReceiver::receive_thread(void *arg) fds[0].events = POLLIN; } #ifdef __PX4_POSIX - struct sockaddr_in srcaddr; + struct sockaddr_in srcaddr = {}; socklen_t addrlen = sizeof(srcaddr); if (_mavlink->get_protocol() == UDP || _mavlink->get_protocol() == TCP) { @@ -1817,8 +1817,8 @@ MavlinkReceiver::receive_thread(void *arg) if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him - memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr)); - PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); + //memcpy(srcaddr_last, &srcaddr, sizeof(*srcaddr_last)); + //PX4_WARN("XXX: not really updating: UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); _mavlink->set_client_source_initialized(); } #endif From de88892f99660ffbe7ddada723afbdabab89eac9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Jan 2016 13:40:30 -0800 Subject: [PATCH 012/128] Update Matrix to fix QuRT build --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 1d9d8e6f9b..48a3288753 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 1d9d8e6f9b0c2ed930ea1c8996f5fb398b3cf322 +Subproject commit 48a3288753a3ec0bbd5e1f826ea413e1a6854fbb From 4351eb147c4d208640a41934a02c1b611bb3b8d7 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sun, 17 Jan 2016 11:04:11 +0000 Subject: [PATCH 013/128] Add native RPi2 build config --- Makefile | 3 ++ cmake/configs/posix_rpi2_default.cmake | 28 ++++++++++ posix-configs/rpi2/init/rcS_navio | 71 ++++++++++++++++++++++++++ src/lib/version/version.h | 5 ++ 4 files changed, 107 insertions(+) create mode 100644 cmake/configs/posix_rpi2_default.cmake create mode 100644 posix-configs/rpi2/init/rcS_navio diff --git a/Makefile b/Makefile index e9ab6f06e1..6f9fb0f706 100644 --- a/Makefile +++ b/Makefile @@ -170,6 +170,9 @@ qurt_eagle_default: posix_eagle_default: $(call cmake-build,$@) + +posix_rpi2_default: + $(call cmake-build,$@) posix: posix_sitl_default diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake new file mode 100644 index 0000000000..45aefdbf64 --- /dev/null +++ b/cmake/configs/posix_rpi2_default.cmake @@ -0,0 +1,28 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + + systemcmds/param + systemcmds/ver + + modules/mavlink + + modules/param + modules/systemlib + modules/uORB + modules/dataman + + lib/mathlib + lib/mathlib/math/filter + lib/geo + lib/geo_lookup + lib/conversion + + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + +) diff --git a/posix-configs/rpi2/init/rcS_navio b/posix-configs/rpi2/init/rcS_navio new file mode 100644 index 0000000000..6fd1d59db1 --- /dev/null +++ b/posix-configs/rpi2/init/rcS_navio @@ -0,0 +1,71 @@ +uorb start +simulator start -s +param load +param set MAV_TYPE 2 +param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 +param set MC_ROLLRATE_P 0.15 +param set MC_YAW_P 2.8 +param set MC_YAWRATE_P 0.35 +param set SYS_AUTOSTART 4010 +param set SYS_RESTART_TYPE 2 +dataman start +param set CAL_GYRO0_ID 2293768 +param set CAL_ACC0_ID 1376264 +param set CAL_ACC1_ID 1310728 +param set CAL_MAG0_ID 196616 +param set CAL_GYRO0_XOFF 0.01 +param set CAL_ACC0_XOFF 0.01 +param set CAL_ACC0_YOFF -0.01 +param set CAL_ACC0_ZOFF 0.01 +param set CAL_ACC0_XSCALE 1.01 +param set CAL_ACC0_YSCALE 1.01 +param set CAL_ACC0_ZSCALE 1.01 +param set CAL_ACC1_XOFF 0.01 +param set CAL_MAG0_XOFF 0.01 +param set MPC_XY_P 0.4 +param set MPC_XY_VEL_P 0.2 +param set MPC_XY_VEL_D 0.005 +param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set MIS_TAKEOFF_ALT 5.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_HOLD_Z_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 +rgbledsim start +tone_alarm start +gyrosim start +accelsim start +barosim start +adcsim start +gpssim start +pwm_out_sim mode_pwm +sleep 1 +sensors start +commander start +land_detector start multicopter +navigator start +attitude_estimator_q start +position_estimator_inav start +mc_pos_control start +mc_att_control start +mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix +mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard +mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 +mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 +mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 +mavlink stream -r 80 -s ATTITUDE -u 14556 +mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 +mavlink stream -r 20 -s RC_CHANNELS -u 14556 +mavlink stream -r 250 -s HIGHRES_IMU -u 14556 +mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 +mavlink boot_complete +sdlog2 start -r 100 -e -t -a diff --git a/src/lib/version/version.h b/src/lib/version/version.h index 9492cec90f..9c1caf8e83 100644 --- a/src/lib/version/version.h +++ b/src/lib/version/version.h @@ -66,7 +66,12 @@ #ifdef CONFIG_ARCH_BOARD_SITL #define HW_ARCH "LINUXTEST" #endif + #ifdef CONFIG_ARCH_BOARD_EAGLE #define HW_ARCH "LINUXTEST" #endif + +#ifdef CONFIG_ARCH_BOARD_RPI2 +#define HW_ARCH "LINUXTEST" +#endif #endif /* VERSION_H_ */ From 37ffb61afdfd2ad4032592933b8224d714abac52 Mon Sep 17 00:00:00 2001 From: Mohammed Kabir Date: Sun, 17 Jan 2016 16:06:50 +0000 Subject: [PATCH 014/128] changes to rpi2 configs --- Makefile | 3 ++ cmake/configs/posix_rpi2_default.cmake | 55 +++++++++++++------- cmake/configs/posix_rpi2_release.cmake | 47 +++++++++++++++++ posix-configs/rpi2/init/rcS_navio | 71 -------------------------- 4 files changed, 87 insertions(+), 89 deletions(-) create mode 100644 cmake/configs/posix_rpi2_release.cmake diff --git a/Makefile b/Makefile index 6f9fb0f706..25b5fa5dfc 100644 --- a/Makefile +++ b/Makefile @@ -174,6 +174,9 @@ posix_eagle_default: posix_rpi2_default: $(call cmake-build,$@) +posix_rpi2_release: + $(call cmake-build,$@) + posix: posix_sitl_default sitl_deprecation: diff --git a/cmake/configs/posix_rpi2_default.cmake b/cmake/configs/posix_rpi2_default.cmake index 45aefdbf64..c745070c9e 100644 --- a/cmake/configs/posix_rpi2_default.cmake +++ b/cmake/configs/posix_rpi2_default.cmake @@ -4,25 +4,44 @@ set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.c set(config_module_list drivers/device - - systemcmds/param - systemcmds/ver - - modules/mavlink - - modules/param - modules/systemlib - modules/uORB - modules/dataman - - lib/mathlib - lib/mathlib/math/filter - lib/geo - lib/geo_lookup - lib/conversion - platforms/common platforms/posix/px4_layer platforms/posix/work_queue - + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/mavlink + modules/attitude_estimator_q + modules/position_estimator_inav + modules/navigator + modules/vtol_att_control + modules/mc_pos_control + modules/mc_att_control + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 + modules/commander + modules/controllib + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery ) diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake new file mode 100644 index 0000000000..c745070c9e --- /dev/null +++ b/cmake/configs/posix_rpi2_release.cmake @@ -0,0 +1,47 @@ +include(posix/px4_impl_posix) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) + +set(config_module_list + drivers/device + platforms/common + platforms/posix/px4_layer + platforms/posix/work_queue + systemcmds/param + systemcmds/mixer + systemcmds/ver + systemcmds/esc_calib + systemcmds/reboot + systemcmds/topic_listener + systemcmds/perf + modules/uORB + modules/param + modules/systemlib + modules/systemlib/mixer + modules/sensors + modules/mavlink + modules/attitude_estimator_q + modules/position_estimator_inav + modules/navigator + modules/vtol_att_control + modules/mc_pos_control + modules/mc_att_control + modules/land_detector + modules/fw_att_control + modules/fw_pos_control_l1 + modules/dataman + modules/sdlog2 + modules/commander + modules/controllib + lib/mathlib + lib/mathlib/math/filter + lib/conversion + lib/ecl + lib/external_lgpl + lib/geo + lib/geo_lookup + lib/launchdetection + lib/terrain_estimation + lib/runway_takeoff + lib/tailsitter_recovery +) diff --git a/posix-configs/rpi2/init/rcS_navio b/posix-configs/rpi2/init/rcS_navio index 6fd1d59db1..e69de29bb2 100644 --- a/posix-configs/rpi2/init/rcS_navio +++ b/posix-configs/rpi2/init/rcS_navio @@ -1,71 +0,0 @@ -uorb start -simulator start -s -param load -param set MAV_TYPE 2 -param set MC_PITCHRATE_P 0.15 -param set MC_PITCH_P 7 -param set MC_ROLL_P 7 -param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.8 -param set MC_YAWRATE_P 0.35 -param set SYS_AUTOSTART 4010 -param set SYS_RESTART_TYPE 2 -dataman start -param set CAL_GYRO0_ID 2293768 -param set CAL_ACC0_ID 1376264 -param set CAL_ACC1_ID 1310728 -param set CAL_MAG0_ID 196616 -param set CAL_GYRO0_XOFF 0.01 -param set CAL_ACC0_XOFF 0.01 -param set CAL_ACC0_YOFF -0.01 -param set CAL_ACC0_ZOFF 0.01 -param set CAL_ACC0_XSCALE 1.01 -param set CAL_ACC0_YSCALE 1.01 -param set CAL_ACC0_ZSCALE 1.01 -param set CAL_ACC1_XOFF 0.01 -param set CAL_MAG0_XOFF 0.01 -param set MPC_XY_P 0.4 -param set MPC_XY_VEL_P 0.2 -param set MPC_XY_VEL_D 0.005 -param set SENS_BOARD_ROT 0 -param set COM_RC_IN_MODE 1 -param set NAV_ACC_RAD 2.0 -param set RTL_RETURN_ALT 30.0 -param set RTL_DESCEND_ALT 10.0 -param set MIS_TAKEOFF_ALT 5.0 -param set MPC_HOLD_MAX_Z 2.0 -param set MPC_HOLD_XY_DZ 0.1 -param set MPC_HOLD_Z_DZ 0.1 -param set MPC_Z_VEL_MAX 2.0 -param set MPC_Z_VEL_P 0.4 -rgbledsim start -tone_alarm start -gyrosim start -accelsim start -barosim start -adcsim start -gpssim start -pwm_out_sim mode_pwm -sleep 1 -sensors start -commander start -land_detector start multicopter -navigator start -attitude_estimator_q start -position_estimator_inav start -mc_pos_control start -mc_att_control start -mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix -mavlink start -u 14556 -r 2000000 -mavlink start -u 14557 -r 2000000 -m onboard -mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 -mavlink stream -r 80 -s LOCAL_POSITION_NED -u 14556 -mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 -mavlink stream -r 80 -s ATTITUDE -u 14556 -mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 -mavlink stream -r 20 -s RC_CHANNELS -u 14556 -mavlink stream -r 250 -s HIGHRES_IMU -u 14556 -mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 -mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 -mavlink boot_complete -sdlog2 start -r 100 -e -t -a From 37cbb90930d4abf3f562ed0998f5bdf09fbe3d62 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sun, 17 Jan 2016 22:44:20 +0530 Subject: [PATCH 015/128] Crosscompiler support --- cmake/configs/posix_rpi2_release.cmake | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/cmake/configs/posix_rpi2_release.cmake b/cmake/configs/posix_rpi2_release.cmake index c745070c9e..69813fb1f7 100644 --- a/cmake/configs/posix_rpi2_release.cmake +++ b/cmake/configs/posix_rpi2_release.cmake @@ -1,6 +1,15 @@ include(posix/px4_impl_posix) -set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-native.cmake) +if ("${RPI_TOOLCHAIN_DIR}" STREQUAL "") + set(RPI_TOOLCHAIN_DIR /opt/rpi_toolchain) +endif() + +set(CMAKE_PROGRAM_PATH + "${RPI_TOOLCHAIN_DIR}/gcc-linaro-arm-linux-gnueabihf-raspbian/bin" + ${CMAKE_PROGRAM_PATH} + ) + +set(CMAKE_TOOLCHAIN_FILE ${CMAKE_SOURCE_DIR}/cmake/toolchains/Toolchain-arm-linux-gnueabihf.cmake) set(config_module_list drivers/device From 9d239c10e521ef686413daf4a9b5d99487fc7f13 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 18 Jan 2016 23:19:51 +0100 Subject: [PATCH 016/128] Restore MAVLink network behaviour with fixed remote system approach --- 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 412ae33ff0..fbd5751abc 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1049,10 +1049,10 @@ Mavlink::init_udp() /* set default target address, but not for onboard mode (will be set on first received packet) */ memset((char *)&_src_addr, 0, sizeof(_src_addr)); if (_mode != MAVLINK_MODE_ONBOARD) { - set_client_source_initialized(); _src_addr.sin_family = AF_INET; inet_aton("127.0.0.1", &_src_addr.sin_addr); _src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP); + set_client_source_initialized(); } /* default broadcast address */ From 56957e7ee48e5112628a0546dab01e669646e4ab Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 19:26:31 +0100 Subject: [PATCH 017/128] Improved listener tool --- Tools/generate_listener.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index f097cb741b..856ab37062 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -136,12 +136,12 @@ extern "C" __EXPORT int listener_main(int argc, char *argv[]); int listener_main(int argc, char *argv[]) { int sub = -1; orb_id_t ID; - if(argc < 3) { - printf("need at least two arguments: topic name, number of messages to print\\n"); + if(argc < 2) { + printf("need at least two arguments: topic name. [optional number of messages to print]\\n"); return 1; } """) -print("\tunsigned num_msgs = atoi(argv[2]);") +print("\tunsigned num_msgs = (argc > 2) ? atoi(argv[2]) : 1;") print("\tif (strncmp(argv[1],\"%s\",50) == 0) {" % messages[0]) print("\t\tsub = orb_subscribe(ORB_ID(%s));" % messages[0]) print("\t\tID = ORB_ID(%s);" % messages[0]) @@ -154,9 +154,11 @@ for index,m in enumerate(messages[1:]): print("\t\tstruct %s_s container;" % m) print("\t\tmemset(&container, 0, sizeof(container));") print("\t\tbool updated;") - print("\t\tfor (unsigned i = 0; i < num_msgs; i++) {") + print("\t\tint i = 0;") + print("\t\twhile(i < num_msgs) {") print("\t\t\torb_check(sub,&updated);") - print("\t\t\tupdated = true;") + print("\t\t\tif (i == 0) { updated = true; } else { usleep(500); }") + print("\t\t\ti++;") print("\t\t\tif (updated) {") print("\t\tprintf(\"\\nTOPIC: %s #%%d\\n\", i);" % m) print("\t\t\torb_copy(ID,sub,&container);") From d1cf8df2ad3be35e81a81820ed3995d90f0f5741 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 19:26:50 +0100 Subject: [PATCH 018/128] Initialize system power struct --- src/drivers/stm32/adc/adc.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp index 9c6e60947a..2b34f360d4 100644 --- a/src/drivers/stm32/adc/adc.cpp +++ b/src/drivers/stm32/adc/adc.cpp @@ -315,13 +315,13 @@ void ADC::update_system_power(void) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 - system_power_s system_power; + system_power_s system_power = {}; system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; for (unsigned i = 0; i < _channel_count; i++) { - if (_samples[i].am_channel == 4) { + if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { // it is 2:1 scaled system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); } @@ -349,13 +349,13 @@ ADC::update_system_power(void) #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V4 - system_power_s system_power; + system_power_s system_power = {}; system_power.timestamp = hrt_absolute_time(); system_power.voltage5V_v = 0; for (unsigned i = 0; i < _channel_count; i++) { - if (_samples[i].am_channel == 4) { + if (_samples[i].am_channel == ADC_5V_RAIL_SENSE) { // it is 2:1 scaled system_power.voltage5V_v = _samples[i].am_data * (6.6f / 4096); } From cd37ffd0bf244fd1f837d1823ff8dbcb6fdf339e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 19:27:05 +0100 Subject: [PATCH 019/128] Do not sample pressure ADC if not configured --- src/modules/sensors/sensors.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 2ecb011939..fb09885e22 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1692,6 +1692,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _battery_current_timestamp = t; +#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ @@ -1720,6 +1721,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); } } +#endif } } From a76ecf38219fc78825c81bc3835ef31422b12597 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 19:27:21 +0100 Subject: [PATCH 020/128] Remove differential pressure ADC --- src/drivers/boards/px4fmu-v4/board_config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index 062f817998..b8fa362f6e 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -160,7 +160,6 @@ __BEGIN_DECLS #define ADC_BATTERY_VOLTAGE_CHANNEL 2 #define ADC_BATTERY_CURRENT_CHANNEL 3 #define ADC_5V_RAIL_SENSE 4 -#define ADC_AIRSPEED_VOLTAGE_CHANNEL 15 /* User GPIOs * From 751a95deb84d6fd5a948431724057bfdfc6d233c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 19:27:36 +0100 Subject: [PATCH 021/128] Commander: Operate blue led on pixracer --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 19562ccb6e..45b43b3d9a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2723,7 +2723,7 @@ control_status_leds(vehicle_status_s *status_local, const actuator_armed_s *actu } } -#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1 +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V1) || defined (CONFIG_ARCH_BOARD_PX4FMU_V4) /* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */ if (actuator_armed->armed) { From 0025ab7258d0358102df81435507e2f551915ba8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 19 Jan 2016 22:00:20 +0100 Subject: [PATCH 022/128] Sensors: Code style --- src/modules/sensors/sensors.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index fb09885e22..5c41857732 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1693,6 +1693,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _battery_current_timestamp = t; #ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL + } else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) { /* calculate airspeed, raw is the difference from */ @@ -1721,6 +1722,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw) _diff_pres_pub = orb_advertise(ORB_ID(differential_pressure), &_diff_pres); } } + #endif } } From 92c946dae1c786b3c3d470b9d96a7f66387bdb8e Mon Sep 17 00:00:00 2001 From: sander Date: Tue, 19 Jan 2016 21:15:06 +0100 Subject: [PATCH 023/128] New default PID's for QuadRanger --- ROMFS/px4fmu_common/init.d/13008_QuadRanger | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/13008_QuadRanger b/ROMFS/px4fmu_common/init.d/13008_QuadRanger index 16b701c596..404d0acd13 100644 --- a/ROMFS/px4fmu_common/init.d/13008_QuadRanger +++ b/ROMFS/px4fmu_common/init.d/13008_QuadRanger @@ -18,13 +18,13 @@ then param set PWM_AUX_REV1 1 param set PWM_AUX_REV2 1 - param set MC_ROLL_P 6.5 - param set MC_ROLLRATE_P 0.1 + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.25 param set MC_ROLLRATE_I 0.002 param set MC_ROLLRATE_D 0.003 param set MC_ROLLRATE_FF 0.0 - param set MC_PITCH_P 6.5 - param set MC_PITCHRATE_P 0.1 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.23 param set MC_PITCHRATE_I 0.002 param set MC_PITCHRATE_D 0.003 param set MC_PITCHRATE_FF 0.0 From cbde246f0a02afb771cccc5e61faed5f7115e22a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 10:08:40 +0100 Subject: [PATCH 024/128] FMUv4: Remove non-existent airspeed sensing channel --- src/drivers/boards/px4fmu-v4/board_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index b8fa362f6e..ea82a7a031 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -154,7 +154,7 @@ __BEGIN_DECLS * * These are the channel numbers of the ADCs of the microcontroller that can be used by the Px4 Firmware in the adc driver */ -#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) | (1 << 15) +#define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) // ADC defines to be used in sensors.cpp to read from a particular channel #define ADC_BATTERY_VOLTAGE_CHANNEL 2 From 3c401c396c690228987df354a7a50bd46a12d790 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 10:47:50 +0100 Subject: [PATCH 025/128] Adjust to AUAV screwup on voltage / current pins --- src/drivers/boards/px4fmu-v4/board_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index ea82a7a031..dcf21e2566 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -157,8 +157,8 @@ __BEGIN_DECLS #define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) // ADC defines to be used in sensors.cpp to read from a particular channel -#define ADC_BATTERY_VOLTAGE_CHANNEL 2 -#define ADC_BATTERY_CURRENT_CHANNEL 3 +#define ADC_BATTERY_VOLTAGE_CHANNEL 3 +#define ADC_BATTERY_CURRENT_CHANNEL 2 #define ADC_5V_RAIL_SENSE 4 /* User GPIOs From 83e45a15643b07427fa25d2d812cc3831bb6324c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 10:59:25 +0100 Subject: [PATCH 026/128] Re-instate UDP handling for MAVLink app --- 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 4efeefb180..3b4b3f588a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1817,8 +1817,8 @@ MavlinkReceiver::receive_thread(void *arg) if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him - //memcpy(srcaddr_last, &srcaddr, sizeof(*srcaddr_last)); - //PX4_WARN("XXX: not really updating: UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr)); + srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; + srcaddr_last->sin_port = srcaddr.sin_port; _mavlink->set_client_source_initialized(); } #endif From b8f11dee9994ee21e177a5faeaaaee28e347e3c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 10:59:39 +0100 Subject: [PATCH 027/128] MAVLink: Code style --- src/modules/mavlink/mavlink_main.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 530eb760b2..8d0d8dc05b 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -339,7 +339,7 @@ public: int get_socket_fd () { return _socket_fd; }; #ifdef __PX4_POSIX - struct sockaddr_in * get_client_source_address() {return &_src_addr;}; + struct sockaddr_in * get_client_source_address() {return &_src_addr;}; void set_client_source_initialized() { _src_addr_initialized = true; }; From a49dbbc9a8207999b252ddca5210d35348f83afa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 11:01:31 +0100 Subject: [PATCH 028/128] Lister: Fix Clang compile error --- Tools/generate_listener.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index 856ab37062..be6f533450 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -154,7 +154,7 @@ for index,m in enumerate(messages[1:]): print("\t\tstruct %s_s container;" % m) print("\t\tmemset(&container, 0, sizeof(container));") print("\t\tbool updated;") - print("\t\tint i = 0;") + print("\t\tunsigned i = 0;") print("\t\twhile(i < num_msgs) {") print("\t\t\torb_check(sub,&updated);") print("\t\t\tif (i == 0) { updated = true; } else { usleep(500); }") From 883148d3d14a8b13e6b1c9fdd11f58db280a28d7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 11:13:24 +0100 Subject: [PATCH 029/128] Revert "Adjust to AUAV screwup on voltage / current pins" This reverts commit 3c401c396c690228987df354a7a50bd46a12d790. --- src/drivers/boards/px4fmu-v4/board_config.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/board_config.h b/src/drivers/boards/px4fmu-v4/board_config.h index dcf21e2566..ea82a7a031 100644 --- a/src/drivers/boards/px4fmu-v4/board_config.h +++ b/src/drivers/boards/px4fmu-v4/board_config.h @@ -157,8 +157,8 @@ __BEGIN_DECLS #define ADC_CHANNELS (1 << 2) | (1 << 3) | (1 << 4) | (1 << 10) | (1 << 11) | (1 << 12) | (1 << 13) | (1 << 14) // ADC defines to be used in sensors.cpp to read from a particular channel -#define ADC_BATTERY_VOLTAGE_CHANNEL 3 -#define ADC_BATTERY_CURRENT_CHANNEL 2 +#define ADC_BATTERY_VOLTAGE_CHANNEL 2 +#define ADC_BATTERY_CURRENT_CHANNEL 3 #define ADC_5V_RAIL_SENSE 4 /* User GPIOs From 619548b10a7e64995c731ec4e8fc73f67286b5f7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 11:53:44 +0100 Subject: [PATCH 030/128] MAVLink: Send for the first 4 seconds to localhost on UDP to not interfere with the network --- src/modules/mavlink/mavlink_main.cpp | 6 ++++++ src/modules/mavlink/mavlink_main.h | 1 + 2 files changed, 7 insertions(+) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index fbd5751abc..bd9f653c58 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -169,6 +169,7 @@ Mavlink::Mavlink() : _flow_control_enabled(true), _last_write_success_time(0), _last_write_try_time(0), + _mavlink_start_time(0), _bytes_tx(0), _bytes_txerr(0), _bytes_rx(0), @@ -886,6 +887,10 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID _last_write_try_time = hrt_absolute_time(); + if (_mavlink_start_time == 0) { + _mavlink_start_time = _last_write_try_time; + } + if (get_protocol() == SERIAL) { /* check if there is space in the buffer, let it overflow else */ unsigned buf_free = get_free_tx_buf(); @@ -938,6 +943,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID /* resend heartbeat via broadcast */ if ((_mode != MAVLINK_MODE_ONBOARD) + && (_mavlink_start_time > 0 && (hrt_elapsed_time(&_mavlink_start_time) > 4 * 1000 * 1000)) && (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) || (tstatus.heartbeat_time == 0)) && msgid == MAVLINK_MSG_ID_HEARTBEAT)) { diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index 8d0d8dc05b..5e5cc34649 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -416,6 +416,7 @@ private: bool _flow_control_enabled; uint64_t _last_write_success_time; uint64_t _last_write_try_time; + uint64_t _mavlink_start_time; unsigned _bytes_tx; unsigned _bytes_txerr; From afb01e6d9aab2e7feb32125948d55d0066565a9e Mon Sep 17 00:00:00 2001 From: tumbili Date: Wed, 20 Jan 2016 13:46:50 +0100 Subject: [PATCH 031/128] fix level horizon feature: allow attitude to settle for a while if changes to the board rotation parameters are done --- src/modules/commander/accelerometer_calibration.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index dd6d5e3a9d..488f662e11 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -595,7 +595,8 @@ int do_level_calibration(int mavlink_fd) { param_get(pitch_offset_handle, &pitch_offset_current); param_get(board_rot_handle, &board_rot_current); - if (board_rot_current == 0) { + // give attitude some time to settle if there have been changes to the board rotation parameters + if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON ) { settle_time = 0; } From 4ce5d4e3e3f3175637aa9c69ac6ba04ae87e3c5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 20 Jan 2016 23:54:01 +0100 Subject: [PATCH 032/128] Make LPE_ENABLED available as default --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5c41857732..f41c9dcf4b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -662,6 +662,7 @@ Sensors::Sensors() : (void)param_find("PWM_AUX_DISARMED"); (void)param_find("TRIG_MODE"); (void)param_find("UAVCAN_ENABLE"); + (void)param_find("LPE_ENABLED"); /* fetch initial parameter values */ parameters_update(); From e6d2d1710970300249ac04fe6892bce943d16f69 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 21 Jan 2016 07:25:18 +0100 Subject: [PATCH 033/128] initialise topic structs properly --- src/modules/ekf2/ekf2_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 22fce7ec6f..5ba1b5b6c6 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -315,7 +315,7 @@ void Ekf2::task_main() _ekf->update(); // generate vehicle attitude data - struct vehicle_attitude_s att; + struct vehicle_attitude_s att = {}; att.timestamp = hrt_absolute_time(); _ekf->copy_quaternion(att.q); @@ -326,7 +326,7 @@ void Ekf2::task_main() att.yaw = euler(2); // generate vehicle local position data - struct vehicle_local_position_s lpos; + struct vehicle_local_position_s lpos = {}; float pos[3] = {}; float vel[3] = {}; @@ -421,7 +421,7 @@ void Ekf2::task_main() } // generate and publish global position data - struct vehicle_global_position_s global_pos; + struct vehicle_global_position_s global_pos = {}; if (_ekf->position_is_valid()) { // TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator From f918b0c99237590ba92cc08a22b55cefdd79ce34 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Jan 2016 09:34:54 +0100 Subject: [PATCH 034/128] Uploader: Make sure to warn about wrong board type --- Tools/px_uploader.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py index 4755eb24ff..596c967407 100755 --- a/Tools/px_uploader.py +++ b/Tools/px_uploader.py @@ -449,8 +449,9 @@ class uploader(object): if self.board_type != fw.property('board_id'): msg = "Firmware not suitable for this board (board_type=%u board_id=%u)" % ( self.board_type, fw.property('board_id')) + print("WARNING: %s" % msg) if args.force: - print("WARNING: %s" % msg) + print("FORCED WRITE, FLASHING ANYWAY!") else: raise IOError(msg) if self.fw_maxsize < fw.property('image_size'): From 7817924aeff4862b89356216df84a021fb9443b3 Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 17 Jan 2016 11:51:07 +0100 Subject: [PATCH 035/128] multirotor takeoff: instead of altering the velocity setpoint for the vehicle to takeoff use the thrust setpoint directly. this does not depend on the tuning of the velocity loop. --- .../mc_pos_control/mc_pos_control_main.cpp | 56 ++++++++++++++----- 1 file changed, 42 insertions(+), 14 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 3bc8f9797e..bf5f4e20d9 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -250,6 +250,9 @@ private: bool _takeoff_jumped; float _vel_z_lp; float _acc_z_lp; + float _takeoff_thrust_sp; + hrt_abstime _takeoff_start_time; + bool _started_takeoff; /** * Update our local parameter cache. @@ -378,7 +381,10 @@ MulticopterPositionControl::MulticopterPositionControl() : _landing_started(0), _takeoff_jumped(false), _vel_z_lp(0), - _acc_z_lp(0) + _acc_z_lp(0), + _takeoff_thrust_sp(0.0f), + _takeoff_start_time(0), + _started_takeoff(false) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -1358,25 +1364,32 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (!_takeoff_jumped) { - /* do a quick jump (ramp vel sp up in 1/2sec) until we shoot over takeoff speed */ - float accel = _vel_sp_prev(2) - _params.tko_jmpspd; - float tmp = _vel_sp_prev(2) + accel * dt * 2.0f; - _vel_sp(2) = math::max(tmp, -_params.tko_jmpspd); - - if (_vel(2) < -_params.tko_speed) { + if (!_started_takeoff) { + _started_takeoff = true; + _takeoff_start_time = hrt_absolute_time(); + } else if (!_takeoff_jumped) { + // ramp thrust setpoint up + if (_vel(2) > -0.5f) { + _takeoff_thrust_sp = (float)hrt_elapsed_time(&_takeoff_start_time) / 1e6f * 0.5f; + _vel_sp.zero(); + _vel_prev.zero(); + } else { + // copter has reached our takeoff speed. split the thrust setpoint up + // into an integral part and into a P part + thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); + thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); + _vel_sp(2) = -1.0f; + _vel_sp_prev(2) = -1.0f; _takeoff_jumped = true; + reset_int_z = false; } - - } else { - /* slowly reduce forced takeoff speed to set climb rate (in 3 sec) */ - float decel = _params.tko_jmpspd - _params.tko_speed; - float tmp = decel / 3.0f * dt + _vel_sp_prev(2); - _vel_sp(2) = math::min(tmp, -_params.tko_speed); } } else { _takeoff_jumped = false; + _started_takeoff = false; + _takeoff_start_time = 0; + _takeoff_thrust_sp = 0.0f; } // limit total horizontal acceleration @@ -1393,6 +1406,14 @@ MulticopterPositionControl::task_main() _vel_sp(1) = vel_sp_hor(1); } + // limit vertical acceleration + float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt; + + if (fabsf(acc_v) > 2 * _params.acc_hor_max) { + acc_v /= fabsf(acc_v); + _vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2); + } + _vel_sp_prev = _vel_sp; _global_vel_sp.vx = _vel_sp(0); @@ -1450,6 +1471,13 @@ MulticopterPositionControl::task_main() // TODO?: + _vel_sp.emult(_params.vel_ff) math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; + if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF + && !_takeoff_jumped) { + // for jumped takeoffs use special thrust setpoint calculated above + thrust_sp.zero(); + thrust_sp(2) = -_takeoff_thrust_sp; + } + if (!_control_mode.flag_control_velocity_enabled) { thrust_sp(0) = 0.0f; thrust_sp(1) = 0.0f; From 461451147443356aaca7ad0473e41991607a9092 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Wed, 20 Jan 2016 21:38:42 +0100 Subject: [PATCH 036/128] use set takeoff speed --- .../mc_pos_control/mc_pos_control_main.cpp | 28 ++++++------------- .../mc_pos_control/mc_pos_control_params.c | 9 ------ 2 files changed, 9 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 bf5f4e20d9..15fc446f44 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -180,7 +180,6 @@ private: param_t xy_ff; param_t tilt_max_air; param_t land_speed; - param_t tko_jmpspd; param_t tko_speed; param_t tilt_max_land; param_t man_roll_max; @@ -199,7 +198,6 @@ private: float thr_max; float tilt_max_air; float land_speed; - float tko_jmpspd; float tko_speed; float tilt_max_land; float man_roll_max; @@ -251,8 +249,6 @@ private: float _vel_z_lp; float _acc_z_lp; float _takeoff_thrust_sp; - hrt_abstime _takeoff_start_time; - bool _started_takeoff; /** * Update our local parameter cache. @@ -382,9 +378,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _takeoff_jumped(false), _vel_z_lp(0), _acc_z_lp(0), - _takeoff_thrust_sp(0.0f), - _takeoff_start_time(0), - _started_takeoff(false) + _takeoff_thrust_sp(0.0f) { memset(&_vehicle_status, 0, sizeof(_vehicle_status)); memset(&_ctrl_state, 0, sizeof(_ctrl_state)); @@ -435,7 +429,6 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.xy_ff = param_find("MPC_XY_FF"); _params_handles.tilt_max_air = param_find("MPC_TILTMAX_AIR"); _params_handles.land_speed = param_find("MPC_LAND_SPEED"); - _params_handles.tko_jmpspd = param_find("MPC_TKO_JMPSPD"); _params_handles.tko_speed = param_find("MPC_TKO_SPEED"); _params_handles.tilt_max_land = param_find("MPC_TILTMAX_LND"); _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); @@ -499,7 +492,6 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.tilt_max_air, &_params.tilt_max_air); _params.tilt_max_air = math::radians(_params.tilt_max_air); param_get(_params_handles.land_speed, &_params.land_speed); - param_get(_params_handles.tko_jmpspd, &_params.tko_jmpspd); param_get(_params_handles.tko_speed, &_params.tko_speed); param_get(_params_handles.tilt_max_land, &_params.tilt_max_land); _params.tilt_max_land = math::radians(_params.tilt_max_land); @@ -1364,13 +1356,10 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { - if (!_started_takeoff) { - _started_takeoff = true; - _takeoff_start_time = hrt_absolute_time(); - } else if (!_takeoff_jumped) { + if (!_takeoff_jumped) { // ramp thrust setpoint up - if (_vel(2) > -0.5f) { - _takeoff_thrust_sp = (float)hrt_elapsed_time(&_takeoff_start_time) / 1e6f * 0.5f; + if (_vel(2) > -(_params.tko_speed / 2.0f)) { + _takeoff_thrust_sp += 0.5f * dt; _vel_sp.zero(); _vel_prev.zero(); } else { @@ -1378,17 +1367,18 @@ MulticopterPositionControl::task_main() // into an integral part and into a P part thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); - _vel_sp(2) = -1.0f; - _vel_sp_prev(2) = -1.0f; _takeoff_jumped = true; reset_int_z = false; } } + if (_takeoff_jumped) { + _vel_sp(2) = -_params.tko_speed; + _vel_sp_prev(2) = -_params.tko_speed; + } + } else { _takeoff_jumped = false; - _started_takeoff = false; - _takeoff_start_time = 0; _takeoff_thrust_sp = 0.0f; } diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 80792304d9..1ddf9114e0 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -234,15 +234,6 @@ PARAM_DEFINE_FLOAT(MPC_TILTMAX_LND, 12.0f); */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 0.5f); -/** - * Takeoff initial speed to jump off the ground - * - * @unit m/s - * @min 0.0 - * @group Multicopter Position Control - */ -PARAM_DEFINE_FLOAT(MPC_TKO_JMPSPD, 2.0f); - /** * Takeoff climb rate * From 9c6f4de753e10c30fa6fa057e293ea3d80030bf0 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 Jan 2016 13:00:44 -1000 Subject: [PATCH 037/128] Fixes buffer overwrite on CONFIG_ARCH_BOARD_AEROCORE --- src/drivers/px4fmu/fmu.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index e017a2388d..7c125d75a1 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -1717,7 +1717,7 @@ ssize_t PX4FMU::write(file *filp, const char *buffer, size_t len) { unsigned count = len / 2; - uint16_t values[6]; + uint16_t values[8]; #ifdef CONFIG_ARCH_BOARD_AEROCORE From 19b81b9ab26e4576450d6cb946ee06a127b2c6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:31:39 +0100 Subject: [PATCH 038/128] Commander: Rate-limit preflight check --- .../commander/state_machine_helper.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 5163087830..db151fbe5c 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -97,6 +97,9 @@ static const char * const state_names[vehicle_status_s::ARMING_STATE_MAX] = { "ARMING_STATE_IN_AIR_RESTORE", }; +static hrt_abstime last_preflight_check = 0; ///< initialize so it gets checked immediately +static int last_prearm_ret = 1; ///< initialize to fail + transition_result_t arming_state_transition(struct vehicle_status_s *status, ///< current vehicle status const struct safety_s *safety, ///< current safety settings @@ -132,12 +135,18 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s } /* re-run the pre-flight check as long as sensors are failing */ if (!status->condition_system_sensors_initialized - && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED || - new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) - && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { + && (new_arming_state == vehicle_status_s::ARMING_STATE_ARMED + || new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY) + && status->hil_state == vehicle_status_s::HIL_STATE_OFF) { - prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); - status->condition_system_sensors_initialized = !prearm_ret; + if (last_preflight_check == 0 || hrt_absolute_time() - last_preflight_check > 1000 * 1000) { + prearm_ret = preflight_check(status, mavlink_fd, false /* pre-flight */); + status->condition_system_sensors_initialized = !prearm_ret; + last_preflight_check = hrt_absolute_time(); + last_prearm_ret = prearm_ret; + } else { + prearm_ret = last_prearm_ret; + } } /* From 85c49ff64251c67879f4a9bfa1cedc71d8d65a77 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:33:40 +0100 Subject: [PATCH 039/128] Commander: Do not save params on already saved param update --- msg/parameter_update.msg | 1 + src/modules/commander/commander.cpp | 19 ++++++++++--------- 2 files changed, 11 insertions(+), 9 deletions(-) diff --git a/msg/parameter_update.msg b/msg/parameter_update.msg index 39dc336e05..78cebf7f4f 100644 --- a/msg/parameter_update.msg +++ b/msg/parameter_update.msg @@ -1 +1,2 @@ uint64 timestamp # time at which the latest parameter was updated +bool saved # wether the change has already been saved to disk diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 45b43b3d9a..767b13367e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1509,12 +1509,12 @@ int commander_thread_main(int argc, char *argv[]) /* EPH / EPV */ param_get(_param_eph, &eph_threshold); param_get(_param_epv, &epv_threshold); - } - /* Set flag to autosave parameters if necessary */ - if (updated && autosave_params != 0) { - /* trigger an autosave */ - need_param_autosave = true; + /* Set flag to autosave parameters if necessary */ + if (updated && autosave_params != 0 && param_changed.saved == false) { + /* trigger an autosave */ + need_param_autosave = true; + } } orb_check(sp_man_sub, &updated); @@ -1570,11 +1570,12 @@ int commander_thread_main(int argc, char *argv[]) /* and the system is not already armed (and potentially flying) */ !armed.armed) { - bool chAirspeed = false; - bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; - + bool chAirspeed = false; + bool hotplug_timeout = hrt_elapsed_time(&commander_boot_timestamp) > HOTPLUG_SENS_TIMEOUT; + /* Perform airspeed check only if circuit breaker is not - * engaged and it's not a rotary wing */ + * engaged and it's not a rotary wing + */ if (!status.circuit_breaker_engaged_airspd_check && !status.is_rotary_wing) { chAirspeed = true; } From 3a43038583c1b863fd85b3043afbc6208406bf3a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:35:56 +0100 Subject: [PATCH 040/128] Params: Provide set and save API --- src/modules/systemlib/param/param.c | 29 +++++++++++++++++++---------- src/modules/systemlib/param/param.h | 10 ++++++++++ 2 files changed, 29 insertions(+), 10 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index f606f20702..dd5c30b667 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -235,9 +235,9 @@ param_find_changed(param_t param) } static void -param_notify_changes(void) +param_notify_changes(bool is_saved) { - struct parameter_update_s pup = { .timestamp = hrt_absolute_time() }; + struct parameter_update_s pup = { .timestamp = hrt_absolute_time(), .saved = is_saved}; /* * If we don't have a handle to our topic, create one now; otherwise @@ -497,7 +497,7 @@ param_get(param_t param, void *val) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool autosave) { int result = -1; bool params_changed = false; @@ -575,7 +575,7 @@ out: * a thing has been set. */ if (params_changed && notify_changes) { - param_notify_changes(); + param_notify_changes(autosave); } return result; @@ -584,13 +584,19 @@ out: int param_set(param_t param, const void *val) { - return param_set_internal(param, val, false, true); + return param_set_internal(param, val, false, true, false); +} + +int +param_set_no_autosave(param_t param, const void *val) +{ + return param_set_internal(param, val, false, true, true); } int param_set_no_notification(param_t param, const void *val) { - return param_set_internal(param, val, false, false); + return param_set_internal(param, val, false, false, false); } bool @@ -643,7 +649,7 @@ param_reset(param_t param) param_unlock(); if (s != NULL) { - param_notify_changes(); + param_notify_changes(false); } return (!param_found); @@ -663,7 +669,7 @@ param_reset_all(void) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } void @@ -695,7 +701,7 @@ param_reset_excludes(const char *excludes[], int num_excludes) param_unlock(); - param_notify_changes(); + param_notify_changes(false); } static const char *param_default_file = PX4_ROOTFSDIR"/eeprom/parameters"; @@ -851,6 +857,9 @@ param_export(int fd, bool only_unsaved) debug("unrecognized parameter type"); goto out; } + + /* allow this process to be interrupted by another process / thread */ + usleep(5); } result = 0; @@ -955,7 +964,7 @@ param_import_callback(bson_decoder_t decoder, void *private, bson_node_t node) goto out; } - if (param_set_internal(param, v, state->mark_saved, true)) { + if (param_set_internal(param, v, state->mark_saved, true, false)) { debug("error setting value for '%s'", node->name); goto out; } diff --git a/src/modules/systemlib/param/param.h b/src/modules/systemlib/param/param.h index f8334361f1..8c49a0b01c 100644 --- a/src/modules/systemlib/param/param.h +++ b/src/modules/systemlib/param/param.h @@ -211,6 +211,16 @@ __EXPORT int param_get(param_t param, void *val); */ __EXPORT int param_set(param_t param, const void *val); +/** + * Set the value of a parameter, but do not trigger an auto-save + * + * @param param A handle returned by param_find or passed by param_foreach. + * @param val The value to set; assumed to point to a variable of the parameter type. + * For structures, the pointer is assumed to point to a structure to be copied. + * @return Zero if the parameter's value could be set from a scalar, nonzero otherwise. + */ +__EXPORT int param_set_no_autosave(param_t param, const void *val); + /** * Set the value of a parameter, but do not notify the system about the change. * From 15b72045ceda4db2bc76c1fb2366cdb6a55d02d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:36:14 +0100 Subject: [PATCH 041/128] Param command: Auto-save after set --- src/systemcmds/param/param.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/param/param.c b/src/systemcmds/param/param.c index 88f5197d27..19be8fa0d6 100644 --- a/src/systemcmds/param/param.c +++ b/src/systemcmds/param/param.c @@ -448,7 +448,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %ld", (long)i); - param_set(param, &newval); + param_set_no_autosave(param, &newval); printf(" -> new: %ld\n", (long)newval); } } @@ -470,7 +470,7 @@ do_set(const char *name, const char *val, bool fail_on_not_found) param_value_unsaved(param) ? '*' : (param_value_is_default(param) ? ' ' : '+'), param_name(param)); printf("curr: %4.4f", (double)f); - param_set(param, &newval); + param_set_no_autosave(param, &newval); printf(" -> new: %4.4f\n", (double)newval); } @@ -483,7 +483,13 @@ do_set(const char *name, const char *val, bool fail_on_not_found) return 1; } - return 0; + if (param_save_default()) { + warnx("Param export failed."); + return 1; + + } else { + return 0; + } } static int From b6cf1b54f98f96305e14d1ba27dfcf59a3f44923 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 22 Jan 2016 10:52:44 +0100 Subject: [PATCH 042/128] ecl ekf2: added default parameter values --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index a11709956f..86df68e404 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit a11709956fd0d082d4fde4da0ad8c79117a38990 +Subproject commit 86df68e404d67268ff9004bb1e10eef218335e73 From 93a9032f87176a6a5e4fb18e5ee927b42997298b Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 22 Jan 2016 10:53:16 +0100 Subject: [PATCH 043/128] ekf2: parameter cleanup --- src/modules/ekf2/ekf2_main.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 5ba1b5b6c6..28b27733d2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -72,7 +72,6 @@ #include #include #include -#include #include @@ -162,9 +161,6 @@ private: EstimatorBase *_ekf; - - void update_parameters(bool force); - int update_subscriptions(); }; From c32938d2a82fecd1e9c59be5327b1cb941aaa89e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:43:38 +0100 Subject: [PATCH 044/128] EKF2: Update params only as they change --- src/modules/ekf2/ekf2_main.cpp | 32 +++++++++++++++++++++++--------- 1 file changed, 23 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 28b27733d2..6c5793ecf0 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -83,7 +83,7 @@ class Ekf2; namespace ekf2 { -Ekf2 *instance; +Ekf2 *instance = nullptr; } @@ -123,6 +123,7 @@ private: int _sensors_sub = -1; int _gps_sub = -1; int _airspeed_sub = -1; + int _params_sub = -1; orb_advert_t _att_pub; orb_advert_t _lpos_pub; @@ -167,14 +168,15 @@ private: Ekf2::Ekf2(): SuperBlock(NULL, "EKF"), + _att_pub(nullptr), + _lpos_pub(nullptr), + _control_state_pub(nullptr), + _vehicle_global_position_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), - _lp_yaw_rate(250.0f, 20.0f) + _lp_yaw_rate(250.0f, 20.0f), + _ekf(new Ekf()) { - _ekf = new Ekf(); - _att_pub = nullptr; - _lpos_pub = nullptr; - _control_state_pub = nullptr; parameters *params = _ekf->getParamHandle(); _mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, ¶ms->mag_delay_ms); @@ -224,18 +226,22 @@ void Ekf2::task_main() { // subscribe to relevant topics _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); + _params_sub = orb_subscribe(ORB_ID(sensor_combined)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _params_sub = orb_subscribe(ORB_ID(parameter_update)); - px4_pollfd_struct_t fds[1]; + px4_pollfd_struct_t fds[2] = {}; fds[0].fd = _sensors_sub; fds[0].events = POLLIN; + fds[1].fd = _params_sub; + fds[1].events = POLLIN; // initialise parameter cache updateParams(); while (!_task_should_exit) { - int ret = px4_poll(fds, 1, 1000); + int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 8); if (ret < 0) { // Poll error, sleep and try again @@ -247,7 +253,15 @@ void Ekf2::task_main() continue; } - updateParams(); + if (fds[1].revents & POLLIN) { + // read from param to clear updated flag + struct parameter_update_s update; + orb_copy(ORB_ID(parameter_update), _params_sub, &update); + updateParams(); + + // fetch sensor data in next loop + continue; + } bool gps_updated = false; bool airspeed_updated = false; From 7a8adaa59155623a04c4a8482e7288ed30aececc Mon Sep 17 00:00:00 2001 From: Roman Date: Sun, 17 Jan 2016 13:25:16 +0100 Subject: [PATCH 045/128] multirotor landinging sudden fall protection: remove condition which made activation of the protection very unlikely --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 3 --- 1 file changed, 3 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 15fc446f44..19f154b699 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1512,7 +1512,6 @@ MulticopterPositionControl::task_main() _landing_started = hrt_absolute_time(); } - #if 0 // TODO quick fix: remove this since in combination with the non-working fall detection // it can lead to the copter falling out of the sky /* don't let it throttle up again during landing */ @@ -1523,7 +1522,6 @@ MulticopterPositionControl::task_main() && hrt_elapsed_time(&_landing_started) > 15e5) { _landing_thrust = thrust_abs; } - #endif /* assume ground, reduce thrust */ if (hrt_elapsed_time(&_landing_started) > 15e5 @@ -1535,7 +1533,6 @@ MulticopterPositionControl::task_main() /* if we suddenly fall, reset landing logic and remove thrust limit */ if (hrt_elapsed_time(&_landing_started) > 15e5 - && _landing_thrust < FLT_EPSILON /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ && (_acc_z_lp > 4.0f || _vel_z_lp > 2.0f * _params.land_speed) From 9e2dd7aab6c96d03b0ab455b5b764c70ce2ee0dc Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 21 Jan 2016 00:41:52 +0100 Subject: [PATCH 046/128] landing without thrust limiting --- .../mc_pos_control/mc_pos_control_main.cpp | 41 ++++++------------- 1 file changed, 13 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 19f154b699..966a930c2c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -243,8 +243,7 @@ private: math::Matrix<3, 3> _R; /**< rotation matrix from attitude quaternions */ float _yaw; /**< yaw angle (euler) */ - float _landing_thrust; - hrt_abstime _landing_started; + bool _in_landing; bool _takeoff_jumped; float _vel_z_lp; float _acc_z_lp; @@ -373,8 +372,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _run_pos_control(true), _run_alt_control(true), _yaw(0.0f), - _landing_thrust(1.0f), - _landing_started(0), + _in_landing(false), _takeoff_jumped(false), _vel_z_lp(0), _acc_z_lp(0), @@ -1508,45 +1506,32 @@ MulticopterPositionControl::task_main() thr_min = 0.0f; } - if (_landing_started == 0) { - _landing_started = hrt_absolute_time(); - } - - // TODO quick fix: remove this since in combination with the non-working fall detection - // it can lead to the copter falling out of the sky - /* don't let it throttle up again during landing */ - if (thrust_sp(2) < 0.0f && thrust_abs < _landing_thrust - /* fix landing thrust after a certain time when velocity change is minimal */ + /* descent stabilized, we're in landing */ + if (!_in_landing && (float)fabs(_acc_z_lp) < 0.1f - && _vel_z_lp > 0.5f * _params.land_speed - && hrt_elapsed_time(&_landing_started) > 15e5) { - _landing_thrust = thrust_abs; + && _vel_z_lp > 0.5f * _params.land_speed) { + _in_landing = true; } /* assume ground, reduce thrust */ - if (hrt_elapsed_time(&_landing_started) > 15e5 - && _landing_thrust > FLT_EPSILON + if (_in_landing && _vel_z_lp < 0.1f ) { - _landing_thrust = 0.0f; + thr_max = 0.0f; } /* if we suddenly fall, reset landing logic and remove thrust limit */ - if (hrt_elapsed_time(&_landing_started) > 15e5 + if (_in_landing /* XXX: magic value, assuming free fall above 4m/s2 acceleration */ && (_acc_z_lp > 4.0f - || _vel_z_lp > 2.0f * _params.land_speed) + || _vel_z_lp > 2.0f * _params.land_speed) ) { - _landing_thrust = _params.thr_max; - _landing_started = 0; + thr_max = _params.thr_max; + _in_landing = false; } - /* add 5% to give it some margin to compensate external influences */ - thr_max = _landing_thrust + 0.05f * _landing_thrust; - } else { - _landing_started = 0; - _landing_thrust = _params.thr_max; + _in_landing = false; } /* limit min lift */ From 08ef2e8a1c08c6189de3f2912944c5ce495d50cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 11:58:04 +0100 Subject: [PATCH 047/128] Param command: Increase stack as needed --- src/systemcmds/param/CMakeLists.txt | 2 +- src/systemcmds/param/module.mk | 45 ----------------------------- 2 files changed, 1 insertion(+), 46 deletions(-) delete mode 100644 src/systemcmds/param/module.mk diff --git a/src/systemcmds/param/CMakeLists.txt b/src/systemcmds/param/CMakeLists.txt index cd35b0da04..8158bfb09e 100644 --- a/src/systemcmds/param/CMakeLists.txt +++ b/src/systemcmds/param/CMakeLists.txt @@ -33,7 +33,7 @@ px4_add_module( MODULE systemcmds__param MAIN param - STACK 1800 + STACK 2500 COMPILE_FLAGS -Os SRCS diff --git a/src/systemcmds/param/module.mk b/src/systemcmds/param/module.mk deleted file mode 100644 index f716eb71e9..0000000000 --- a/src/systemcmds/param/module.mk +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -# -# Build the parameters tool. -# - -MODULE_COMMAND = param -SRCS = param.c - -# Note: measurements yielded a max of 900 bytes used. -MODULE_STACKSIZE = 1800 - -MAXOPTIMIZATION = -Os - From 1cfa9d924d5eb58818f5c874e49085b6b8dfc025 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 12:07:17 +0100 Subject: [PATCH 048/128] Fixed ekf2 stop / start routine --- src/modules/ekf2/ekf2_main.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 6c5793ecf0..521193336c 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -115,6 +115,8 @@ public: void print_status(); + void exit() { _task_should_exit = true; } + private: static constexpr float _dt_max = 0.02; bool _task_should_exit = false; /**< if true, task should exit */ @@ -471,6 +473,9 @@ void Ekf2::task_main() } } } + + delete ekf2::instance; + ekf2::instance = nullptr; } void Ekf2::task_main_trampoline(int argc, char *argv[]) @@ -535,8 +540,13 @@ int ekf2_main(int argc, char *argv[]) return 1; } - delete ekf2::instance; - ekf2::instance = nullptr; + ekf2::instance->exit(); + + // wait for the destruction of the instance + while (ekf2::instance != nullptr) { + usleep(50000); + } + return 0; } From f460e955549d3fb4425d42f23f9e51cd98978132 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 14:21:09 +0100 Subject: [PATCH 049/128] Param: Increase robustness of default save command --- src/modules/systemlib/param/param.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index dd5c30b667..235b83ad08 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -497,7 +497,7 @@ param_get(param_t param, void *val) } static int -param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool autosave) +param_set_internal(param_t param, const void *val, bool mark_saved, bool notify_changes, bool is_saved) { int result = -1; bool params_changed = false; @@ -575,7 +575,7 @@ out: * a thing has been set. */ if (params_changed && notify_changes) { - param_notify_changes(autosave); + param_notify_changes(is_saved); } return result; @@ -744,7 +744,13 @@ param_save_default(void) return ERROR; } - res = param_export(fd, false); + res = 1; + int attempts = 5; + + while (res != OK && attempts > 0) { + res = param_export(fd, false); + attempts--; + } if (res != OK) { warnx("failed to write parameters to file: %s", filename); From 7452cfdf631d384899c7d494bb68424d39fca6ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 14:21:19 +0100 Subject: [PATCH 050/128] EKF2: Fix polling code --- src/modules/ekf2/ekf2_main.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 521193336c..a0eb643cb4 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -228,7 +228,6 @@ void Ekf2::task_main() { // subscribe to relevant topics _sensors_sub = orb_subscribe(ORB_ID(sensor_combined)); - _params_sub = orb_subscribe(ORB_ID(sensor_combined)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -243,14 +242,14 @@ void Ekf2::task_main() updateParams(); while (!_task_should_exit) { - int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 8); + int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); if (ret < 0) { // Poll error, sleep and try again usleep(10000); continue; - } else if (ret == 0 || fds[0].revents != POLLIN) { + } else if (ret == 0) { // Poll timeout or no new data, do nothing continue; } @@ -263,6 +262,9 @@ void Ekf2::task_main() // fetch sensor data in next loop continue; + } else if (!(fds[0].revents & POLLIN)) { + // no new data + continue; } bool gps_updated = false; From 67eed88767afa59e2e4f6ee263c152977b2c5b3e Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 12 Jan 2016 07:31:44 +0100 Subject: [PATCH 051/128] added message for ekf2 innovations message --- msg/ekf2_innovations.msg | 7 +++++++ src/modules/uORB/objects_common.cpp | 3 +++ 2 files changed, 10 insertions(+) create mode 100644 msg/ekf2_innovations.msg diff --git a/msg/ekf2_innovations.msg b/msg/ekf2_innovations.msg new file mode 100644 index 0000000000..cb2ab03146 --- /dev/null +++ b/msg/ekf2_innovations.msg @@ -0,0 +1,7 @@ +uint64 timestamp # Timestamp in microseconds since boot +float32[6] vel_pos_innov # velocity and position innovations +float32[3] mag_innov # earth magnetic field innovations +float32 heading_innov # heading innovation +float32[6] vel_pos_innov_var # velocity and position innovation variances +float32[3] mag_innov_var # earth magnetic field innovation variance +float32 heading_innov_var # heading innovation variance diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 15dab60df4..0ff426e3b5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -273,3 +273,6 @@ ORB_DEFINE(camera_trigger, struct camera_trigger_s); #include "topics/vehicle_command_ack.h" ORB_DEFINE(vehicle_command_ack, struct vehicle_command_ack_s); + +#include "topics/ekf2_innovations.h" +ORB_DEFINE(ekf2_innovations, struct ekf2_innovations_s); From 21f7641e8d71505252c07a59c89089dcfa484388 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 12 Jan 2016 07:32:38 +0100 Subject: [PATCH 052/128] log ekf2 estimator status --- src/modules/ekf2/ekf2_main.cpp | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a0eb643cb4..2c84e714d2 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include @@ -131,6 +132,8 @@ private: orb_advert_t _lpos_pub; orb_advert_t _control_state_pub; orb_advert_t _vehicle_global_position_pub; + orb_advert_t _estimator_status_pub; + /* Low pass filter for attitude rates */ math::LowPassFilter2p _lp_roll_rate; @@ -174,12 +177,12 @@ Ekf2::Ekf2(): _lpos_pub(nullptr), _control_state_pub(nullptr), _vehicle_global_position_pub(nullptr), + _estimator_status_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_rate(250.0f, 20.0f), _ekf(new Ekf()) { - parameters *params = _ekf->getParamHandle(); _mag_delay_ms = new control::BlockParamFloat(this, "EKF2_MAG_DELAY", false, ¶ms->mag_delay_ms); _baro_delay_ms = new control::BlockParamFloat(this, "EKF2_BARO_DELAY", false, ¶ms->baro_delay_ms); @@ -474,6 +477,19 @@ void Ekf2::task_main() orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos); } } + + // publish estimator status + struct estimator_status_s status = {}; + status.timestamp = hrt_absolute_time(); + _ekf->get_state_delayed(status.states); + _ekf->get_covariances(status.covariances); + + if (_estimator_status_pub == nullptr) { + _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &status); + + } else { + orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); + } } delete ekf2::instance; From c185a12c8e061b82095f0575c9d69d3676476813 Mon Sep 17 00:00:00 2001 From: Roman Date: Thu, 14 Jan 2016 15:27:04 +0100 Subject: [PATCH 053/128] log ekf2 innovations and innovation variances --- src/modules/ekf2/ekf2_main.cpp | 22 +++++++++++++++++++++ src/modules/sdlog2/sdlog2.c | 29 ++++++++++++++++++++++++++++ src/modules/sdlog2/sdlog2_messages.h | 14 ++++++++++++++ 3 files changed, 65 insertions(+) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index 2c84e714d2..a48a52bcab 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include @@ -133,6 +134,7 @@ private: orb_advert_t _control_state_pub; orb_advert_t _vehicle_global_position_pub; orb_advert_t _estimator_status_pub; + orb_advert_t _estimator_innovations_pub; /* Low pass filter for attitude rates */ @@ -178,6 +180,7 @@ Ekf2::Ekf2(): _control_state_pub(nullptr), _vehicle_global_position_pub(nullptr), _estimator_status_pub(nullptr), + _estimator_innovations_pub(nullptr), _lp_roll_rate(250.0f, 30.0f), _lp_pitch_rate(250.0f, 30.0f), _lp_yaw_rate(250.0f, 20.0f), @@ -490,6 +493,25 @@ void Ekf2::task_main() } else { orb_publish(ORB_ID(estimator_status), _estimator_status_pub, &status); } + + // publish estimator innovation data + struct ekf2_innovations_s innovations = {}; + innovations.timestamp = hrt_absolute_time(); + _ekf->get_vel_pos_innov(&innovations.vel_pos_innov[0]); + _ekf->get_mag_innov(&innovations.mag_innov[0]); + _ekf->get_heading_innov(&innovations.heading_innov); + + _ekf->get_vel_pos_innov_var(&innovations.vel_pos_innov_var[0]); + _ekf->get_mag_innov_var(&innovations.mag_innov_var[0]); + _ekf->get_heading_innov_var(&innovations.heading_innov_var); + + if (_estimator_innovations_pub == nullptr) { + _estimator_innovations_pub = orb_advertise(ORB_ID(ekf2_innovations), &innovations); + + } else { + orb_publish(ORB_ID(ekf2_innovations), _estimator_innovations_pub, &innovations); + } + } delete ekf2::instance; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 57aec762e6..858d6af1ae 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -109,6 +109,7 @@ #include #include #include +#include #include #include @@ -1095,6 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct time_offset_s time_offset; struct mc_att_ctrl_status_s mc_att_ctrl_status; struct control_state_s ctrl_state; + struct ekf2_innovations_s innovations; } buf; memset(&buf, 0, sizeof(buf)); @@ -1144,6 +1146,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_TSYN_s log_TSYN; struct log_MACS_s log_MACS; struct log_CTS_s log_CTS; + struct log_EST4_s log_INO1; + struct log_EST5_s log_INO2; } body; } log_msg = { LOG_PACKET_HEADER_INIT(0) @@ -1187,6 +1191,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int tsync_sub; int mc_att_ctrl_status_sub; int ctrl_state_sub; + int innov_sub; } subs; subs.cmd_sub = -1; @@ -1222,6 +1227,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.mc_att_ctrl_status_sub = -1; subs.ctrl_state_sub = -1; subs.encoders_sub = -1; + subs.innov_sub = -1; /* add new topics HERE */ @@ -1810,6 +1816,29 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(EST3); } + /* --- EKF2 INNOVATIONS --- */ + if (copy_if_updated(ORB_ID(ekf2_innovations), &subs.innov_sub, &buf.innovations)) { + log_msg.msg_type = LOG_EST4_MSG; + memset(&(log_msg.body.log_INO1.s), 0, sizeof(log_msg.body.log_INO1.s)); + for (unsigned i = 0; i < 6; i++) { + log_msg.body.log_INO1.s[i] = buf.innovations.vel_pos_innov[i]; + log_msg.body.log_INO1.s[i + 6] = buf.innovations.vel_pos_innov_var[i]; + } + LOGBUFFER_WRITE_AND_COUNT(EST4); + + log_msg.msg_type = LOG_EST5_MSG; + memset(&(log_msg.body.log_INO2.s), 0, sizeof(log_msg.body.log_INO2.s)); + for (unsigned i = 0; i < 3; i++) { + log_msg.body.log_INO2.s[i] = buf.innovations.mag_innov[i]; + log_msg.body.log_INO2.s[i + 3] = buf.innovations.mag_innov_var[i]; + } + + log_msg.body.log_INO2.s[6] = buf.innovations.heading_innov; + log_msg.body.log_INO2.s[7] = buf.innovations.heading_innov_var; + LOGBUFFER_WRITE_AND_COUNT(EST5); + + } + /* --- TECS STATUS --- */ if (copy_if_updated(ORB_ID(tecs_status), &subs.tecs_status_sub, &buf.tecs_status)) { log_msg.msg_type = LOG_TECS_MSG; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 86c2c58868..fc256fe9d9 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -416,6 +416,18 @@ struct log_EST3_s { float cov[16]; }; +/* --- EST4 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST4_MSG 48 +struct log_EST4_s { + float s[12]; +}; + +/* --- EST4 - ESTIMATOR INNOVATIONS --- */ +#define LOG_EST5_MSG 49 +struct log_EST5_s { + float s[8]; +}; + /* --- TEL0..3 - TELEMETRY STATUS --- */ #define LOG_TEL0_MSG 36 #define LOG_TEL1_MSG 37 @@ -553,6 +565,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(EST1, "ffffffffffffffff", "s12,s13,s14,s15,s16,s17,s18,s19,s20,s21,s22,s23,s24,s25,s26,s27"), LOG_FORMAT(EST2, "ffffffffffff", "P0,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11"), LOG_FORMAT(EST3, "ffffffffffffffff", "P12,P13,P14,P15,P16,P17,P18,P19,P20,P21,P22,P23,P24,P25,P26,P27"), + LOG_FORMAT(EST4, "ffffffffffff", "VxI,VyI,VzI,PxI,PyI,PzI,VxIV,VyIV,VzIV,PxIV,PyIV,PzIV"), + LOG_FORMAT(EST5, "ffffffff", "MAGxI,MAGyI,MAGzI,MAGxIV,MAGyIV,MAGzIV,HeadI,HeadIV"), LOG_FORMAT(PWR, "fffBBBBB", "Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"), LOG_FORMAT(MOCP, "fffffff", "QuatW,QuatX,QuatY,QuatZ,X,Y,Z"), LOG_FORMAT(VISN, "ffffffffff", "X,Y,Z,VX,VY,VZ,QuatW,QuatX,QuatY,QuatZ"), From 7a8ef6c0e45a58c229a9f10bf9df685e06bc5450 Mon Sep 17 00:00:00 2001 From: tumbili Date: Fri, 22 Jan 2016 12:37:07 +0100 Subject: [PATCH 054/128] elc ekf2: support logging --- src/lib/ecl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/ecl b/src/lib/ecl index 86df68e404..8d0022ab1e 160000 --- a/src/lib/ecl +++ b/src/lib/ecl @@ -1 +1 @@ -Subproject commit 86df68e404d67268ff9004bb1e10eef218335e73 +Subproject commit 8d0022ab1e65543124efd2f55b4e72b9e672cd61 From e88367d7224134f9ef81fc6fbfc11d1e93ae0c3b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 17:11:46 +0100 Subject: [PATCH 055/128] SPI: Support setting the lock mode --- src/drivers/device/spi.h | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/drivers/device/spi.h b/src/drivers/device/spi.h index 9c3bf36f25..986a322432 100644 --- a/src/drivers/device/spi.h +++ b/src/drivers/device/spi.h @@ -77,6 +77,15 @@ protected: int irq = 0); virtual ~SPI(); + /** + * Locking modes supported by the driver. + */ + enum LockMode { + LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ + LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ + LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ + }; + virtual int init(); /** @@ -117,13 +126,14 @@ protected: void set_frequency(uint32_t frequency); /** - * Locking modes supported by the driver. + * Set the SPI bus locking mode + * + * This set the SPI locking mode. For devices competing with NuttX SPI + * drivers on a bus the right lock mode is LOCK_THREADS. + * + * @param mode Locking mode */ - enum LockMode { - LOCK_PREEMPTION, /**< the default; lock against all forms of preemption. */ - LOCK_THREADS, /**< lock only against other threads, using SPI_LOCK */ - LOCK_NONE /**< perform no locking, only safe if the bus is entirely private */ - }; + void set_lockmode(enum LockMode mode) { locking_mode = mode; } LockMode locking_mode; /**< selected locking mode */ From c659d7851f1f9b17232244213c788b7cc5d261f9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 17:12:13 +0100 Subject: [PATCH 056/128] MS5611: Use the right locking mechanism on Pixracer --- src/drivers/ms5611/ms5611_spi.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 78192c3a98..d5cd963adb 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -143,6 +143,11 @@ MS5611_SPI::~MS5611_SPI() int MS5611_SPI::init() { + // Sharing a bus with NuttX drivers +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + set_lockmode(SPI::LOCK_THREADS); +#endif + int ret; ret = SPI::init(); From 871b4b482e9b059e562912125d2dd411a59837ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Jan 2016 18:07:27 +0100 Subject: [PATCH 057/128] Do not enable new bus sync yet --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index d5cd963adb..1ca0d9cec4 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -145,7 +145,7 @@ MS5611_SPI::init() { // Sharing a bus with NuttX drivers #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - set_lockmode(SPI::LOCK_THREADS); + //set_lockmode(SPI::LOCK_THREADS); #endif int ret; From 0102e47708ff44d478cde919638b2f65b91ac0a6 Mon Sep 17 00:00:00 2001 From: Roman Date: Sat, 16 Jan 2016 17:22:06 +0100 Subject: [PATCH 058/128] for multicopter landings make sure that the copter moves to the landing waypoint first before the descending phase starts --- src/modules/navigator/mission.cpp | 7 +++++- src/modules/navigator/mission_block.cpp | 31 ++++++++++++++++++++++--- 2 files changed, 34 insertions(+), 4 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 242b0faedc..67802c6af7 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,7 +150,6 @@ void Mission::on_active() { check_mission_valid(); - /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -181,6 +180,12 @@ Mission::on_active() } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); + } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _waypoint_position_reached && _navigator->get_vstatus()->is_rotary_wing) { + // the copter has reached the landing spot location + // set the same landing item again but this time the vehicle will actually + // descend and land + set_mission_item_reached(); + set_mission_items(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 64512d3d3d..77362a9e2b 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -92,9 +92,17 @@ MissionBlock::is_mission_item_reached() } return true; } - case NAV_CMD_LAND: - return _navigator->get_vstatus()->condition_landed; + if (!_navigator->get_vstatus()->is_rotary_wing) { + return _navigator->get_vstatus()->condition_landed; + } else { + if (_waypoint_position_reached) { + // the copter has reached the position of the landing spot + // it can can start to descend + return _navigator->get_vstatus()->condition_landed; + } + } + break; /* TODO: count turns */ /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ @@ -159,6 +167,12 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } + } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { + // check if we have reached our landing spot + if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { + _waypoint_position_reached = true; + return false; + } } 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 || @@ -222,6 +236,7 @@ MissionBlock::is_mission_item_reached() return true; } } + return false; } @@ -284,7 +299,17 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite break; case NAV_CMD_LAND: - sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; + + if (!_waypoint_position_reached) { + // we first need to move to the landing waypoint + // use the altitude specified in the mission item + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; + } else { + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; + } + break; case NAV_CMD_LOITER_TIME_LIMIT: From 1474ddbb78553eba8a3851a6b1f8458b426080a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 13:23:32 +0100 Subject: [PATCH 059/128] MS5611: Ensure to set the lockmode at the right location --- src/drivers/ms5611/ms5611_spi.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 1ca0d9cec4..6d10d91ef8 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -143,11 +143,6 @@ MS5611_SPI::~MS5611_SPI() int MS5611_SPI::init() { - // Sharing a bus with NuttX drivers -#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) - //set_lockmode(SPI::LOCK_THREADS); -#endif - int ret; ret = SPI::init(); @@ -157,6 +152,11 @@ MS5611_SPI::init() goto out; } + /* sharing a bus with NuttX drivers */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + //set_lockmode(SPI::LOCK_THREADS); +#endif + /* send reset command */ ret = _reset(); From c18d31ce415d8611473db79198e98db155b1be62 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 13:23:53 +0100 Subject: [PATCH 060/128] Param write: Support locking the bus --- src/modules/systemlib/param/param.c | 43 +++++++++++++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 235b83ad08..63c7fea86e 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -44,6 +44,8 @@ //#include #include #include +#include +#include #include #include #include @@ -791,6 +793,36 @@ param_load_default(void) return 0; } +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) +//struct spi_dev_s *dev = nullptr; +irqstate_t state; +#endif + +static void +param_bus_lock(bool lock) +{ + +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + // FMUv4 has baro and FRAM on the same bus, + // as this offers on average a 100% silent + // bus for the baro operation + + // XXX this would be the preferred locking method + // if (dev == nullptr) { + // dev = up_spiinitialize(PX4_SPI_BUS_BARO); + // } + + // SPI_LOCK(dev, lock); + + // we lock like this for Pixracer for now + if (lock) { + state = irqsave(); + } else { + irqrestore(state); + } +#endif +} + int param_export(int fd, bool only_unsaved) { @@ -800,7 +832,9 @@ param_export(int fd, bool only_unsaved) param_lock(); + param_bus_lock(true); bson_encoder_init_file(&encoder, fd); + param_bus_lock(false); /* no modified parameters -> we are done */ if (param_values == NULL) { @@ -825,6 +859,9 @@ param_export(int fd, bool only_unsaved) /* append the appropriate BSON type object */ + /* lock as short as possible */ + param_bus_lock(true); + switch (param_type(s->param)) { case PARAM_TYPE_INT32: @@ -832,6 +869,7 @@ param_export(int fd, bool only_unsaved) if (bson_encoder_append_int(&encoder, param_name(s->param), i)) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -842,6 +880,7 @@ param_export(int fd, bool only_unsaved) if (bson_encoder_append_double(&encoder, param_name(s->param), f)) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -854,6 +893,7 @@ param_export(int fd, bool only_unsaved) param_size(s->param), param_get_value_ptr(s->param))) { debug("BSON append failed for '%s'", param_name(s->param)); + param_bus_lock(false); goto out; } @@ -861,9 +901,12 @@ param_export(int fd, bool only_unsaved) default: debug("unrecognized parameter type"); + param_bus_lock(false); goto out; } + param_bus_lock(false); + /* allow this process to be interrupted by another process / thread */ usleep(5); } From 99068aebac4d16f2b84eec739c8678bdabc8e661 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 13:32:32 +0100 Subject: [PATCH 061/128] FMUv4: Run FRAM bus faster --- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 37852242fb..55327c1605 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -306,7 +306,7 @@ __EXPORT int nsh_archinitialize(void) */ // XXX start with 10.4 MHz and go up to 20 once validated - SPI_SETFREQUENCY(spi2, 12 * 1000 * 1000); + SPI_SETFREQUENCY(spi2, 20 * 1000 * 1000); SPI_SETBITS(spi2, 8); SPI_SETMODE(spi2, SPIDEV_MODE3); SPI_SELECT(spi2, SPIDEV_FLASH, false); From b37082e3904d6a0adeab59e76c903f819abfa7ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 13:32:42 +0100 Subject: [PATCH 062/128] MS5611: Run SPI bus faster --- src/drivers/ms5611/ms5611_spi.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/ms5611/ms5611_spi.cpp b/src/drivers/ms5611/ms5611_spi.cpp index 6d10d91ef8..5fe0fc501d 100644 --- a/src/drivers/ms5611/ms5611_spi.cpp +++ b/src/drivers/ms5611/ms5611_spi.cpp @@ -131,7 +131,7 @@ MS5611_spi_interface(ms5611::prom_u &prom_buf, uint8_t busnum) } MS5611_SPI::MS5611_SPI(uint8_t bus, spi_dev_e device, ms5611::prom_u &prom_buf) : - SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 11 * 1000 * 1000 /* will be rounded to 10.4 MHz */), + SPI("MS5611_SPI", nullptr, bus, device, SPIDEV_MODE3, 20 * 1000 * 1000 /* will be rounded to 10.4 MHz */), _prom(prom_buf) { } From 4b893053a060ff7a61549e17cf937ae611de8356 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 13:32:52 +0100 Subject: [PATCH 063/128] param: Lock read operation --- src/modules/systemlib/param/param.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index 63c7fea86e..b1ef5f34ca 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -1042,15 +1042,21 @@ param_import_internal(int fd, bool mark_saved) int result = -1; struct param_import_state state; + param_bus_lock(true); if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { debug("decoder init failed"); + param_bus_lock(false); goto out; } + param_bus_lock(false); state.mark_saved = mark_saved; do { + param_bus_lock(true); result = bson_decoder_next(&decoder); + usleep(1); + param_bus_lock(false); } while (result > 0); From f52ce2001df76e0a3f9508a22d00d66601ef09cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 14:29:15 +0100 Subject: [PATCH 064/128] Param lib: fix code style --- src/modules/systemlib/param/param.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c index b1ef5f34ca..1c6518830e 100644 --- a/src/modules/systemlib/param/param.c +++ b/src/modules/systemlib/param/param.c @@ -817,9 +817,11 @@ param_bus_lock(bool lock) // we lock like this for Pixracer for now if (lock) { state = irqsave(); + } else { irqrestore(state); } + #endif } @@ -1043,11 +1045,13 @@ param_import_internal(int fd, bool mark_saved) struct param_import_state state; param_bus_lock(true); + if (bson_decoder_init_file(&decoder, fd, param_import_callback, &state)) { debug("decoder init failed"); param_bus_lock(false); goto out; } + param_bus_lock(false); state.mark_saved = mark_saved; From c2aaeefa6c42838f1234a4389767a2f17d5be02c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 1 Jan 2016 13:18:54 +0100 Subject: [PATCH 065/128] sdlog2: Log actuator output group 1 as well --- src/modules/sdlog2/sdlog2.c | 14 +++++++++++--- src/modules/sdlog2/sdlog2_messages.h | 11 +++++++---- 2 files changed, 18 insertions(+), 7 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 858d6af1ae..e5f1b7480f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1118,7 +1118,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_STAT_s log_STAT; struct log_VTOL_s log_VTOL; struct log_RC_s log_RC; - struct log_OUT0_s log_OUT0; + struct log_OUT_s log_OUT; struct log_AIRS_s log_AIRS; struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; @@ -1164,6 +1164,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int att_sp_sub; int rates_sp_sub; int act_outputs_sub; + int act_outputs_1_sub; int act_controls_sub; int act_controls_1_sub; int local_pos_sub; @@ -1203,6 +1204,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.att_sp_sub = -1; subs.rates_sp_sub = -1; subs.act_outputs_sub = -1; + subs.act_outputs_1_sub = -1; subs.act_controls_sub = -1; subs.act_controls_1_sub = -1; subs.local_pos_sub = -1; @@ -1532,8 +1534,14 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- ACTUATOR OUTPUTS --- */ if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; - memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); - LOGBUFFER_WRITE_AND_COUNT(OUT0); + memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT); + } + + if (copy_if_updated(ORB_ID(actuator_outputs), &subs.act_outputs_1_sub, &buf.act_outputs)) { + log_msg.msg_type = LOG_OUT1_MSG; + memcpy(log_msg.body.log_OUT.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT.output)); + LOGBUFFER_WRITE_AND_COUNT(OUT); } /* --- ACTUATOR CONTROL --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index fc256fe9d9..a7e2960def 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -195,9 +195,9 @@ struct log_RC_s { uint32_t frame_drop; }; -/* --- OUT0 - ACTUATOR_0 OUTPUT --- */ +/* --- OUT - ACTUATOR OUTPUT --- */ #define LOG_OUT0_MSG 12 -struct log_OUT0_s { +struct log_OUT_s { float output[8]; }; @@ -491,6 +491,8 @@ struct log_MACS_s { float yaw_rate_integ; }; +/* WARNING: ID 46 is already in use for ATTC1 */ + /* --- CONTROL STATE --- */ #define LOG_CTS_MSG 47 struct log_CTS_s { @@ -503,7 +505,7 @@ struct log_CTS_s { float yaw_rate; }; -/* WARNING: ID 46 is already in use for ATTC1 */ +#define LOG_OUT1_MSG 50 /********** SYSTEM MESSAGES, ID > 0x80 **********/ @@ -547,7 +549,8 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(VTOL, "f", "Arsp"), LOG_FORMAT(CTS, "fffffff", "Vx_b,Vy_b,Vz_b,Vinf,P,Q,R"), LOG_FORMAT(RC, "ffffffffffffBBBL", "C0,C1,C2,C3,C4,C5,C6,C7,C8,C9,C10,C11,RSSI,CNT,Lost,Drop"), - LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT_S(OUT0, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), + LOG_FORMAT_S(OUT1, OUT, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "fff", "IndSpeed,TrueSpeed,AirTemp"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "BffffffLLHhB", "ID,RawX,RawY,RX,RY,RZ,Dist,TSpan,DtSonar,FrmCnt,GT,Qlty"), From e17f3441190bbd3b8437e000550feb13d6edc929 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 18:23:15 +0100 Subject: [PATCH 066/128] MAVLink: Enable 4 streams and fix OSD rate configuration --- src/modules/mavlink/CMakeLists.txt | 2 +- src/modules/mavlink/mavlink_main.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/CMakeLists.txt b/src/modules/mavlink/CMakeLists.txt index 3ae2e570ec..625c88f9aa 100644 --- a/src/modules/mavlink/CMakeLists.txt +++ b/src/modules/mavlink/CMakeLists.txt @@ -41,7 +41,7 @@ px4_add_module( ${MODULE_CFLAGS} -Wno-attributes -Wno-packed - -DMAVLINK_COMM_NUM_BUFFERS=3 + -DMAVLINK_COMM_NUM_BUFFERS=4 -Wno-packed -Wno-tautological-constant-out-of-range-compare -Os diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index bd9f653c58..e31cf29411 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -1802,7 +1802,6 @@ Mavlink::task_main(int argc, char *argv[]) configure_stream("GLOBAL_POSITION_INT", 10.0f); configure_stream("HOME_POSITION", 0.5f); configure_stream("ATTITUDE_TARGET", 10.0f); - configure_stream("BATTERY_STATUS", 1.0f); configure_stream("SYSTEM_TIME", 1.0f); configure_stream("RC_CHANNELS", 5.0f); configure_stream("SERVO_OUTPUT_RAW_0", 1.0f); From 03434a0b9e24c8001fb7389d533cb5b8f1a2e874 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 18:25:07 +0100 Subject: [PATCH 067/128] Start 3rd MAVLink instance on OSD port --- ROMFS/px4fmu_common/init.d/rcS | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 5e859ae222..4ce6610b1f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -550,7 +550,7 @@ then # Avoid using ttyS1 for MAVLink on FMUv4 if ver hwcmp PX4FMU_V4 then - set MAVLINK_F "-r 1200 -d /dev/ttyS2" + set MAVLINK_F "-r 1200 -d /dev/ttyS1" # Start MAVLink on Wifi (ESP8266 port) mavlink start -r 800000 -b 921600 -d /dev/ttyS0 fi @@ -560,6 +560,12 @@ then mavlink start $MAVLINK_F unset MAVLINK_F + if ver hwcmp PX4FMU_V4 + then + # Start MAVLink on OSD (TELEM2 port) + mavlink start -r 1200 -d /dev/ttyS2 -b 57600 -m osd + fi + # # MAVLink onboard / TELEM2 # From c6d30315d9de5fae074eb03dc76dbd76e8f26fab Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 24 Jan 2016 02:11:59 -0500 Subject: [PATCH 068/128] Matrix update. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index 48a3288753..a92d5c1338 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit 48a3288753a3ec0bbd5e1f826ea413e1a6854fbb +Subproject commit a92d5c13389df17eaae1b27ed7b3e767c845bf31 From e2fd2f466ebb5c2f8be0af0fbc2f2f8e77bf9c96 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 23 Jan 2016 17:59:40 +0100 Subject: [PATCH 069/128] UDP testing for Linux folks --- src/modules/mavlink/mavlink_receiver.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 3b4b3f588a..06f49cc3ec 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1817,8 +1817,12 @@ MavlinkReceiver::receive_thread(void *arg) if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) || (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) { // if we were sending to localhost before but have a new host then accept him +// This is causing issues on Linux, so use default port for now +// this will kill tablet testing on Linux and VMs +#ifndef __PX4_LINUX srcaddr_last->sin_addr.s_addr = srcaddr.sin_addr.s_addr; srcaddr_last->sin_port = srcaddr.sin_port; +#endif _mavlink->set_client_source_initialized(); } #endif From bf03b8cb18ff6404911ea172e673428efac329b9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 11:13:09 +0100 Subject: [PATCH 070/128] Yaw rate limit: use a lower limit for less twitching --- src/modules/mc_att_control/mc_att_control_params.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 011a6755ad..fc296ccf74 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -254,13 +254,14 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); * Limit for yaw rate, has effect for large rotations in autonomous mode, * to avoid large control output and mixer saturation. A value of significantly * over 60 degrees per second can already lead to mixer saturation. + * A value of 30 degrees / second is recommended to avoid very audible twitches. * * @unit deg/s * @min 0.0 * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 60.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 30.0f); /** * Max acro roll rate From 759b107468123603353354f5fd566f9f6bb5771c Mon Sep 17 00:00:00 2001 From: James Goppert Date: Sun, 24 Jan 2016 05:13:54 -0500 Subject: [PATCH 071/128] Update LPE sitl init script. --- posix-configs/SITL/init/rcS_lpe_jmavsim_iris | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris index 7d59fe876f..75ca7ea09a 100644 --- a/posix-configs/SITL/init/rcS_lpe_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_lpe_jmavsim_iris @@ -3,8 +3,10 @@ simulator start -s param load param set MAV_TYPE 2 param set MC_PITCHRATE_P 0.15 +param set MC_PITCH_P 7 +param set MC_ROLL_P 7 param set MC_ROLLRATE_P 0.15 -param set MC_YAW_P 2.0 +param set MC_YAW_P 2.8 param set MC_YAWRATE_P 0.35 param set SYS_AUTOSTART 4010 param set SYS_RESTART_TYPE 2 @@ -26,12 +28,22 @@ param set MPC_XY_P 0.4 param set MPC_XY_VEL_P 0.2 param set MPC_XY_VEL_D 0.005 param set SENS_BOARD_ROT 0 +param set COM_RC_IN_MODE 1 +param set NAV_ACC_RAD 2.0 +param set RTL_RETURN_ALT 30.0 +param set RTL_DESCEND_ALT 10.0 +param set MIS_TAKEOFF_ALT 5.0 +param set MPC_HOLD_MAX_Z 2.0 +param set MPC_HOLD_XY_DZ 0.1 +param set MPC_HOLD_Z_DZ 0.1 +param set MPC_Z_VEL_MAX 2.0 +param set MPC_Z_VEL_P 0.4 # changes for LPE param set COM_RC_IN_MODE 1 param set LPE_BETA_MAX 10000 -rgbled start +rgbledsim start tone_alarm start gyrosim start accelsim start @@ -49,6 +61,7 @@ mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix mavlink start -u 14556 -r 2000000 +mavlink start -u 14557 -r 2000000 -m onboard mavlink stream -r 80 -s POSITION_TARGET_LOCAL_NED -u 14556 mavlink stream -r 80 -s LOCAL_POSITION_NED_COV -u 14556 mavlink stream -r 80 -s GLOBAL_POSITION_INT -u 14556 @@ -57,6 +70,7 @@ mavlink stream -r 80 -s ATTITUDE_TARGET -u 14556 mavlink stream -r 20 -s RC_CHANNELS -u 14556 mavlink stream -r 250 -s HIGHRES_IMU -u 14556 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556 +mavlink stream -r 20 -s MANUAL_CONTROL -u 14556 mavlink boot_complete sdlog2 start -r 100 -e -t -a From 9a8e94bb68d2c78ea1bd9d62ac993d937b797a31 Mon Sep 17 00:00:00 2001 From: sander Date: Sat, 23 Jan 2016 16:59:00 +0100 Subject: [PATCH 072/128] Reject mission when starting with LAND and vehicle is landed --- src/modules/navigator/mission.cpp | 6 ++++-- .../navigator/mission_feasibility_checker.cpp | 20 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 5 +++-- 3 files changed, 23 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 67802c6af7..ff76b5620d 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -264,7 +264,8 @@ Mission::update_offboard_mission() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _navigator->get_mission_result()->valid = !failed; _navigator->increment_mission_instance_count(); @@ -848,7 +849,8 @@ Mission::check_mission_valid() dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(), _navigator->get_home_position()->alt, _navigator->home_position_valid(), _navigator->get_global_position()->lat, _navigator->get_global_position()->lon, - _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius()); + _param_dist_1wp.get(), _navigator->get_mission_result()->warning, _navigator->get_acceptance_radius(), + _navigator->get_vstatus()->condition_landed); _navigator->increment_mission_instance_count(); _navigator->set_mission_result_updated(); diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 2cf1c45f56..d8ee9c8628 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -65,7 +65,8 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, - float default_acceptance_rad) + float default_acceptance_rad, + bool condition_landed) { bool failed = false; bool warned = false; @@ -84,7 +85,7 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota } // check if all mission item commands are supported - failed = failed || !checkMissionItemValidity(dm_current, nMissionItems); + failed = failed || !checkMissionItemValidity(dm_current, nMissionItems, condition_landed); failed = failed || !checkGeofence(dm_current, nMissionItems, geofence); failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned); @@ -211,7 +212,7 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, return true; } -bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems) { +bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed) { // do not allow mission if we find unsupported item for (size_t i = 0; i < nMissionItems; i++) { struct mission_item_s missionitem; @@ -223,7 +224,7 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s return false; } - // check if we find unsupported item and reject mission if so + // check if we find unsupported items and reject mission if so if (missionitem.nav_cmd != NAV_CMD_IDLE && missionitem.nav_cmd != NAV_CMD_WAYPOINT && missionitem.nav_cmd != NAV_CMD_LOITER_UNLIMITED && @@ -240,6 +241,17 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s mavlink_log_critical(_mavlink_fd, "Rejecting mission item %i: unsupported action.", (int)(i+1)); return false; } + + // check if the mission starts with a land command while the vehicle is landed + if (missionitem.nav_cmd == NAV_CMD_LAND && + i == 0 && + condition_landed) { + + mavlink_log_critical(_mavlink_fd, "Rejecting mission that starts with LAND command while vehicle is landed."); + return false; + } + + } return true; } diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index a1831999ac..7b44c3246d 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -63,7 +63,7 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false); - bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems); + bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems, bool condition_landed); bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued); /* Checks specific to fixedwing airframes */ @@ -83,7 +83,8 @@ public: */ bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid, - double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad); + double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued, float default_acceptance_rad, + bool condition_landed); }; From 47dcf7155414b16efac3e849cfa03a7c9f507d3d Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Jan 2016 15:27:38 -0700 Subject: [PATCH 073/128] initial test of FrSky SmartPort telemetry --- src/drivers/frsky_telemetry/sPort_data.h | 51 ++++ src/drivers/frsky_telemetry/sPort_telemetry.c | 267 ++++++++++++++++++ 2 files changed, 318 insertions(+) create mode 100644 src/drivers/frsky_telemetry/sPort_data.h create mode 100644 src/drivers/frsky_telemetry/sPort_telemetry.c diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h new file mode 100644 index 0000000000..b5840af641 --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -0,0 +1,51 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 sPort_data.h + * @author Stefan Rado + * + * FrSky SmartPort telemetry implementation. + * + */ +#ifndef _FRSKY_DATA_H +#define _FRSKY_DATA_H + +// Public functions +void frsky_init(void); +void frsky_send_frame1(int uart); +void frsky_send_frame2(int uart); +void frsky_send_frame3(int uart); + +#endif /* _FRSKY_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/sPort_telemetry.c b/src/drivers/frsky_telemetry/sPort_telemetry.c new file mode 100644 index 0000000000..3a85f12e48 --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_telemetry.c @@ -0,0 +1,267 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 frsky_telemetry.c + * @author Stefan Rado + * + * FrSky SmartPort telemetry implementation. + * + * This daemon emulates FrSky SmartPort sensors by responding to polling + * packets received from an attached FrSky X series receiver. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "frsky_data.h" + + +/* thread state */ +static volatile bool thread_should_exit = false; +static volatile bool thread_running = false; +static int frsky_task; + +/* functions */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static void usage(void); +static int frsky_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int frsky_telemetry_main(int argc, char *argv[]); + + +/** + * Opens the UART device and sets all required serial parameters. + */ +static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +{ + /* Open UART */ + const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", uart_name); + } + + /* Back up the original UART configuration to restore it after exit */ + int termios_state; + + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); + close(uart); + return -1; + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Disable output post-processing */ + uart_config.c_oflag &= ~OPOST; + + /* Set baud rate */ + static const speed_t speed = B9600; + + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); + close(uart); + return -1; + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + warnx("ERR: %s (tcsetattr)\n", uart_name); + close(uart); + return -1; + } + + return uart; +} + +/** + * Print command usage information + */ +static void usage() +{ + fprintf(stderr, + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); + exit(1); +} + +/** + * The daemon thread. + */ +static int frsky_telemetry_thread_main(int argc, char *argv[]) +{ + /* Default values for arguments */ + char *device_name = "/dev/ttyS1"; /* USART2 */ + + /* Work around some stupidity in task_create's argv handling */ + argc -= 2; + argv += 2; + + int ch; + + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + case 'd': + device_name = optarg; + break; + + default: + usage(); + break; + } + } + + /* Open UART */ + struct termios uart_config_original; + const int uart = frsky_open_uart(device_name, &uart_config_original); + + if (uart < 0) { + err(1, "could not open %s", device_name); + } + + /* Subscribe to topics */ + frsky_init(); + + thread_running = true; + + /* Main thread loop */ + unsigned int iteration = 0; + + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; +} + +/** + * The main command function. + * Processes command line arguments and starts the daemon. + */ +int frsky_telemetry_main(int argc, char *argv[]) +{ + if (argc < 2) { + warnx("missing command"); + usage(); + } + + if (!strcmp(argv[1], "start")) { + + /* this is not an error */ + if (thread_running) { + errx(0, "frsky_telemetry already running"); + } + + thread_should_exit = false; + frsky_task = px4_task_spawn_cmd("frsky_telemetry", + SCHED_DEFAULT, + SCHED_PRIORITY_DEFAULT, + 2000, + frsky_telemetry_thread_main, + (char *const *)argv); + + while (!thread_running) { + usleep(200); + } + + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + + /* this is not an error */ + if (!thread_running) { + errx(0, "frsky_telemetry already stopped"); + } + + thread_should_exit = true; + + while (thread_running) { + usleep(200000); + warnx("."); + } + + warnx("terminated."); + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + errx(0, "running"); + + } else { + errx(1, "not running"); + } + } + + warnx("unrecognized command"); + usage(); + /* not getting here */ + return 0; +} From 214709dc897ffe9154090931511bdb3dbde19ce3 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Jan 2016 15:32:47 -0700 Subject: [PATCH 074/128] move smartport telemetry to new module --- src/drivers/sPort_telemetry/CMakeLists.txt | 45 +++ src/drivers/sPort_telemetry/sPort_data.c | 311 ++++++++++++++++++ .../sPort_data.h | 14 +- .../sPort_telemetry.c | 92 +++--- 4 files changed, 415 insertions(+), 47 deletions(-) create mode 100644 src/drivers/sPort_telemetry/CMakeLists.txt create mode 100644 src/drivers/sPort_telemetry/sPort_data.c rename src/drivers/{frsky_telemetry => sPort_telemetry}/sPort_data.h (90%) rename src/drivers/{frsky_telemetry => sPort_telemetry}/sPort_telemetry.c (74%) diff --git a/src/drivers/sPort_telemetry/CMakeLists.txt b/src/drivers/sPort_telemetry/CMakeLists.txt new file mode 100644 index 0000000000..d19291720d --- /dev/null +++ b/src/drivers/sPort_telemetry/CMakeLists.txt @@ -0,0 +1,45 @@ +############################################################################ +# +# Copyright (c) 2015 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ +px4_add_module( + MODULE drivers__sPort_telemetry + MAIN sPort_telemetry + STACK 1200 + COMPILE_FLAGS + -Os + SRCS + sPort_data.c + sPort_telemetry.c + DEPENDS + platforms__common + ) +# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c new file mode 100644 index 0000000000..a5f4cdcbf7 --- /dev/null +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -0,0 +1,311 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 sPort_data.c + * @author Stefan Rado + * + * FrSky SmartPort telemetry implementation. + * + */ + +#include "sPort_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +/* FrSky SmartPort sensor IDs */ +#define FRSKY_ID_GPS_ALT_BP 0x01 +#define FRSKY_ID_TEMP1 0x02 +#define FRSKY_ID_RPM 0x03 +#define FRSKY_ID_FUEL 0x04 +#define FRSKY_ID_TEMP2 0x05 +#define FRSKY_ID_VOLTS 0x06 +#define FRSKY_ID_GPS_ALT_AP 0x09 +#define FRSKY_ID_BARO_ALT_BP 0x10 +#define FRSKY_ID_GPS_SPEED_BP 0x11 +#define FRSKY_ID_GPS_LONG_BP 0x12 +#define FRSKY_ID_GPS_LAT_BP 0x13 +#define FRSKY_ID_GPS_COURS_BP 0x14 +#define FRSKY_ID_GPS_DAY_MONTH 0x15 +#define FRSKY_ID_GPS_YEAR 0x16 +#define FRSKY_ID_GPS_HOUR_MIN 0x17 +#define FRSKY_ID_GPS_SEC 0x18 +#define FRSKY_ID_GPS_SPEED_AP 0x19 +#define FRSKY_ID_GPS_LONG_AP 0x1A +#define FRSKY_ID_GPS_LAT_AP 0x1B +#define FRSKY_ID_GPS_COURS_AP 0x1C +#define FRSKY_ID_BARO_ALT_AP 0x21 +#define FRSKY_ID_GPS_LONG_EW 0x22 +#define FRSKY_ID_GPS_LAT_NS 0x23 +#define FRSKY_ID_ACCEL_X 0x24 +#define FRSKY_ID_ACCEL_Y 0x25 +#define FRSKY_ID_ACCEL_Z 0x26 +#define FRSKY_ID_CURRENT 0x28 +#define FRSKY_ID_VARIO 0x30 +#define FRSKY_ID_VFAS 0x39 +#define FRSKY_ID_VOLTS_BP 0x3A +#define FRSKY_ID_VOLTS_AP 0x3B + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void sPort_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x10 start byte. + */ +static void sPort_send_start(int uart) +{ + static const uint8_t c = 0x10; + write(uart, &c, sizeof(c)); +} + +static void update_crc(uint16_t *crc) +{ + *crc += 0x10; + *crc += *crc >> 8; + *crc &= 0xFF; + *crc += *crc >> 8; + *crc &= 0xFF; +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void sPort_send_byte(int uart, uint8_t value) +{ + const uint8_t x7E[] = { 0x7D, 0x5E }; + const uint8_t x7D[] = { 0x7D, 0x5D }; + + switch (value) { + case 0x5E: + write(uart, x7E, sizeof(x7E)); + break; + + case 0x5D: + write(uart, x7D, sizeof(x7D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +void sPort_send_data(int uart, uint16_t id, uint32_t data) +{ + union { + uint16_t word; + uint8_t byte[2]; + } wbuf; + + uint16_t crc = 0; + sPort_send_start(uart); + + wbuf.word = id; + + for (int i = 0; i < 2; i++) { + update_crc(&crc); + sPort_send_byte(uart, wbuf.byte[i]); /* LSB first */ + } + + uint8_t *bbuf = (uint8_t *)data; + + for (int i = 0; i < 4; i++) { + update_crc(&crc); + sPort_send_byte(uart, bbuf[i]); /* LSB first */ + } +} + +#ifdef xxxx +/** + * Sends frame 1 (every 200ms): + * acceleration values, barometer altitude, temperature, battery voltage & current + */ +void sPort_send_frame1(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send formatted frame */ + sPort_send_data(uart, FRSKY_ID_ACCEL_X, + roundf(raw.accelerometer_m_s2[0] * 1000.0f)); + sPort_send_data(uart, FRSKY_ID_ACCEL_Y, + roundf(raw.accelerometer_m_s2[1] * 1000.0f)); + sPort_send_data(uart, FRSKY_ID_ACCEL_Z, + roundf(raw.accelerometer_m_s2[2] * 1000.0f)); + + sPort_send_data(uart, FRSKY_ID_BARO_ALT_BP, + raw.baro_alt_meter[0]); + sPort_send_data(uart, FRSKY_ID_BARO_ALT_AP, + roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); + + sPort_send_data(uart, FRSKY_ID_TEMP1, + roundf(raw.baro_temp_celcius[0])); + + sPort_send_data(uart, FRSKY_ID_VFAS, + roundf(battery.voltage_v * 10.0f)); + sPort_send_data(uart, FRSKY_ID_CURRENT, + (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); + + sPort_send_start(uart); +} + +/** + * Formats the decimal latitude/longitude to the required degrees/minutes. + */ +static float sPort_format_gps(float dec) +{ + float dm_deg = (int) dec; + return (dm_deg * 100.0f) + (dec - dm_deg) * 60; +} + +/** + * Sends frame 2 (every 1000ms): + * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level + */ +void sPort_send_frame2(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send formatted frame */ + float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; + char lat_ns = 0, lon_ew = 0; + int sec = 0; + + if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; + struct tm *tm_gps = gmtime(&time_gps); + + course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; + lat = sPort_format_gps(fabsf(global_pos.lat)); + lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; + lon = sPort_format_gps(fabsf(global_pos.lon)); + lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; + speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) + * 25.0f / 46.0f; + alt = global_pos.alt; + sec = tm_gps->tm_sec; + } + + sPort_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); + sPort_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); + + sPort_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); + sPort_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); + sPort_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); + + sPort_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); + sPort_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); + sPort_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); + + sPort_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); + sPort_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); + + sPort_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); + sPort_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); + + sPort_send_data(uart, FRSKY_ID_FUEL, + roundf(vehicle_status.battery_remaining * 100.0f)); + + sPort_send_data(uart, FRSKY_ID_GPS_SEC, sec); + + sPort_send_start(uart); +} + +/** + * Sends frame 3 (every 5000ms): + * GPS date & time + */ +void sPort_send_frame3(int uart) +{ + /* get a local copy of the battery data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send formatted frame */ + time_t time_gps = global_pos.time_utc_usec / 1000000ULL; + struct tm *tm_gps = gmtime(&time_gps); + uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); + sPort_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); + sPort_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); + sPort_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); + sPort_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); + + sPort_send_start(uart); +} +#endif // xxxx diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h similarity index 90% rename from src/drivers/frsky_telemetry/sPort_data.h rename to src/drivers/sPort_telemetry/sPort_data.h index b5840af641..07088b16ee 100644 --- a/src/drivers/frsky_telemetry/sPort_data.h +++ b/src/drivers/sPort_telemetry/sPort_data.h @@ -39,13 +39,13 @@ * FrSky SmartPort telemetry implementation. * */ -#ifndef _FRSKY_DATA_H -#define _FRSKY_DATA_H +#ifndef _SPORT_DATA_H +#define _SPORT_DATA_H + +#include // Public functions -void frsky_init(void); -void frsky_send_frame1(int uart); -void frsky_send_frame2(int uart); -void frsky_send_frame3(int uart); +void sPort_init(void); +void sPort_send_data(int uart, uint16_t id, uint32_t data); -#endif /* _FRSKY_TELEMETRY_H */ +#endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/frsky_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c similarity index 74% rename from src/drivers/frsky_telemetry/sPort_telemetry.c rename to src/drivers/sPort_telemetry/sPort_telemetry.c index 3a85f12e48..dbf9d0068c 100644 --- a/src/drivers/frsky_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -33,7 +33,8 @@ ****************************************************************************/ /** - * @file frsky_telemetry.c + * @file sPort_telemetry.c + * @author Mark Whitehorn * @author Stefan Rado * * FrSky SmartPort telemetry implementation. @@ -54,33 +55,44 @@ #include #include -#include "frsky_data.h" +#include "sPort_data.h" /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; -static int frsky_task; +static int sPort_task; /* functions */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original); static void usage(void); -static int frsky_telemetry_thread_main(int argc, char *argv[]); -__EXPORT int frsky_telemetry_main(int argc, char *argv[]); +static int sPort_telemetry_thread_main(int argc, char *argv[]); +__EXPORT int sPort_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ - const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + const int uart = open(uart_name, O_RDWR | O_NOCTTY); if (uart < 0) { err(1, "Error opening port: %s", uart_name); } + /* make sure uart FD is blocking */ + int flags = fcntl(uart, F_GETFL); + if (flags < 0) { + err(1, "Error getting FD flags: %s", uart_name); + } + flags &= ~O_NONBLOCK; + flags = fcntl(uart, F_SETFL, flags); + if (flags < 0) { + err(1, "Error setting FD flags: %s", uart_name); + } + /* Back up the original UART configuration to restore it after exit */ int termios_state; @@ -98,7 +110,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or uart_config.c_oflag &= ~OPOST; /* Set baud rate */ - static const speed_t speed = B9600; + static const speed_t speed = B57600; if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); @@ -121,16 +133,16 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: sPort_telemetry start [-d ]\n" + " sPort_telemetry stop\n" + " sPort_telemetry status\n"); exit(1); } /** * The daemon thread. */ -static int frsky_telemetry_thread_main(int argc, char *argv[]) +static int sPort_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ char *device_name = "/dev/ttyS1"; /* USART2 */ @@ -154,42 +166,42 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } /* Open UART */ + warnx("opening uart"); struct termios uart_config_original; - const int uart = frsky_open_uart(device_name, &uart_config_original); + const int uart = sPort_open_uart(device_name, &uart_config_original); if (uart < 0) { + warnx("could not open %s", device_name); err(1, "could not open %s", device_name); } /* Subscribe to topics */ - frsky_init(); + sPort_init(); thread_running = true; /* Main thread loop */ - unsigned int iteration = 0; + char sbuf[2]; + uint8_t fiftyfive = 0x55; while (!thread_should_exit) { - /* Sleep 200 ms */ - usleep(200000); - - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); - - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); - } - - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); - - iteration = 0; - } - - iteration++; + /* wait for poll frame starting with value 0x7E */ + int newBytes = read(uart, &sbuf[0], 1); +// warnx("%x, %x \n", sbuf[0], sbuf[1]); + + if (newBytes < 1 || sbuf[0] != 0x7E) continue; + + /* read the ID byte */ + sbuf[1] = read(uart, &sbuf[0], 1); + +// sPort_send_data(uart, id, 7); + + /*** test ***/ + /* write single byte */ + write(uart, &fiftyfive, 1); + /* read it back */ + read(uart, &sbuf[0], 1); } /* Reset the UART flags to original state */ @@ -204,7 +216,7 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) * The main command function. * Processes command line arguments and starts the daemon. */ -int frsky_telemetry_main(int argc, char *argv[]) +int sPort_telemetry_main(int argc, char *argv[]) { if (argc < 2) { warnx("missing command"); @@ -215,15 +227,15 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) { - errx(0, "frsky_telemetry already running"); + errx(0, "sPort_telemetry already running"); } thread_should_exit = false; - frsky_task = px4_task_spawn_cmd("frsky_telemetry", + sPort_task = px4_task_spawn_cmd("sPort_telemetry", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2000, - frsky_telemetry_thread_main, + sPort_telemetry_thread_main, (char *const *)argv); while (!thread_running) { @@ -237,7 +249,7 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (!thread_running) { - errx(0, "frsky_telemetry already stopped"); + errx(0, "sPort_telemetry already stopped"); } thread_should_exit = true; From bb565f5d6aed28dbcc4d839c5592eb423e776664 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Jan 2016 20:01:34 -0700 Subject: [PATCH 075/128] add sPort_telemetry cmd to fmu-v4 module list and change default port to ttyS6/usart8 --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 + src/drivers/sPort_telemetry/sPort_telemetry.c | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 8bb4005353..6ed03f88ec 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -35,6 +35,7 @@ set(config_module_list drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry + drivers/sPort_telemetry modules/sensors drivers/mkblctrl drivers/px4flow diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index dbf9d0068c..d8e0675385 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -145,7 +145,7 @@ static void usage() static int sPort_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ - char *device_name = "/dev/ttyS1"; /* USART2 */ + char *device_name = "/dev/ttyS6"; /* USART8 */ /* Work around some stupidity in task_create's argv handling */ argc -= 2; From ff690dda8072bb8068772cebfe4cd7979d491151 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sun, 17 Jan 2016 20:41:36 -0700 Subject: [PATCH 076/128] increase priority of sPort_telemetry to 200 --- src/drivers/sPort_telemetry/sPort_data.c | 33 ++++++------ src/drivers/sPort_telemetry/sPort_telemetry.c | 50 +++++++++---------- 2 files changed, 40 insertions(+), 43 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index a5f4cdcbf7..c3028003cd 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -112,14 +112,12 @@ void sPort_init() static void sPort_send_start(int uart) { static const uint8_t c = 0x10; - write(uart, &c, sizeof(c)); + write(uart, &c, 1); } -static void update_crc(uint16_t *crc) +static void update_crc(uint16_t *crc, uint8_t b) { - *crc += 0x10; - *crc += *crc >> 8; - *crc &= 0xFF; + *crc += b; *crc += *crc >> 8; *crc &= 0xFF; } @@ -152,27 +150,30 @@ static void sPort_send_byte(int uart, uint8_t value) */ void sPort_send_data(int uart, uint16_t id, uint32_t data) { - union { - uint16_t word; - uint8_t byte[2]; - } wbuf; - + union { + uint32_t word; + uint8_t byte[4]; + } buf; + uint16_t crc = 0; sPort_send_start(uart); +// write(uart, 0x10, 1); - wbuf.word = id; + buf.word = id; for (int i = 0; i < 2; i++) { - update_crc(&crc); - sPort_send_byte(uart, wbuf.byte[i]); /* LSB first */ + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ } - uint8_t *bbuf = (uint8_t *)data; + buf.word = data; for (int i = 0; i < 4; i++) { - update_crc(&crc); - sPort_send_byte(uart, bbuf[i]); /* LSB first */ + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ } + + sPort_send_byte(uart, crc); } #ifdef xxxx diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index d8e0675385..cd1500038b 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -49,11 +49,13 @@ #include #include #include +#include #include #include #include #include #include +#include #include "sPort_data.h" @@ -76,23 +78,13 @@ __EXPORT int sPort_telemetry_main(int argc, char *argv[]); static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original) { /* Open UART */ - const int uart = open(uart_name, O_RDWR | O_NOCTTY); + const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); +// const int uart = open(uart_name, O_RDWR | O_NOCTTY); if (uart < 0) { err(1, "Error opening port: %s", uart_name); } - /* make sure uart FD is blocking */ - int flags = fcntl(uart, F_GETFL); - if (flags < 0) { - err(1, "Error getting FD flags: %s", uart_name); - } - flags &= ~O_NONBLOCK; - flags = fcntl(uart, F_SETFL, flags); - if (flags < 0) { - err(1, "Error setting FD flags: %s", uart_name); - } - /* Back up the original UART configuration to restore it after exit */ int termios_state; @@ -175,33 +167,37 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) err(1, "could not open %s", device_name); } + /* poll descriptor */ + struct pollfd fds[1]; + fds[0].fd = uart; + fds[0].events = POLLIN; + /* Subscribe to topics */ sPort_init(); thread_running = true; /* Main thread loop */ - char sbuf[2]; - uint8_t fiftyfive = 0x55; + char sbuf[20]; while (!thread_should_exit) { /* wait for poll frame starting with value 0x7E */ - int newBytes = read(uart, &sbuf[0], 1); -// warnx("%x, %x \n", sbuf[0], sbuf[1]); + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1); + if (status < 1) continue; + // read 2 bytes + int newBytes = read(uart, &sbuf[0], 2); if (newBytes < 1 || sbuf[0] != 0x7E) continue; - /* read the ID byte */ - sbuf[1] = read(uart, &sbuf[0], 1); - -// sPort_send_data(uart, id, 7); - - /*** test ***/ - /* write single byte */ - write(uart, &fiftyfive, 1); - /* read it back */ - read(uart, &sbuf[0], 1); + /* device ID 4 */ + static uint8_t counter = 0; + if (sbuf[1] == 0xe4) { + /* send data for A2 */ + sPort_send_data(uart, 0xf103, counter++); + /* read it back */ + read(uart, &sbuf[0], sizeof(sbuf)); + } } /* Reset the UART flags to original state */ @@ -233,7 +229,7 @@ int sPort_telemetry_main(int argc, char *argv[]) thread_should_exit = false; sPort_task = px4_task_spawn_cmd("sPort_telemetry", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, + 200, 2000, sPort_telemetry_thread_main, (char *const *)argv); From 8949a88f2514cc8f1506ea50061963574f9df3e2 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 18 Jan 2016 20:01:52 -0700 Subject: [PATCH 077/128] fix CRC calc and add battery voltage report --- src/drivers/sPort_telemetry/sPort_data.c | 37 ++++++++++++++----- src/drivers/sPort_telemetry/sPort_data.h | 1 + src/drivers/sPort_telemetry/sPort_telemetry.c | 10 ++--- 3 files changed, 33 insertions(+), 15 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index c3028003cd..7030212fd9 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -109,13 +109,13 @@ void sPort_init() /** * Sends a 0x10 start byte. */ -static void sPort_send_start(int uart) -{ - static const uint8_t c = 0x10; - write(uart, &c, 1); -} +//static void sPort_send_start(int uart) +//{ +// static const uint8_t c = 0x10; +// write(uart, &c, 1); +//} -static void update_crc(uint16_t *crc, uint8_t b) +static void update_crc(uint16_t *crc, unsigned char b) { *crc += b; *crc += *crc >> 8; @@ -155,9 +155,12 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) uint8_t byte[4]; } buf; - uint16_t crc = 0; - sPort_send_start(uart); -// write(uart, 0x10, 1); + /* send start byte */ + static const uint8_t c = 0x10; + write(uart, &c, 1); + + /* init crc */ + uint16_t crc = c; buf.word = id; @@ -173,7 +176,21 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) sPort_send_byte(uart, buf.byte[i]); /* LSB first */ } - sPort_send_byte(uart, crc); + sPort_send_byte(uart, 0xFF - crc); +} + + +void sPort_send_A2(int uart) +{ + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + /* send data for A2 */ +// uint32_t voltage = (int)(255 * battery.voltage_v / 18.4f); + uint32_t voltage = (int)(255 * 5.5f / 18.4f); + sPort_send_data(uart, 0xf103, voltage); } #ifdef xxxx diff --git a/src/drivers/sPort_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h index 07088b16ee..24dfefb169 100644 --- a/src/drivers/sPort_telemetry/sPort_data.h +++ b/src/drivers/sPort_telemetry/sPort_data.h @@ -47,5 +47,6 @@ // Public functions void sPort_init(void); void sPort_send_data(int uart, uint16_t id, uint32_t data); +void sPort_send_A2(int uart); #endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index cd1500038b..b8a382e94d 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -191,12 +191,12 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) if (newBytes < 1 || sbuf[0] != 0x7E) continue; /* device ID 4 */ - static uint8_t counter = 0; - if (sbuf[1] == 0xe4) { - /* send data for A2 */ - sPort_send_data(uart, 0xf103, counter++); + if (sbuf[1] == 0x1B) { + /* send battery voltage */ + sPort_send_A2(uart); + /* read it back */ - read(uart, &sbuf[0], sizeof(sbuf)); + read(uart, &sbuf[0], sizeof(sbuf)); } } From 82bb0564e22dd589adb9b62d8dc874314a38254f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Mon, 18 Jan 2016 20:19:57 -0700 Subject: [PATCH 078/128] fix byte stuffing typo and add comments --- src/drivers/sPort_telemetry/sPort_data.c | 14 +++++--- src/drivers/sPort_telemetry/sPort_telemetry.c | 33 ++++++++++++++----- 2 files changed, 34 insertions(+), 13 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index 7030212fd9..dc43e0fd40 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -131,11 +131,11 @@ static void sPort_send_byte(int uart, uint8_t value) const uint8_t x7D[] = { 0x7D, 0x5D }; switch (value) { - case 0x5E: + case 0x7E: write(uart, x7E, sizeof(x7E)); break; - case 0x5D: + case 0x7D: write(uart, x7D, sizeof(x7D)); break; @@ -187,9 +187,15 @@ void sPort_send_A2(int uart) memset(&battery, 0, sizeof(battery)); orb_copy(ORB_ID(battery_status), battery_sub, &battery); + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + /* send data for A2 */ -// uint32_t voltage = (int)(255 * battery.voltage_v / 18.4f); - uint32_t voltage = (int)(255 * 5.5f / 18.4f); + uint32_t voltage = (int)(255 * vehicle_status.battery_voltage / 16.8f); +// static uint8_t counter=0; +// uint32_t voltage = counter++; sPort_send_data(uart, 0xf103, voltage); } diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index b8a382e94d..8a5c4e0e3a 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -182,21 +182,36 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) while (!thread_should_exit) { - /* wait for poll frame starting with value 0x7E */ + /* wait for poll frame starting with value 0x7E + * note that only the bus master is supposed to put a 0x7E on the bus. + * slaves use byte stuffing to send 0x7E and 0x7D. + */ int status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1); - if (status < 1) continue; - - // read 2 bytes - int newBytes = read(uart, &sbuf[0], 2); - if (newBytes < 1 || sbuf[0] != 0x7E) continue; - + + if (status < 1) { continue; } + + // read 1 byte + int newBytes = read(uart, &sbuf[0], 1); + + if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } + + /* wait for ID byte */ + status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1); + + if (status < 1) { continue; } + + newBytes = read(uart, &sbuf[1], 1); + + // allow a minimum of 500usec before reply + usleep(500); + /* device ID 4 */ if (sbuf[1] == 0x1B) { /* send battery voltage */ sPort_send_A2(uart); - + /* read it back */ - read(uart, &sbuf[0], sizeof(sbuf)); + read(uart, &sbuf[0], sizeof(sbuf)); } } From bbc03731c78384284a17dd5c71e818459e8e8487 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Tue, 19 Jan 2016 15:34:28 -0700 Subject: [PATCH 079/128] initial implementation of 5 virtual sensors for Bat V,I, Baro alt, Fuel and GPS speed. Voltage and current scaling can be set in OpenTX. Fuel is in percent. Alt in meters. Need to look at what OpenTX wants for speed. --- src/drivers/sPort_telemetry/sPort_data.c | 193 ++++-------------- src/drivers/sPort_telemetry/sPort_data.h | 37 +++- src/drivers/sPort_telemetry/sPort_telemetry.c | 31 ++- 3 files changed, 98 insertions(+), 163 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index dc43e0fd40..02774dda56 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -55,39 +55,6 @@ #include -/* FrSky SmartPort sensor IDs */ -#define FRSKY_ID_GPS_ALT_BP 0x01 -#define FRSKY_ID_TEMP1 0x02 -#define FRSKY_ID_RPM 0x03 -#define FRSKY_ID_FUEL 0x04 -#define FRSKY_ID_TEMP2 0x05 -#define FRSKY_ID_VOLTS 0x06 -#define FRSKY_ID_GPS_ALT_AP 0x09 -#define FRSKY_ID_BARO_ALT_BP 0x10 -#define FRSKY_ID_GPS_SPEED_BP 0x11 -#define FRSKY_ID_GPS_LONG_BP 0x12 -#define FRSKY_ID_GPS_LAT_BP 0x13 -#define FRSKY_ID_GPS_COURS_BP 0x14 -#define FRSKY_ID_GPS_DAY_MONTH 0x15 -#define FRSKY_ID_GPS_YEAR 0x16 -#define FRSKY_ID_GPS_HOUR_MIN 0x17 -#define FRSKY_ID_GPS_SEC 0x18 -#define FRSKY_ID_GPS_SPEED_AP 0x19 -#define FRSKY_ID_GPS_LONG_AP 0x1A -#define FRSKY_ID_GPS_LAT_AP 0x1B -#define FRSKY_ID_GPS_COURS_AP 0x1C -#define FRSKY_ID_BARO_ALT_AP 0x21 -#define FRSKY_ID_GPS_LONG_EW 0x22 -#define FRSKY_ID_GPS_LAT_NS 0x23 -#define FRSKY_ID_ACCEL_X 0x24 -#define FRSKY_ID_ACCEL_Y 0x25 -#define FRSKY_ID_ACCEL_Z 0x26 -#define FRSKY_ID_CURRENT 0x28 -#define FRSKY_ID_VARIO 0x30 -#define FRSKY_ID_VFAS 0x39 -#define FRSKY_ID_VOLTS_BP 0x3A -#define FRSKY_ID_VOLTS_AP 0x3B - #define frac(f) (f - (int)f) static int battery_sub = -1; @@ -180,156 +147,68 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) } -void sPort_send_A2(int uart) +// TODO: correct scaling +void sPort_send_BATV(int uart) { - /* get a local copy of the battery data */ - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - /* get a local copy of the vehicle status data */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - /* send data for A2 */ + /* send battery voltage as VFAS */ uint32_t voltage = (int)(255 * vehicle_status.battery_voltage / 16.8f); -// static uint8_t counter=0; -// uint32_t voltage = counter++; - sPort_send_data(uart, 0xf103, voltage); + sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); } -#ifdef xxxx -/** - * Sends frame 1 (every 200ms): - * acceleration values, barometer altitude, temperature, battery voltage & current - */ -void sPort_send_frame1(int uart) +// TODO: correct scaling +void sPort_send_CUR(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send data */ + uint32_t current = (int)(255 * vehicle_status.battery_current / 50.0f); + sPort_send_data(uart, SMARTPORT_ID_CURR, current); +} + +// TODO: verify scaling +void sPort_send_ALT(int uart) { /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /* get a local copy of the battery data */ - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - - /* send formatted frame */ - sPort_send_data(uart, FRSKY_ID_ACCEL_X, - roundf(raw.accelerometer_m_s2[0] * 1000.0f)); - sPort_send_data(uart, FRSKY_ID_ACCEL_Y, - roundf(raw.accelerometer_m_s2[1] * 1000.0f)); - sPort_send_data(uart, FRSKY_ID_ACCEL_Z, - roundf(raw.accelerometer_m_s2[2] * 1000.0f)); - - sPort_send_data(uart, FRSKY_ID_BARO_ALT_BP, - raw.baro_alt_meter[0]); - sPort_send_data(uart, FRSKY_ID_BARO_ALT_AP, - roundf(frac(raw.baro_alt_meter[0]) * 100.0f)); - - sPort_send_data(uart, FRSKY_ID_TEMP1, - roundf(raw.baro_temp_celcius[0])); - - sPort_send_data(uart, FRSKY_ID_VFAS, - roundf(battery.voltage_v * 10.0f)); - sPort_send_data(uart, FRSKY_ID_CURRENT, - (battery.current_a < 0) ? 0 : roundf(battery.current_a * 10.0f)); - - sPort_send_start(uart); + /* send data */ + uint32_t alt = (int)(raw.baro_alt_meter[0]); + sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } -/** - * Formats the decimal latitude/longitude to the required degrees/minutes. - */ -static float sPort_format_gps(float dec) -{ - float dm_deg = (int) dec; - return (dm_deg * 100.0f) + (dec - dm_deg) * 60; -} - -/** - * Sends frame 2 (every 1000ms): - * GPS course, latitude, longitude, ground speed, GPS altitude, remaining battery level - */ -void sPort_send_frame2(int uart) +// TODO: verify scaling +void sPort_send_SPD(int uart) { /* get a local copy of the global position data */ struct vehicle_global_position_s global_pos; memset(&global_pos, 0, sizeof(global_pos)); orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + /* send data for A2 */ + float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + uint32_t ispeed = (int)speed; + sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); +} + +// TODO: verify scaling +void sPort_send_FUEL(int uart) +{ /* get a local copy of the vehicle status data */ struct vehicle_status_s vehicle_status; memset(&vehicle_status, 0, sizeof(vehicle_status)); orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - /* send formatted frame */ - float course = 0, lat = 0, lon = 0, speed = 0, alt = 0; - char lat_ns = 0, lon_ew = 0; - int sec = 0; - - if (global_pos.timestamp != 0 && hrt_absolute_time() < global_pos.timestamp + 20000) { - time_t time_gps = global_pos.time_utc_usec / 1000000ULL; - struct tm *tm_gps = gmtime(&time_gps); - - course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f; - lat = sPort_format_gps(fabsf(global_pos.lat)); - lat_ns = (global_pos.lat < 0) ? 'S' : 'N'; - lon = sPort_format_gps(fabsf(global_pos.lon)); - lon_ew = (global_pos.lon < 0) ? 'W' : 'E'; - speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e) - * 25.0f / 46.0f; - alt = global_pos.alt; - sec = tm_gps->tm_sec; - } - - sPort_send_data(uart, FRSKY_ID_GPS_COURS_BP, course); - sPort_send_data(uart, FRSKY_ID_GPS_COURS_AP, frac(course) * 1000.0f); - - sPort_send_data(uart, FRSKY_ID_GPS_LAT_BP, lat); - sPort_send_data(uart, FRSKY_ID_GPS_LAT_AP, frac(lat) * 10000.0f); - sPort_send_data(uart, FRSKY_ID_GPS_LAT_NS, lat_ns); - - sPort_send_data(uart, FRSKY_ID_GPS_LONG_BP, lon); - sPort_send_data(uart, FRSKY_ID_GPS_LONG_AP, frac(lon) * 10000.0f); - sPort_send_data(uart, FRSKY_ID_GPS_LONG_EW, lon_ew); - - sPort_send_data(uart, FRSKY_ID_GPS_SPEED_BP, speed); - sPort_send_data(uart, FRSKY_ID_GPS_SPEED_AP, frac(speed) * 100.0f); - - sPort_send_data(uart, FRSKY_ID_GPS_ALT_BP, alt); - sPort_send_data(uart, FRSKY_ID_GPS_ALT_AP, frac(alt) * 100.0f); - - sPort_send_data(uart, FRSKY_ID_FUEL, - roundf(vehicle_status.battery_remaining * 100.0f)); - - sPort_send_data(uart, FRSKY_ID_GPS_SEC, sec); - - sPort_send_start(uart); + /* send data */ + uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); + sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); } - -/** - * Sends frame 3 (every 5000ms): - * GPS date & time - */ -void sPort_send_frame3(int uart) -{ - /* get a local copy of the battery data */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - - /* send formatted frame */ - time_t time_gps = global_pos.time_utc_usec / 1000000ULL; - struct tm *tm_gps = gmtime(&time_gps); - uint16_t hour_min = (tm_gps->tm_min << 8) | (tm_gps->tm_hour & 0xff); - sPort_send_data(uart, FRSKY_ID_GPS_DAY_MONTH, tm_gps->tm_mday); - sPort_send_data(uart, FRSKY_ID_GPS_YEAR, tm_gps->tm_year); - sPort_send_data(uart, FRSKY_ID_GPS_HOUR_MIN, hour_min); - sPort_send_data(uart, FRSKY_ID_GPS_SEC, tm_gps->tm_sec); - - sPort_send_start(uart); -} -#endif // xxxx diff --git a/src/drivers/sPort_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h index 24dfefb169..895fde3465 100644 --- a/src/drivers/sPort_telemetry/sPort_data.h +++ b/src/drivers/sPort_telemetry/sPort_data.h @@ -44,9 +44,44 @@ #include +/* FrSky SmartPort polling IDs captured from X4R */ +#define SMARTPORT_POLL_1 0x1B +#define SMARTPORT_POLL_2 0x34 +#define SMARTPORT_POLL_3 0x95 +#define SMARTPORT_POLL_4 0x16 +#define SMARTPORT_POLL_5 0xB7 + +/* FrSky SmartPort sensor IDs */ +#define SMARTPORT_ID_RSSI 0xf101 +#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX +#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX +#define SMARTPORT_ID_BATV 0xf104 +#define SMARTPORT_ID_SWR 0xf105 +#define SMARTPORT_ID_T1 0x0400 +#define SMARTPORT_ID_T2 0x0410 +#define SMARTPORT_ID_RPM 0x0500 +#define SMARTPORT_ID_FUEL 0x0600 +#define SMARTPORT_ID_ALT 0x0100 +#define SMARTPORT_ID_VARIO 0x0110 +#define SMARTPORT_ID_ACCX 0x0700 +#define SMARTPORT_ID_ACCY 0x0710 +#define SMARTPORT_ID_ACCZ 0x0720 +#define SMARTPORT_ID_CURR 0x0200 +#define SMARTPORT_ID_VFAS 0x0210 +#define SMARTPORT_ID_CELLS 0x0300 +#define SMARTPORT_ID_GPS_LON_LAT 0x0800 +#define SMARTPORT_ID_GPS_ALT 0x0820 +#define SMARTPORT_ID_GPS_SPD 0x0830 +#define SMARTPORT_ID_GPS_CRS 0x0840 +#define SMARTPORT_ID_GPS_TIME 0x0850 + // Public functions void sPort_init(void); void sPort_send_data(int uart, uint16_t id, uint32_t data); -void sPort_send_A2(int uart); +void sPort_send_BATV(int uart); +void sPort_send_CUR(int uart); +void sPort_send_ALT(int uart); +void sPort_send_SPD(int uart); +void sPort_send_FUEL(int uart); #endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index 8a5c4e0e3a..fdb56e152a 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -205,14 +205,35 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) // allow a minimum of 500usec before reply usleep(500); - /* device ID 4 */ - if (sbuf[1] == 0x1B) { + switch (sbuf[1]) { + case SMARTPORT_POLL_1: /* send battery voltage */ - sPort_send_A2(uart); + sPort_send_BATV(uart); + break; + + case SMARTPORT_POLL_2: + /* send battery current */ + sPort_send_CUR(uart); + break; + + case SMARTPORT_POLL_3: + /* send altitude */ + sPort_send_ALT(uart); + break; + + case SMARTPORT_POLL_4: + /* send speed */ + sPort_send_SPD(uart); + break; + + case SMARTPORT_POLL_5: + /* send fuel */ + sPort_send_FUEL(uart); + break; - /* read it back */ - read(uart, &sbuf[0], sizeof(sbuf)); } + /* TODO: flush the input buffer if in full duplex mode */ + read(uart, &sbuf[0], sizeof(sbuf)); } /* Reset the UART flags to original state */ From ea4937491b8615d98466233abd914e5569f0f305 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 09:49:26 -0700 Subject: [PATCH 080/128] fix scale factor for VFAS: battery voltage report --- src/drivers/sPort_telemetry/sPort_data.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index 02774dda56..4159dfd532 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -156,7 +156,7 @@ void sPort_send_BATV(int uart) orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send battery voltage as VFAS */ - uint32_t voltage = (int)(255 * vehicle_status.battery_voltage / 16.8f); + uint32_t voltage = (int)(100 * vehicle_status.battery_voltage); sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); } From 4e4d1a98f6e1be5768e483d453ccbaabc07286fd Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 10:24:58 -0700 Subject: [PATCH 081/128] update comments on scale factors --- src/drivers/sPort_telemetry/sPort_data.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index 4159dfd532..df20634e49 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -147,7 +147,7 @@ void sPort_send_data(int uart, uint16_t id, uint32_t data) } -// TODO: correct scaling +// scaling correct with OpenTX 2.1.7 void sPort_send_BATV(int uart) { /* get a local copy of the vehicle status data */ @@ -160,7 +160,7 @@ void sPort_send_BATV(int uart) sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); } -// TODO: correct scaling +// TODO: verify scaling void sPort_send_CUR(int uart) { /* get a local copy of the vehicle status data */ @@ -169,7 +169,7 @@ void sPort_send_CUR(int uart) orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send data */ - uint32_t current = (int)(255 * vehicle_status.battery_current / 50.0f); + uint32_t current = (int)(100 * vehicle_status.battery_current); sPort_send_data(uart, SMARTPORT_ID_CURR, current); } From 6cb631716aa17796c56ef3c7385b162abbc0e346 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 10:41:39 -0700 Subject: [PATCH 082/128] corrected scales for OpenTX 2.1.7 as specified in the comments --- src/drivers/sPort_telemetry/sPort_data.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index df20634e49..2f10c74f5b 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -160,7 +160,7 @@ void sPort_send_BATV(int uart) sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); } -// TODO: verify scaling +// verified scaling void sPort_send_CUR(int uart) { /* get a local copy of the vehicle status data */ @@ -169,11 +169,11 @@ void sPort_send_CUR(int uart) orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); /* send data */ - uint32_t current = (int)(100 * vehicle_status.battery_current); + uint32_t current = (int)(10 * vehicle_status.battery_current); sPort_send_data(uart, SMARTPORT_ID_CURR, current); } -// TODO: verify scaling +// verified scaling for "calculated" altitude option void sPort_send_ALT(int uart) { /* get a local copy of the current sensor values */ @@ -182,11 +182,11 @@ void sPort_send_ALT(int uart) orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); /* send data */ - uint32_t alt = (int)(raw.baro_alt_meter[0]); + uint32_t alt = (int)(10 * raw.baro_alt_meter[0]); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } -// TODO: verify scaling +// verified scaling for "calculated" option void sPort_send_SPD(int uart) { /* get a local copy of the global position data */ @@ -196,11 +196,11 @@ void sPort_send_SPD(int uart) /* send data for A2 */ float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - uint32_t ispeed = (int)speed; + uint32_t ispeed = (int)(10 * speed); sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); } -// TODO: verify scaling +// verified scaling void sPort_send_FUEL(int uart) { /* get a local copy of the vehicle status data */ From 99286db832f8fc4549de90fa81dc7431568ea0fc Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 14:46:48 -0700 Subject: [PATCH 083/128] running astyle inside QtCreator doesn't guarantee that travis will pass format checks --- src/drivers/sPort_telemetry/sPort_telemetry.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index fdb56e152a..877a685b3e 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -232,8 +232,9 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) break; } + /* TODO: flush the input buffer if in full duplex mode */ - read(uart, &sbuf[0], sizeof(sbuf)); + read(uart, &sbuf[0], sizeof(sbuf)); } /* Reset the UART flags to original state */ From 52ebbda5ac399c68c544b0cbf5e6a22b05889174 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 15:28:38 -0700 Subject: [PATCH 084/128] add conditional start of sPort_telemetry daemon for FMUv4 to rcS --- ROMFS/px4fmu_common/init.d/rcS | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 4ce6610b1f..38430f67f9 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -841,6 +841,11 @@ then fi fi +if ver hwcmp PX4FMU_V4 +then + sPort_telemetry start -d /dev/ttyS6 +fi + if ver hwcmp PX4FMU_V2 then # Check for flow sensor - as it is a background task, launch it last From 8d089d95d4d4e7c51a352b9f7fe981f332a120ad Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Wed, 20 Jan 2016 16:35:28 -0700 Subject: [PATCH 085/128] change baro altitude scaling to 100 to match OpenTX "custom" altitude mode --- src/drivers/sPort_telemetry/sPort_data.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c index 2f10c74f5b..5267f44f61 100644 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ b/src/drivers/sPort_telemetry/sPort_data.c @@ -173,7 +173,9 @@ void sPort_send_CUR(int uart) sPort_send_data(uart, SMARTPORT_ID_CURR, current); } -// verified scaling for "calculated" altitude option +// verified scaling for "custom" altitude option +// OpenTX uses the initial reading as field elevation and displays +// the difference (altitude - field) void sPort_send_ALT(int uart) { /* get a local copy of the current sensor values */ @@ -182,7 +184,7 @@ void sPort_send_ALT(int uart) orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); /* send data */ - uint32_t alt = (int)(10 * raw.baro_alt_meter[0]); + uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); sPort_send_data(uart, SMARTPORT_ID_ALT, alt); } From a402b3beeb51f56d6bde5f6f92a65a1e7a28b020 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 22 Jan 2016 14:42:39 -0700 Subject: [PATCH 086/128] change frsky_telemetry default port to USART8 for pixracer add a 50msec timeout to poll calls in sPort_telemetry to prevent hangs --- src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +- src/drivers/sPort_telemetry/sPort_telemetry.c | 5 +++-- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6100f5f1d0..94b68448fa 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -133,7 +133,7 @@ static void usage() static int frsky_telemetry_thread_main(int argc, char *argv[]) { /* Default values for arguments */ - char *device_name = "/dev/ttyS1"; /* USART2 */ + char *device_name = "/dev/ttyS6"; /* USART8 */ /* Work around some stupidity in task_create's argv handling */ argc -= 2; diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index 877a685b3e..3adde806da 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -185,8 +185,9 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) /* wait for poll frame starting with value 0x7E * note that only the bus master is supposed to put a 0x7E on the bus. * slaves use byte stuffing to send 0x7E and 0x7D. + * we expect a poll frame every 12msec */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1); + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); if (status < 1) { continue; } @@ -196,7 +197,7 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } /* wait for ID byte */ - status = poll(fds, sizeof(fds) / sizeof(fds[0]), -1); + status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); if (status < 1) { continue; } From 4f55ae5306580300541bbc1ba5e58c960cde18ab Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 22 Jan 2016 17:51:50 -0700 Subject: [PATCH 087/128] add D type telemetry fallback to sPort_telemetry daemon --- src/drivers/sPort_telemetry/sPort_telemetry.c | 180 +++++++++++------- 1 file changed, 116 insertions(+), 64 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index 3adde806da..2ca8e65449 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -58,6 +58,7 @@ #include #include "sPort_data.h" +#include "../frsky_telemetry/frsky_data.h" /* thread state */ @@ -66,7 +67,7 @@ static volatile bool thread_running = false; static int sPort_task; /* functions */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original); +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); static void usage(void); static int sPort_telemetry_thread_main(int argc, char *argv[]); __EXPORT int sPort_telemetry_main(int argc, char *argv[]); @@ -75,11 +76,10 @@ __EXPORT int sPort_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config_original) +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original) { /* Open UART */ const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); -// const int uart = open(uart_name, O_RDWR | O_NOCTTY); if (uart < 0) { err(1, "Error opening port: %s", uart_name); @@ -95,22 +95,21 @@ static int sPort_open_uart(const char *uart_name, struct termios *uart_config_or } /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); + tcgetattr(uart, uart_config); /* Disable output post-processing */ - uart_config.c_oflag &= ~OPOST; + uart_config->c_oflag &= ~OPOST; /* Set baud rate */ static const speed_t speed = B57600; - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) { warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; @@ -160,7 +159,8 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) /* Open UART */ warnx("opening uart"); struct termios uart_config_original; - const int uart = sPort_open_uart(device_name, &uart_config_original); + struct termios uart_config; + const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); if (uart < 0) { warnx("could not open %s", device_name); @@ -172,70 +172,122 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) fds[0].fd = uart; fds[0].events = POLLIN; - /* Subscribe to topics */ - sPort_init(); - thread_running = true; /* Main thread loop */ char sbuf[20]; - while (!thread_should_exit) { + /* look for polling frames indicating SmartPort telemetry + * if not found, send D type telemetry frames instead + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - /* wait for poll frame starting with value 0x7E - * note that only the bus master is supposed to put a 0x7E on the bus. - * slaves use byte stuffing to send 0x7E and 0x7D. - * we expect a poll frame every 12msec - */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - // read 1 byte - int newBytes = read(uart, &sbuf[0], 1); - - if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } - - /* wait for ID byte */ - status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - newBytes = read(uart, &sbuf[1], 1); - - // allow a minimum of 500usec before reply - usleep(500); - - switch (sbuf[1]) { - case SMARTPORT_POLL_1: - /* send battery voltage */ - sPort_send_BATV(uart); - break; - - case SMARTPORT_POLL_2: - /* send battery current */ - sPort_send_CUR(uart); - break; - - case SMARTPORT_POLL_3: - /* send altitude */ - sPort_send_ALT(uart); - break; - - case SMARTPORT_POLL_4: - /* send speed */ - sPort_send_SPD(uart); - break; - - case SMARTPORT_POLL_5: - /* send fuel */ - sPort_send_FUEL(uart); - break; + if (status < 1) { + /* timed out: reconfigure UART and send D type telemetry */ + warnx("sending FrSky D type telemetry"); + if (cfsetispeed(&uart_config, B9600) < 0 || cfsetospeed(&uart_config, B9600) < 0) { + warnx("ERR: %s:(cfsetispeed, cfsetospeed)\n", device_name); + close(uart); + return -1; } - /* TODO: flush the input buffer if in full duplex mode */ - read(uart, &sbuf[0], sizeof(sbuf)); + if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { + warnx("ERR: %s (tcsetattr)\n", device_name); + close(uart); + return -1; + } + + int iteration = 0; + + /* Subscribe to topics */ + frsky_init(); + + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; + } + + } else { + /* Subscribe to topics */ + sPort_init(); + + /* send S.port telemetry */ + while (!thread_should_exit) { + + /* wait for poll frame starting with value 0x7E + * note that only the bus master is supposed to put a 0x7E on the bus. + * slaves use byte stuffing to send 0x7E and 0x7D. + * we expect a poll frame every 12msec + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + // read 1 byte + int newBytes = read(uart, &sbuf[0], 1); + + if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } + + /* wait for ID byte */ + status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + newBytes = read(uart, &sbuf[1], 1); + + // allow a minimum of 500usec before reply + usleep(500); + + switch (sbuf[1]) { + case SMARTPORT_POLL_1: + /* send battery voltage */ + sPort_send_BATV(uart); + break; + + case SMARTPORT_POLL_2: + /* send battery current */ + sPort_send_CUR(uart); + break; + + case SMARTPORT_POLL_3: + /* send altitude */ + sPort_send_ALT(uart); + break; + + case SMARTPORT_POLL_4: + /* send speed */ + sPort_send_SPD(uart); + break; + + case SMARTPORT_POLL_5: + /* send fuel */ + sPort_send_FUEL(uart); + break; + + } + + /* TODO: flush the input buffer if in full duplex mode */ + read(uart, &sbuf[0], sizeof(sbuf)); + } } /* Reset the UART flags to original state */ From 3b9ef1cef57231999cd4780f4b41b6288e7dbca4 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 22 Jan 2016 20:55:06 -0700 Subject: [PATCH 088/128] reduce reporting frequency for smartport sensors. reduces CPU load to approx. 1% --- src/drivers/sPort_telemetry/sPort_telemetry.c | 58 +++++++++++++++---- 1 file changed, 48 insertions(+), 10 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index 2ca8e65449..e5d99d23ef 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -252,35 +252,73 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) if (status < 1) { continue; } + hrt_abstime now = hrt_absolute_time(); + newBytes = read(uart, &sbuf[1], 1); // allow a minimum of 500usec before reply usleep(500); + static hrt_abstime lastBATV = 0; + static hrt_abstime lastCUR = 0; + static hrt_abstime lastALT = 0; + static hrt_abstime lastSPD = 0; + static hrt_abstime lastFUEL = 0; + switch (sbuf[1]) { + case SMARTPORT_POLL_1: - /* send battery voltage */ - sPort_send_BATV(uart); + + /* report BATV at 1Hz */ + if (now - lastBATV > 1000 * 1000) { + lastBATV = now; + /* send battery voltage */ + sPort_send_BATV(uart); + } + break; + case SMARTPORT_POLL_2: - /* send battery current */ - sPort_send_CUR(uart); + /* report battery current at 5Hz */ + if (now - lastCUR > 200 * 1000) { + lastCUR = now; + /* send battery current */ + sPort_send_CUR(uart); + } + break; + case SMARTPORT_POLL_3: - /* send altitude */ - sPort_send_ALT(uart); + /* report altitude at 5Hz */ + if (now - lastALT > 200 * 1000) { + lastALT = now; + /* send altitude */ + sPort_send_ALT(uart); + } + break; + case SMARTPORT_POLL_4: - /* send speed */ - sPort_send_SPD(uart); + /* report speed at 5Hz */ + if (now - lastSPD > 200 * 1000) { + lastSPD = now; + /* send speed */ + sPort_send_SPD(uart); + } + break; case SMARTPORT_POLL_5: - /* send fuel */ - sPort_send_FUEL(uart); + /* report fuel at 1Hz */ + if (now - lastFUEL > 1000 * 1000) { + lastFUEL = now; + /* send fuel */ + sPort_send_FUEL(uart); + } + break; } From 528e2826d53d303811d05e4d8dc0a590d00cb304 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Fri, 22 Jan 2016 20:56:03 -0700 Subject: [PATCH 089/128] run astyle --- src/drivers/sPort_telemetry/sPort_telemetry.c | 22 +++++++++++-------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c index e5d99d23ef..943f3c89fc 100644 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ b/src/drivers/sPort_telemetry/sPort_telemetry.c @@ -259,11 +259,11 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) // allow a minimum of 500usec before reply usleep(500); - static hrt_abstime lastBATV = 0; - static hrt_abstime lastCUR = 0; - static hrt_abstime lastALT = 0; - static hrt_abstime lastSPD = 0; - static hrt_abstime lastFUEL = 0; + static hrt_abstime lastBATV = 0; + static hrt_abstime lastCUR = 0; + static hrt_abstime lastALT = 0; + static hrt_abstime lastSPD = 0; + static hrt_abstime lastFUEL = 0; switch (sbuf[1]) { @@ -280,7 +280,8 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) case SMARTPORT_POLL_2: - /* report battery current at 5Hz */ + + /* report battery current at 5Hz */ if (now - lastCUR > 200 * 1000) { lastCUR = now; /* send battery current */ @@ -291,7 +292,8 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) case SMARTPORT_POLL_3: - /* report altitude at 5Hz */ + + /* report altitude at 5Hz */ if (now - lastALT > 200 * 1000) { lastALT = now; /* send altitude */ @@ -302,7 +304,8 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) case SMARTPORT_POLL_4: - /* report speed at 5Hz */ + + /* report speed at 5Hz */ if (now - lastSPD > 200 * 1000) { lastSPD = now; /* send speed */ @@ -312,7 +315,8 @@ static int sPort_telemetry_thread_main(int argc, char *argv[]) break; case SMARTPORT_POLL_5: - /* report fuel at 1Hz */ + + /* report fuel at 1Hz */ if (now - lastFUEL > 1000 * 1000) { lastFUEL = now; /* send fuel */ From 42f90796832de782ad25fda2252a57a3464afa28 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 08:12:13 -0700 Subject: [PATCH 090/128] move sPort_telemetry.c to src/drivers/frsky_telemetry and rename daemon to frsky_telemetry --- src/drivers/frsky_telemetry/frsky_telemetry.c | 236 +++++++++++++++--- 1 file changed, 196 insertions(+), 40 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 94b68448fa..a3b1af4544 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -33,13 +33,15 @@ ****************************************************************************/ /** - * @file frsky_telemetry.c + * @file sPort_telemetry.c * @author Stefan Rado + * @author Mark Whitehorn * - * FrSky telemetry implementation. + * FrSky D8 mode and SmartPort (D16 mode) telemetry implementation. * - * This daemon emulates an FrSky sensor hub by periodically sending data - * packets to an attached FrSky receiver. + * This daemon emulates the FrSky Sensor Hub for D8 mode. + * For X series receivers (D16 mode) it emulates SmartPort sensors by responding to polling + * packets received from an attached FrSky X series receiver. * */ @@ -48,22 +50,26 @@ #include #include #include +#include #include #include #include #include #include +#include -#include "frsky_data.h" +#include "sPort_data.h" +#include "../frsky_telemetry/frsky_data.h" /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; -static int frsky_task; +static int sPort_task; /* functions */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original); +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); +static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed); static void usage(void); static int frsky_telemetry_thread_main(int argc, char *argv[]); __EXPORT int frsky_telemetry_main(int argc, char *argv[]); @@ -72,10 +78,10 @@ __EXPORT int frsky_telemetry_main(int argc, char *argv[]); /** * Opens the UART device and sets all required serial parameters. */ -static int frsky_open_uart(const char *uart_name, struct termios *uart_config_original) +static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original) { /* Open UART */ - const int uart = open(uart_name, O_WRONLY | O_NOCTTY); + const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); if (uart < 0) { err(1, "Error opening port: %s", uart_name); @@ -91,22 +97,21 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or } /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); + tcgetattr(uart, uart_config); /* Disable output post-processing */ - uart_config.c_oflag &= ~OPOST; + uart_config->c_oflag &= ~OPOST; /* Set baud rate */ - static const speed_t speed = B9600; + static const speed_t speed = B57600; - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); close(uart); return -1; } - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) { warnx("ERR: %s (tcsetattr)\n", uart_name); close(uart); return -1; @@ -115,15 +120,29 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or return uart; } +static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed) +{ + + if (cfsetispeed(uart_config, speed) < 0) { + return -1; + } + + if (tcsetattr(uart, TCSANOW, uart_config) < 0) { + return -1; + } + + return uart; +} + /** * Print command usage information */ static void usage() { fprintf(stderr, - "usage: frsky_telemetry start [-d ]\n" - " frsky_telemetry stop\n" - " frsky_telemetry status\n"); + "usage: sPort_telemetry start [-d ]\n" + " sPort_telemetry stop\n" + " sPort_telemetry status\n"); exit(1); } @@ -154,42 +173,179 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } /* Open UART */ + warnx("opening uart"); struct termios uart_config_original; - const int uart = frsky_open_uart(device_name, &uart_config_original); + struct termios uart_config; + const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); if (uart < 0) { + warnx("could not open %s", device_name); err(1, "could not open %s", device_name); } - /* Subscribe to topics */ - frsky_init(); + /* poll descriptor */ + struct pollfd fds[1]; + fds[0].fd = uart; + fds[0].events = POLLIN; thread_running = true; /* Main thread loop */ - unsigned int iteration = 0; + char sbuf[20]; - while (!thread_should_exit) { + /* look for polling frames indicating SmartPort telemetry + * if not found, send D type telemetry frames instead + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - /* Sleep 200 ms */ - usleep(200000); + if (status < 1) { + /* timed out: reconfigure UART and send D type telemetry */ + warnx("SmartPort receiver not detected, sending FrSky D type telemetry"); - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); - - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); + status = set_uart_speed(uart, &uart_config, B9600); + if (status < 0) { + warnx("error setting speed for %s, quitting", device_name); + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; } - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); + int iteration = 0; - iteration = 0; + /* Subscribe to topics */ + frsky_init(); + + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) { + frsky_send_frame2(uart); + } + + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; } - iteration++; + } else { + + /* Subscribe to topics */ + sPort_init(); + + /* send S.port telemetry */ + while (!thread_should_exit) { + + /* wait for poll frame starting with value 0x7E + * note that only the bus master is supposed to put a 0x7E on the bus. + * slaves use byte stuffing to send 0x7E and 0x7D. + * we expect a poll frame every 12msec + */ + int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + // read 1 byte + int newBytes = read(uart, &sbuf[0], 1); + + if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } + + /* wait for ID byte */ + status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); + + if (status < 1) { continue; } + + hrt_abstime now = hrt_absolute_time(); + + newBytes = read(uart, &sbuf[1], 1); + + // allow a minimum of 500usec before reply + usleep(500); + + static hrt_abstime lastBATV = 0; + static hrt_abstime lastCUR = 0; + static hrt_abstime lastALT = 0; + static hrt_abstime lastSPD = 0; + static hrt_abstime lastFUEL = 0; + + switch (sbuf[1]) { + + case SMARTPORT_POLL_1: + + /* report BATV at 1Hz */ + if (now - lastBATV > 1000 * 1000) { + lastBATV = now; + /* send battery voltage */ + sPort_send_BATV(uart); + } + + break; + + + case SMARTPORT_POLL_2: + + /* report battery current at 5Hz */ + if (now - lastCUR > 200 * 1000) { + lastCUR = now; + /* send battery current */ + sPort_send_CUR(uart); + } + + break; + + + case SMARTPORT_POLL_3: + + /* report altitude at 5Hz */ + if (now - lastALT > 200 * 1000) { + lastALT = now; + /* send altitude */ + sPort_send_ALT(uart); + } + + break; + + + case SMARTPORT_POLL_4: + + /* report speed at 5Hz */ + if (now - lastSPD > 200 * 1000) { + lastSPD = now; + /* send speed */ + sPort_send_SPD(uart); + } + + break; + + case SMARTPORT_POLL_5: + + /* report fuel at 1Hz */ + if (now - lastFUEL > 1000 * 1000) { + lastFUEL = now; + /* send fuel */ + sPort_send_FUEL(uart); + } + + break; + + } + + /* TODO: flush the input buffer if in full duplex mode */ + read(uart, &sbuf[0], sizeof(sbuf)); + } } /* Reset the UART flags to original state */ @@ -215,13 +371,13 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) { - errx(0, "frsky_telemetry already running"); + errx(0, "sPort_telemetry already running"); } thread_should_exit = false; - frsky_task = px4_task_spawn_cmd("frsky_telemetry", + sPort_task = px4_task_spawn_cmd("sPort_telemetry", SCHED_DEFAULT, - SCHED_PRIORITY_DEFAULT, + 200, 2000, frsky_telemetry_thread_main, (char *const *)argv); @@ -237,7 +393,7 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (!thread_running) { - errx(0, "frsky_telemetry already stopped"); + errx(0, "sPort_telemetry already stopped"); } thread_should_exit = true; From 29d759768e1328e01be0eb685087a9b35c79643c Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 08:59:17 -0700 Subject: [PATCH 091/128] clean up structure --- src/drivers/frsky_telemetry/frsky_telemetry.c | 98 ++++++++++--------- 1 file changed, 50 insertions(+), 48 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index a3b1af4544..6661bff8ff 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -193,56 +193,13 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) /* Main thread loop */ char sbuf[20]; - /* look for polling frames indicating SmartPort telemetry + /* look for polling bytes indicating SmartPort telemetry * if not found, send D type telemetry frames instead */ int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - if (status < 1) { - /* timed out: reconfigure UART and send D type telemetry */ - warnx("SmartPort receiver not detected, sending FrSky D type telemetry"); - - status = set_uart_speed(uart, &uart_config, B9600); - if (status < 0) { - warnx("error setting speed for %s, quitting", device_name); - /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); - close(uart); - - thread_running = false; - return 0; - } - - int iteration = 0; - - /* Subscribe to topics */ - frsky_init(); - - while (!thread_should_exit) { - - /* Sleep 200 ms */ - usleep(200000); - - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); - - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); - } - - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); - - iteration = 0; - } - - iteration++; - } - - } else { - + if (status > 0) { + /* traffic on the port, assume it's a SmartPort master */ /* Subscribe to topics */ sPort_init(); @@ -340,12 +297,57 @@ static int frsky_telemetry_thread_main(int argc, char *argv[]) } break; + } + } + } else { + + /* timed out: reconfigure UART and send D type telemetry */ + warnx("SmartPort receiver not detected, sending FrSky D type telemetry"); + + status = set_uart_speed(uart, &uart_config, B9600); + + if (status < 0) { + warnx("error setting speed for %s, quitting", device_name); + /* Reset the UART flags to original state */ + tcsetattr(uart, TCSANOW, &uart_config_original); + close(uart); + + thread_running = false; + return 0; + } + + int iteration = 0; + + /* Subscribe to topics */ + frsky_init(); + + /* send D8 mode telemetry */ + while (!thread_should_exit) { + + /* Sleep 200 ms */ + usleep(200000); + + /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ + frsky_send_frame1(uart); + + /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ + if (iteration % 5 == 0) { + frsky_send_frame2(uart); } - /* TODO: flush the input buffer if in full duplex mode */ - read(uart, &sbuf[0], sizeof(sbuf)); + /* Send frame 3 (every 5000ms): date, time */ + if (iteration % 25 == 0) { + frsky_send_frame3(uart); + + iteration = 0; + } + + iteration++; } + + /* TODO: flush the input buffer if in full duplex mode */ + read(uart, &sbuf[0], sizeof(sbuf)); } /* Reset the UART flags to original state */ From 8f8b4485f154c7b95ab0520611e197814b3bf4b1 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 09:23:37 -0700 Subject: [PATCH 092/128] change rcS to start frsky_telemetry daemon for FMUv4 --- ROMFS/px4fmu_common/init.d/rcS | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 38430f67f9..8f87cecd79 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -843,7 +843,7 @@ fi if ver hwcmp PX4FMU_V4 then - sPort_telemetry start -d /dev/ttyS6 + frsky_telemetry start -d /dev/ttyS6 fi if ver hwcmp PX4FMU_V2 From c81c34b147ee0f654a241d23b5444f948854bc38 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 09:57:28 -0700 Subject: [PATCH 093/128] add sPort telemetry handling into frsky_telemetry daemon --- src/drivers/frsky_telemetry/sPort_data.c | 217 +++++++++++++++++++++++ src/drivers/frsky_telemetry/sPort_data.h | 87 +++++++++ 2 files changed, 304 insertions(+) create mode 100644 src/drivers/frsky_telemetry/sPort_data.c create mode 100644 src/drivers/frsky_telemetry/sPort_data.h diff --git a/src/drivers/frsky_telemetry/sPort_data.c b/src/drivers/frsky_telemetry/sPort_data.c new file mode 100644 index 0000000000..6af5ecc09e --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.c @@ -0,0 +1,217 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 sPort_data.c + * @author Stefan Rado + * @author Mark Whitehorn + * + * FrSky SmartPort telemetry implementation. + * + */ + +#include "sPort_data.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#define frac(f) (f - (int)f) + +static int battery_sub = -1; +static int sensor_sub = -1; +static int global_position_sub = -1; +static int vehicle_status_sub = -1; + +/** + * Initializes the uORB subscriptions. + */ +void sPort_init() +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); +} + +/** + * Sends a 0x10 start byte. + */ +//static void sPort_send_start(int uart) +//{ +// static const uint8_t c = 0x10; +// write(uart, &c, 1); +//} + +static void update_crc(uint16_t *crc, unsigned char b) +{ + *crc += b; + *crc += *crc >> 8; + *crc &= 0xFF; +} + +/** + * Sends one byte, performing byte-stuffing if necessary. + */ +static void sPort_send_byte(int uart, uint8_t value) +{ + const uint8_t x7E[] = { 0x7D, 0x5E }; + const uint8_t x7D[] = { 0x7D, 0x5D }; + + switch (value) { + case 0x7E: + write(uart, x7E, sizeof(x7E)); + break; + + case 0x7D: + write(uart, x7D, sizeof(x7D)); + break; + + default: + write(uart, &value, sizeof(value)); + break; + } +} + +/** + * Sends one data id/value pair. + */ +void sPort_send_data(int uart, uint16_t id, uint32_t data) +{ + union { + uint32_t word; + uint8_t byte[4]; + } buf; + + /* send start byte */ + static const uint8_t c = 0x10; + write(uart, &c, 1); + + /* init crc */ + uint16_t crc = c; + + buf.word = id; + + for (int i = 0; i < 2; i++) { + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ + } + + buf.word = data; + + for (int i = 0; i < 4; i++) { + update_crc(&crc, buf.byte[i]); + sPort_send_byte(uart, buf.byte[i]); /* LSB first */ + } + + sPort_send_byte(uart, 0xFF - crc); +} + + +// scaling correct with OpenTX 2.1.7 +void sPort_send_BATV(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send battery voltage as VFAS */ + uint32_t voltage = (int)(100 * vehicle_status.battery_voltage); + sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); +} + +// verified scaling +void sPort_send_CUR(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send data */ + uint32_t current = (int)(10 * vehicle_status.battery_current); + sPort_send_data(uart, SMARTPORT_ID_CURR, current); +} + +// verified scaling for "custom" altitude option +// OpenTX uses the initial reading as field elevation and displays +// the difference (altitude - field) +void sPort_send_ALT(int uart) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* send data */ + uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); + sPort_send_data(uart, SMARTPORT_ID_ALT, alt); +} + +// verified scaling for "calculated" option +void sPort_send_SPD(int uart) +{ + /* get a local copy of the global position data */ + struct vehicle_global_position_s global_pos; + memset(&global_pos, 0, sizeof(global_pos)); + orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); + + /* send data for A2 */ + float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); + uint32_t ispeed = (int)(10 * speed); + sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); +} + +// verified scaling +void sPort_send_FUEL(int uart) +{ + /* get a local copy of the vehicle status data */ + struct vehicle_status_s vehicle_status; + memset(&vehicle_status, 0, sizeof(vehicle_status)); + orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); + + /* send data */ + uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); + sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); +} diff --git a/src/drivers/frsky_telemetry/sPort_data.h b/src/drivers/frsky_telemetry/sPort_data.h new file mode 100644 index 0000000000..8173780b88 --- /dev/null +++ b/src/drivers/frsky_telemetry/sPort_data.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. + * Author: Stefan Rado + * + * 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 sPort_data.h + * @author Mark Whitehorn + * + * FrSky SmartPort telemetry implementation. + * + */ +#ifndef _SPORT_DATA_H +#define _SPORT_DATA_H + +#include + +/* FrSky SmartPort polling IDs captured from X4R */ +#define SMARTPORT_POLL_1 0x1B +#define SMARTPORT_POLL_2 0x34 +#define SMARTPORT_POLL_3 0x95 +#define SMARTPORT_POLL_4 0x16 +#define SMARTPORT_POLL_5 0xB7 + +/* FrSky SmartPort sensor IDs */ +#define SMARTPORT_ID_RSSI 0xf101 +#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX +#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX +#define SMARTPORT_ID_BATV 0xf104 +#define SMARTPORT_ID_SWR 0xf105 +#define SMARTPORT_ID_T1 0x0400 +#define SMARTPORT_ID_T2 0x0410 +#define SMARTPORT_ID_RPM 0x0500 +#define SMARTPORT_ID_FUEL 0x0600 +#define SMARTPORT_ID_ALT 0x0100 +#define SMARTPORT_ID_VARIO 0x0110 +#define SMARTPORT_ID_ACCX 0x0700 +#define SMARTPORT_ID_ACCY 0x0710 +#define SMARTPORT_ID_ACCZ 0x0720 +#define SMARTPORT_ID_CURR 0x0200 +#define SMARTPORT_ID_VFAS 0x0210 +#define SMARTPORT_ID_CELLS 0x0300 +#define SMARTPORT_ID_GPS_LON_LAT 0x0800 +#define SMARTPORT_ID_GPS_ALT 0x0820 +#define SMARTPORT_ID_GPS_SPD 0x0830 +#define SMARTPORT_ID_GPS_CRS 0x0840 +#define SMARTPORT_ID_GPS_TIME 0x0850 + +// Public functions +void sPort_init(void); +void sPort_send_data(int uart, uint16_t id, uint32_t data); +void sPort_send_BATV(int uart); +void sPort_send_CUR(int uart); +void sPort_send_ALT(int uart); +void sPort_send_SPD(int uart); +void sPort_send_FUEL(int uart); + +#endif /* _SPORT_TELEMETRY_H */ From 527fe4b37496a498bb8ad920d873f3aca35b761f Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 10:34:03 -0700 Subject: [PATCH 094/128] remove sPort_telemetry directory --- src/drivers/frsky_telemetry/CMakeLists.txt | 1 + src/drivers/frsky_telemetry/frsky_telemetry.c | 6 +- src/drivers/sPort_telemetry/CMakeLists.txt | 45 -- src/drivers/sPort_telemetry/sPort_data.c | 216 ---------- src/drivers/sPort_telemetry/sPort_data.h | 87 ---- src/drivers/sPort_telemetry/sPort_telemetry.c | 407 ------------------ 6 files changed, 4 insertions(+), 758 deletions(-) delete mode 100644 src/drivers/sPort_telemetry/CMakeLists.txt delete mode 100644 src/drivers/sPort_telemetry/sPort_data.c delete mode 100644 src/drivers/sPort_telemetry/sPort_data.h delete mode 100644 src/drivers/sPort_telemetry/sPort_telemetry.c diff --git a/src/drivers/frsky_telemetry/CMakeLists.txt b/src/drivers/frsky_telemetry/CMakeLists.txt index 7d6c17f463..a35310aaf7 100644 --- a/src/drivers/frsky_telemetry/CMakeLists.txt +++ b/src/drivers/frsky_telemetry/CMakeLists.txt @@ -38,6 +38,7 @@ px4_add_module( -Os SRCS frsky_data.c + sPort_data.c frsky_telemetry.c DEPENDS platforms__common diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 6661bff8ff..90df63eea0 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -59,13 +59,13 @@ #include #include "sPort_data.h" -#include "../frsky_telemetry/frsky_data.h" +#include "frsky_data.h" /* thread state */ static volatile bool thread_should_exit = false; static volatile bool thread_running = false; -static int sPort_task; +static int frsky_task; /* functions */ static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); @@ -377,7 +377,7 @@ int frsky_telemetry_main(int argc, char *argv[]) } thread_should_exit = false; - sPort_task = px4_task_spawn_cmd("sPort_telemetry", + frsky_task = px4_task_spawn_cmd("frsky_telemetry", SCHED_DEFAULT, 200, 2000, diff --git a/src/drivers/sPort_telemetry/CMakeLists.txt b/src/drivers/sPort_telemetry/CMakeLists.txt deleted file mode 100644 index d19291720d..0000000000 --- a/src/drivers/sPort_telemetry/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ -px4_add_module( - MODULE drivers__sPort_telemetry - MAIN sPort_telemetry - STACK 1200 - COMPILE_FLAGS - -Os - SRCS - sPort_data.c - sPort_telemetry.c - DEPENDS - platforms__common - ) -# vim: set noet ft=cmake fenc=utf-8 ff=unix : diff --git a/src/drivers/sPort_telemetry/sPort_data.c b/src/drivers/sPort_telemetry/sPort_data.c deleted file mode 100644 index 5267f44f61..0000000000 --- a/src/drivers/sPort_telemetry/sPort_data.c +++ /dev/null @@ -1,216 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_data.c - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - */ - -#include "sPort_data.h" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include - -#define frac(f) (f - (int)f) - -static int battery_sub = -1; -static int sensor_sub = -1; -static int global_position_sub = -1; -static int vehicle_status_sub = -1; - -/** - * Initializes the uORB subscriptions. - */ -void sPort_init() -{ - battery_sub = orb_subscribe(ORB_ID(battery_status)); - global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); -} - -/** - * Sends a 0x10 start byte. - */ -//static void sPort_send_start(int uart) -//{ -// static const uint8_t c = 0x10; -// write(uart, &c, 1); -//} - -static void update_crc(uint16_t *crc, unsigned char b) -{ - *crc += b; - *crc += *crc >> 8; - *crc &= 0xFF; -} - -/** - * Sends one byte, performing byte-stuffing if necessary. - */ -static void sPort_send_byte(int uart, uint8_t value) -{ - const uint8_t x7E[] = { 0x7D, 0x5E }; - const uint8_t x7D[] = { 0x7D, 0x5D }; - - switch (value) { - case 0x7E: - write(uart, x7E, sizeof(x7E)); - break; - - case 0x7D: - write(uart, x7D, sizeof(x7D)); - break; - - default: - write(uart, &value, sizeof(value)); - break; - } -} - -/** - * Sends one data id/value pair. - */ -void sPort_send_data(int uart, uint16_t id, uint32_t data) -{ - union { - uint32_t word; - uint8_t byte[4]; - } buf; - - /* send start byte */ - static const uint8_t c = 0x10; - write(uart, &c, 1); - - /* init crc */ - uint16_t crc = c; - - buf.word = id; - - for (int i = 0; i < 2; i++) { - update_crc(&crc, buf.byte[i]); - sPort_send_byte(uart, buf.byte[i]); /* LSB first */ - } - - buf.word = data; - - for (int i = 0; i < 4; i++) { - update_crc(&crc, buf.byte[i]); - sPort_send_byte(uart, buf.byte[i]); /* LSB first */ - } - - sPort_send_byte(uart, 0xFF - crc); -} - - -// scaling correct with OpenTX 2.1.7 -void sPort_send_BATV(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send battery voltage as VFAS */ - uint32_t voltage = (int)(100 * vehicle_status.battery_voltage); - sPort_send_data(uart, SMARTPORT_ID_VFAS, voltage); -} - -// verified scaling -void sPort_send_CUR(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send data */ - uint32_t current = (int)(10 * vehicle_status.battery_current); - sPort_send_data(uart, SMARTPORT_ID_CURR, current); -} - -// verified scaling for "custom" altitude option -// OpenTX uses the initial reading as field elevation and displays -// the difference (altitude - field) -void sPort_send_ALT(int uart) -{ - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* send data */ - uint32_t alt = (int)(100 * raw.baro_alt_meter[0]); - sPort_send_data(uart, SMARTPORT_ID_ALT, alt); -} - -// verified scaling for "calculated" option -void sPort_send_SPD(int uart) -{ - /* get a local copy of the global position data */ - struct vehicle_global_position_s global_pos; - memset(&global_pos, 0, sizeof(global_pos)); - orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_pos); - - /* send data for A2 */ - float speed = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e); - uint32_t ispeed = (int)(10 * speed); - sPort_send_data(uart, SMARTPORT_ID_GPS_SPD, ispeed); -} - -// verified scaling -void sPort_send_FUEL(int uart) -{ - /* get a local copy of the vehicle status data */ - struct vehicle_status_s vehicle_status; - memset(&vehicle_status, 0, sizeof(vehicle_status)); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vehicle_status); - - /* send data */ - uint32_t fuel = (int)(100 * vehicle_status.battery_remaining); - sPort_send_data(uart, SMARTPORT_ID_FUEL, fuel); -} diff --git a/src/drivers/sPort_telemetry/sPort_data.h b/src/drivers/sPort_telemetry/sPort_data.h deleted file mode 100644 index 895fde3465..0000000000 --- a/src/drivers/sPort_telemetry/sPort_data.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_data.h - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - */ -#ifndef _SPORT_DATA_H -#define _SPORT_DATA_H - -#include - -/* FrSky SmartPort polling IDs captured from X4R */ -#define SMARTPORT_POLL_1 0x1B -#define SMARTPORT_POLL_2 0x34 -#define SMARTPORT_POLL_3 0x95 -#define SMARTPORT_POLL_4 0x16 -#define SMARTPORT_POLL_5 0xB7 - -/* FrSky SmartPort sensor IDs */ -#define SMARTPORT_ID_RSSI 0xf101 -#define SMARTPORT_ID_RXA1 0xf102 // supplied by RX -#define SMARTPORT_ID_RXA2 0xf103 // supplied by RX -#define SMARTPORT_ID_BATV 0xf104 -#define SMARTPORT_ID_SWR 0xf105 -#define SMARTPORT_ID_T1 0x0400 -#define SMARTPORT_ID_T2 0x0410 -#define SMARTPORT_ID_RPM 0x0500 -#define SMARTPORT_ID_FUEL 0x0600 -#define SMARTPORT_ID_ALT 0x0100 -#define SMARTPORT_ID_VARIO 0x0110 -#define SMARTPORT_ID_ACCX 0x0700 -#define SMARTPORT_ID_ACCY 0x0710 -#define SMARTPORT_ID_ACCZ 0x0720 -#define SMARTPORT_ID_CURR 0x0200 -#define SMARTPORT_ID_VFAS 0x0210 -#define SMARTPORT_ID_CELLS 0x0300 -#define SMARTPORT_ID_GPS_LON_LAT 0x0800 -#define SMARTPORT_ID_GPS_ALT 0x0820 -#define SMARTPORT_ID_GPS_SPD 0x0830 -#define SMARTPORT_ID_GPS_CRS 0x0840 -#define SMARTPORT_ID_GPS_TIME 0x0850 - -// Public functions -void sPort_init(void); -void sPort_send_data(int uart, uint16_t id, uint32_t data); -void sPort_send_BATV(int uart); -void sPort_send_CUR(int uart); -void sPort_send_ALT(int uart); -void sPort_send_SPD(int uart); -void sPort_send_FUEL(int uart); - -#endif /* _SPORT_TELEMETRY_H */ diff --git a/src/drivers/sPort_telemetry/sPort_telemetry.c b/src/drivers/sPort_telemetry/sPort_telemetry.c deleted file mode 100644 index 943f3c89fc..0000000000 --- a/src/drivers/sPort_telemetry/sPort_telemetry.c +++ /dev/null @@ -1,407 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2014 PX4 Development Team. All rights reserved. - * Author: Stefan Rado - * - * 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 sPort_telemetry.c - * @author Mark Whitehorn - * @author Stefan Rado - * - * FrSky SmartPort telemetry implementation. - * - * This daemon emulates FrSky SmartPort sensors by responding to polling - * packets received from an attached FrSky X series receiver. - * - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "sPort_data.h" -#include "../frsky_telemetry/frsky_data.h" - - -/* thread state */ -static volatile bool thread_should_exit = false; -static volatile bool thread_running = false; -static int sPort_task; - -/* functions */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original); -static void usage(void); -static int sPort_telemetry_thread_main(int argc, char *argv[]); -__EXPORT int sPort_telemetry_main(int argc, char *argv[]); - - -/** - * Opens the UART device and sets all required serial parameters. - */ -static int sPort_open_uart(const char *uart_name, struct termios *uart_config, struct termios *uart_config_original) -{ - /* Open UART */ - const int uart = open(uart_name, O_RDWR | O_NOCTTY | O_NONBLOCK); - - if (uart < 0) { - err(1, "Error opening port: %s", uart_name); - } - - /* Back up the original UART configuration to restore it after exit */ - int termios_state; - - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state); - close(uart); - return -1; - } - - /* Fill the struct for the new configuration */ - tcgetattr(uart, uart_config); - - /* Disable output post-processing */ - uart_config->c_oflag &= ~OPOST; - - /* Set baud rate */ - static const speed_t speed = B57600; - - if (cfsetispeed(uart_config, speed) < 0 || cfsetospeed(uart_config, speed) < 0) { - warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state); - close(uart); - return -1; - } - - if ((termios_state = tcsetattr(uart, TCSANOW, uart_config)) < 0) { - warnx("ERR: %s (tcsetattr)\n", uart_name); - close(uart); - return -1; - } - - return uart; -} - -/** - * Print command usage information - */ -static void usage() -{ - fprintf(stderr, - "usage: sPort_telemetry start [-d ]\n" - " sPort_telemetry stop\n" - " sPort_telemetry status\n"); - exit(1); -} - -/** - * The daemon thread. - */ -static int sPort_telemetry_thread_main(int argc, char *argv[]) -{ - /* Default values for arguments */ - char *device_name = "/dev/ttyS6"; /* USART8 */ - - /* Work around some stupidity in task_create's argv handling */ - argc -= 2; - argv += 2; - - int ch; - - while ((ch = getopt(argc, argv, "d:")) != EOF) { - switch (ch) { - case 'd': - device_name = optarg; - break; - - default: - usage(); - break; - } - } - - /* Open UART */ - warnx("opening uart"); - struct termios uart_config_original; - struct termios uart_config; - const int uart = sPort_open_uart(device_name, &uart_config, &uart_config_original); - - if (uart < 0) { - warnx("could not open %s", device_name); - err(1, "could not open %s", device_name); - } - - /* poll descriptor */ - struct pollfd fds[1]; - fds[0].fd = uart; - fds[0].events = POLLIN; - - thread_running = true; - - /* Main thread loop */ - char sbuf[20]; - - /* look for polling frames indicating SmartPort telemetry - * if not found, send D type telemetry frames instead - */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 3000); - - if (status < 1) { - /* timed out: reconfigure UART and send D type telemetry */ - warnx("sending FrSky D type telemetry"); - - if (cfsetispeed(&uart_config, B9600) < 0 || cfsetospeed(&uart_config, B9600) < 0) { - warnx("ERR: %s:(cfsetispeed, cfsetospeed)\n", device_name); - close(uart); - return -1; - } - - if (tcsetattr(uart, TCSANOW, &uart_config) < 0) { - warnx("ERR: %s (tcsetattr)\n", device_name); - close(uart); - return -1; - } - - int iteration = 0; - - /* Subscribe to topics */ - frsky_init(); - - while (!thread_should_exit) { - - /* Sleep 200 ms */ - usleep(200000); - - /* Send frame 1 (every 200ms): acceleration values, altitude (vario), temperatures, current & voltages, RPM */ - frsky_send_frame1(uart); - - /* Send frame 2 (every 1000ms): course, latitude, longitude, speed, altitude (GPS), fuel level */ - if (iteration % 5 == 0) { - frsky_send_frame2(uart); - } - - /* Send frame 3 (every 5000ms): date, time */ - if (iteration % 25 == 0) { - frsky_send_frame3(uart); - - iteration = 0; - } - - iteration++; - } - - } else { - /* Subscribe to topics */ - sPort_init(); - - /* send S.port telemetry */ - while (!thread_should_exit) { - - /* wait for poll frame starting with value 0x7E - * note that only the bus master is supposed to put a 0x7E on the bus. - * slaves use byte stuffing to send 0x7E and 0x7D. - * we expect a poll frame every 12msec - */ - int status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - // read 1 byte - int newBytes = read(uart, &sbuf[0], 1); - - if (newBytes < 1 || sbuf[0] != 0x7E) { continue; } - - /* wait for ID byte */ - status = poll(fds, sizeof(fds) / sizeof(fds[0]), 50); - - if (status < 1) { continue; } - - hrt_abstime now = hrt_absolute_time(); - - newBytes = read(uart, &sbuf[1], 1); - - // allow a minimum of 500usec before reply - usleep(500); - - static hrt_abstime lastBATV = 0; - static hrt_abstime lastCUR = 0; - static hrt_abstime lastALT = 0; - static hrt_abstime lastSPD = 0; - static hrt_abstime lastFUEL = 0; - - switch (sbuf[1]) { - - case SMARTPORT_POLL_1: - - /* report BATV at 1Hz */ - if (now - lastBATV > 1000 * 1000) { - lastBATV = now; - /* send battery voltage */ - sPort_send_BATV(uart); - } - - break; - - - case SMARTPORT_POLL_2: - - /* report battery current at 5Hz */ - if (now - lastCUR > 200 * 1000) { - lastCUR = now; - /* send battery current */ - sPort_send_CUR(uart); - } - - break; - - - case SMARTPORT_POLL_3: - - /* report altitude at 5Hz */ - if (now - lastALT > 200 * 1000) { - lastALT = now; - /* send altitude */ - sPort_send_ALT(uart); - } - - break; - - - case SMARTPORT_POLL_4: - - /* report speed at 5Hz */ - if (now - lastSPD > 200 * 1000) { - lastSPD = now; - /* send speed */ - sPort_send_SPD(uart); - } - - break; - - case SMARTPORT_POLL_5: - - /* report fuel at 1Hz */ - if (now - lastFUEL > 1000 * 1000) { - lastFUEL = now; - /* send fuel */ - sPort_send_FUEL(uart); - } - - break; - - } - - /* TODO: flush the input buffer if in full duplex mode */ - read(uart, &sbuf[0], sizeof(sbuf)); - } - } - - /* Reset the UART flags to original state */ - tcsetattr(uart, TCSANOW, &uart_config_original); - close(uart); - - thread_running = false; - return 0; -} - -/** - * The main command function. - * Processes command line arguments and starts the daemon. - */ -int sPort_telemetry_main(int argc, char *argv[]) -{ - if (argc < 2) { - warnx("missing command"); - usage(); - } - - if (!strcmp(argv[1], "start")) { - - /* this is not an error */ - if (thread_running) { - errx(0, "sPort_telemetry already running"); - } - - thread_should_exit = false; - sPort_task = px4_task_spawn_cmd("sPort_telemetry", - SCHED_DEFAULT, - 200, - 2000, - sPort_telemetry_thread_main, - (char *const *)argv); - - while (!thread_running) { - usleep(200); - } - - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - - /* this is not an error */ - if (!thread_running) { - errx(0, "sPort_telemetry already stopped"); - } - - thread_should_exit = true; - - while (thread_running) { - usleep(200000); - warnx("."); - } - - warnx("terminated."); - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - errx(0, "running"); - - } else { - errx(1, "not running"); - } - } - - warnx("unrecognized command"); - usage(); - /* not getting here */ - return 0; -} From cfc3af8374ed62ff42f808ccb400ebbfdacddf23 Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 15:01:22 -0700 Subject: [PATCH 095/128] update fmu-v4 cmake config --- cmake/configs/nuttx_px4fmu-v4_default.cmake | 1 - 1 file changed, 1 deletion(-) diff --git a/cmake/configs/nuttx_px4fmu-v4_default.cmake b/cmake/configs/nuttx_px4fmu-v4_default.cmake index 6ed03f88ec..8bb4005353 100644 --- a/cmake/configs/nuttx_px4fmu-v4_default.cmake +++ b/cmake/configs/nuttx_px4fmu-v4_default.cmake @@ -35,7 +35,6 @@ set(config_module_list drivers/ets_airspeed drivers/meas_airspeed drivers/frsky_telemetry - drivers/sPort_telemetry modules/sensors drivers/mkblctrl drivers/px4flow From d96a17adfb37717c3ee9465221ec8ed657e7973e Mon Sep 17 00:00:00 2001 From: Mark Whitehorn Date: Sat, 23 Jan 2016 16:18:33 -0700 Subject: [PATCH 096/128] missed some name changes from sPort to frsky --- src/drivers/frsky_telemetry/frsky_telemetry.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c index 90df63eea0..d527696501 100644 --- a/src/drivers/frsky_telemetry/frsky_telemetry.c +++ b/src/drivers/frsky_telemetry/frsky_telemetry.c @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file sPort_telemetry.c + * @file frsky_telemetry.c * @author Stefan Rado * @author Mark Whitehorn * @@ -140,9 +140,9 @@ static int set_uart_speed(int uart, struct termios *uart_config, speed_t speed) static void usage() { fprintf(stderr, - "usage: sPort_telemetry start [-d ]\n" - " sPort_telemetry stop\n" - " sPort_telemetry status\n"); + "usage: frsky_telemetry start [-d ]\n" + " frsky_telemetry stop\n" + " frsky_telemetry status\n"); exit(1); } @@ -373,7 +373,7 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (thread_running) { - errx(0, "sPort_telemetry already running"); + errx(0, "frsky_telemetry already running"); } thread_should_exit = false; @@ -395,7 +395,7 @@ int frsky_telemetry_main(int argc, char *argv[]) /* this is not an error */ if (!thread_running) { - errx(0, "sPort_telemetry already stopped"); + errx(0, "frsky_telemetry already stopped"); } thread_should_exit = true; From 69d4b1b6920736e99ecfb1d9ff027f1da6a050c6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 13:22:25 +0100 Subject: [PATCH 097/128] Fix config meta data --- ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol index a9e7ff35b2..cb4f4d5ac3 100644 --- a/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol +++ b/ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol @@ -2,7 +2,7 @@ # # @name Duorotor Tailsitter # -# @type VTOL Tailsitter +# @type VTOL Duo Tailsitter # # @maintainer Roman Bapst # From fb216548071e4ad636a24af8010daf634c3d68bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 13:22:45 +0100 Subject: [PATCH 098/128] PX4 airframes: Fix XML parser --- Tools/px4airframes/xmlout.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/px4airframes/xmlout.py b/Tools/px4airframes/xmlout.py index 841fabd4b5..bd3ac1436a 100644 --- a/Tools/px4airframes/xmlout.py +++ b/Tools/px4airframes/xmlout.py @@ -59,7 +59,7 @@ class XMLOutput(): xml_group.attrib["image"] = "VTOLPlane" elif (group.GetName() == "VTOL Quad Tailsitter"): xml_group.attrib["image"] = "VTOLQuadRotorTailSitter" - elif (group.GetName() == "VTOLTiltRotor"): + elif (group.GetName() == "VTOL Tiltrotor"): xml_group.attrib["image"] = "VTOLTiltRotor" elif (group.GetName() == "Coaxial Helicopter"): xml_group.attrib["image"] = "HelicopterCoaxial" From 06b496e2578957ee73cfe4c013f2fe264d757e58 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 14:08:41 +0100 Subject: [PATCH 099/128] Navigator: Only set acceptance radius based on navigation capabilities for fixed wing flight. --- 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 b16f9b9b91..2bba051573 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -616,7 +616,11 @@ Navigator::get_acceptance_radius(float mission_item_radius) { float radius = mission_item_radius; - if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { + // XXX only use navigation capabilities for now + // when in fixed wing mode + // this might need locking against a commanded transition + // so that a stale _vstatus doesn't trigger an accepted mission item. + if (!_vstatus.is_rotary_wing && !_vstatus.in_transition_mode && hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) { if (_nav_caps.turn_distance > radius) { radius = _nav_caps.turn_distance; } From a284b77c7b72b2f291878990f0e5ad17750273ab Mon Sep 17 00:00:00 2001 From: Artem Sabadyr Date: Sun, 24 Jan 2016 15:30:52 +0300 Subject: [PATCH 100/128] gps advertise fix --- src/drivers/gps/gps.cpp | 38 +++++++------------------------------- 1 file changed, 7 insertions(+), 31 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 05ff3e5b91..4ecf4d661e 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -363,36 +363,7 @@ GPS::task_main() /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - if (_mode == GPS_DRIVER_MODE_UBX) { - /* Publish initial report that we have access to a GPS, - * but set all critical state fields to indicate we have - * no valid position lock - */ - - _report_gps_pos.timestamp_time = hrt_absolute_time(); - - /* reset the timestamps for data, because we have no data yet */ - _report_gps_pos.timestamp_position = 0; - _report_gps_pos.timestamp_variance = 0; - _report_gps_pos.timestamp_velocity = 0; - - /* set a massive variance */ - _report_gps_pos.eph = 10000.0f; - _report_gps_pos.epv = 10000.0f; - _report_gps_pos.fix_type = 0; - - if (!(_pub_blocked)) { - if (_report_gps_pos_pub != nullptr) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); - - } else { - _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); - } - } - - /* GPS is obviously detected successfully, reset statistics */ - _Helper->reset_update_rates(); - } + _Helper->reset_update_rates(); int helper_ret; @@ -402,7 +373,12 @@ GPS::task_main() if (!(_pub_blocked)) { if (helper_ret & 1) { - orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } } if (_p_report_sat_info && (helper_ret & 2)) { From 9169a585bf52ac287ec2783b6d6d2a0dc0a262c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 15:21:17 +0100 Subject: [PATCH 101/128] Deprecate ROS target --- Makefile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Makefile b/Makefile index 25b5fa5dfc..ef7db710dd 100644 --- a/Makefile +++ b/Makefile @@ -154,7 +154,8 @@ posix_sitl_ekf2: $(call cmake-build,$@) ros_sitl_default: - $(call cmake-build,$@) + @echo "This target is deprecated. Use make 'posix_sitl_default gazebo' instead." +# $(call cmake-build,$@) qurt_eagle_travis: $(call cmake-build,$@) From 6e26d1b8dc9cf7b97e2c51fd2fff75264f4a958e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Jan 2016 08:24:23 +0100 Subject: [PATCH 102/128] px_romfs_pruner.py: ignore files starting with . --- Tools/px_romfs_pruner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index a7984c66dd..f77731be9c 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -57,7 +57,8 @@ def main(): for (root, dirs, files) in os.walk(args.folder): for file in files: # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or ".DS_Store" in file: + if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or \ + ".DS_Store" in file or file.startswith("."): continue file_path = os.path.join(root, file) From 443592136b75509a93ee81d99bff7f760634b2bb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Jan 2016 08:41:35 +0100 Subject: [PATCH 103/128] px_romfs_pruner.py: PEP8ify and whitespace - Changed from 8 spaces indent back to 4 which I find appropriate for Python. - Fixed linelength to 80 chars. --- Tools/px_romfs_pruner.py | 71 +++++++++++++++++++++------------------- 1 file changed, 37 insertions(+), 34 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index f77731be9c..b0235e577b 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -1,8 +1,7 @@ #!/usr/bin/env python ############################################################################ # -# Copyright (C) 2014 PX4 Development Team. All rights reserved. -# Author: Julian Oes +# Copyright (C) 2014-2015 PX4 Development Team. All rights reserved. # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,49 +36,53 @@ """ px_romfs_pruner.py: Delete all comments and newlines before ROMFS is converted to an image + +@author: Julian Oes """ from __future__ import print_function -import argparse, re +import argparse +import re import os def main(): + # Parse commandline arguments + parser = argparse.ArgumentParser(description="ROMFS pruner.") + parser.add_argument('--folder', action="store", + help="ROMFS scratch folder.") + args = parser.parse_args() - # Parse commandline arguments - parser = argparse.ArgumentParser(description="ROMFS pruner.") - parser.add_argument('--folder', action="store", help="ROMFS scratch folder.") - args = parser.parse_args() + print("Pruning ROMFS files.") - print("Pruning ROMFS files.") + # go through + for (root, dirs, files) in os.walk(args.folder): + for file in files: + # only prune text files + if ".zip" in file or ".bin" in file or ".swp" in file \ + or ".data" in file or ".DS_Store" in file \ + or file.startswith("."): + continue - # go through - for (root, dirs, files) in os.walk(args.folder): - for file in files: - # only prune text files - if ".zip" in file or ".bin" in file or ".swp" in file or ".data" in file or \ - ".DS_Store" in file or file.startswith("."): - continue + file_path = os.path.join(root, file) - file_path = os.path.join(root, file) - - # read file line by line - pruned_content = "" - with open(file_path, "rU") as f: - for line in f: - - # handle mixer files differently than startup files - if file_path.endswith(".mix"): - if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): - pruned_content += line - else: - if not line.isspace() and not line.strip().startswith("#"): - pruned_content += line - # overwrite old scratch file - with open(file_path, "wb") as f: - pruned_content = re.sub("\r\n", "\n", pruned_content) - f.write(pruned_content.encode("ascii", errors='strict')) + # read file line by line + pruned_content = "" + with open(file_path, "rU") as f: + for line in f: + # handle mixer files differently than startup files + if file_path.endswith(".mix"): + if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): + pruned_content += line + else: + if not line.isspace() \ + and not line.strip().startswith("#"): + pruned_content += line + # overwrite old scratch file + with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) + f.write(pruned_content.encode("ascii", errors='strict')) if __name__ == '__main__': - main() + main() From 4b55c5276e2ce58d8cb2e20c48b45cb452f846a6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 24 Jan 2016 09:45:30 +0100 Subject: [PATCH 104/128] px_romfs_pruner.py: fix indenting --- Tools/px_romfs_pruner.py | 34 +++++++++++++++++----------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/Tools/px_romfs_pruner.py b/Tools/px_romfs_pruner.py index b0235e577b..67f0b57cbf 100644 --- a/Tools/px_romfs_pruner.py +++ b/Tools/px_romfs_pruner.py @@ -64,24 +64,24 @@ def main(): or file.startswith("."): continue - file_path = os.path.join(root, file) + file_path = os.path.join(root, file) - # read file line by line - pruned_content = "" - with open(file_path, "rU") as f: - for line in f: - # handle mixer files differently than startup files - if file_path.endswith(".mix"): - if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): - pruned_content += line - else: - if not line.isspace() \ - and not line.strip().startswith("#"): - pruned_content += line - # overwrite old scratch file - with open(file_path, "wb") as f: - pruned_content = re.sub("\r\n", "\n", pruned_content) - f.write(pruned_content.encode("ascii", errors='strict')) + # read file line by line + pruned_content = "" + with open(file_path, "rU") as f: + for line in f: + # handle mixer files differently than startup files + if file_path.endswith(".mix"): + if line.startswith(("Z:", "M:", "R: ", "O:", "S:")): + pruned_content += line + else: + if not line.isspace() \ + and not line.strip().startswith("#"): + pruned_content += line + # overwrite old scratch file + with open(file_path, "wb") as f: + pruned_content = re.sub("\r\n", "\n", pruned_content) + f.write(pruned_content.encode("ascii", errors='strict')) if __name__ == '__main__': From b2237ce525fe45f67aed945b6b9f8fe1bcf58ca9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 16:02:21 +0100 Subject: [PATCH 105/128] Current scaling: Employ per-board defaults --- src/modules/sensors/sensor_params.c | 2 +- src/modules/sensors/sensors.cpp | 16 ++++++++++++++++ 2 files changed, 17 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dc081f0644..d2faa1a4f5 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -1842,7 +1842,7 @@ PARAM_DEFINE_FLOAT(BAT_V_SCALING, -1.0f); * * @group Battery Calibration */ -PARAM_DEFINE_FLOAT(BAT_C_SCALING, 0.0124); /* scaling for 3DR power brick */ +PARAM_DEFINE_FLOAT(BAT_C_SCALING, -1.0); /** diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index f41c9dcf4b..79a2b54a63 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -883,6 +883,22 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { warnx("%s", paramerr); + } else if (_parameters.battery_current_scaling < 0.0f) { + /* apply scaling according to defaults if set to default */ +#if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) + /* current scaling for ACSP4 */ + _parameters.battery_current_scaling = 0.029296875f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V2) + /* current scaling for 3DR power brick */ + _parameters.battery_current_scaling = 0.0124f; +#elif defined (CONFIG_ARCH_BOARD_AEROCORE) + _parameters.battery_current_scaling = 0.0124f; +#elif defined (CONFIG_ARCH_BOARD_PX4FMU_V1) + _parameters.battery_current_scaling = 0.0124f; +#else + /* ensure a missing default leads to an unrealistic current value */ + _parameters.battery_voltage_scaling = 0.00001f; +#endif } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); From 737fe1fc7f689456722a56cbd21bbb1844771931 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 16:30:21 +0100 Subject: [PATCH 106/128] Always perform yaw SP generation, not only in multicopter mode --- src/modules/navigator/mission.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index ff76b5620d..495c17c093 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -193,9 +193,8 @@ Mission::on_active() } } - /* see if we need to update the current yaw heading for rotary wing types */ - if (_navigator->get_vstatus()->is_rotary_wing - && _param_yawmode.get() != MISSION_YAWMODE_NONE + /* see if we need to update the current yaw heading */ + if (_param_yawmode.get() != MISSION_YAWMODE_NONE && _param_yawmode.get() < MISSION_YAWMODE_MAX && _mission_type != MISSION_TYPE_NONE) { heading_sp_update(); From 081da8bb7f37c0a6a7323d85e19c0fd5b4283eeb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 16:34:36 +0100 Subject: [PATCH 107/128] Navigator: Force yaw pointing towards waypoint for all cases --- src/modules/navigator/mission.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 495c17c093..cda6aaeef3 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -194,9 +194,10 @@ Mission::on_active() } /* see if we need to update the current yaw heading */ - if (_param_yawmode.get() != MISSION_YAWMODE_NONE + if ((_param_yawmode.get() != MISSION_YAWMODE_NONE && _param_yawmode.get() < MISSION_YAWMODE_MAX - && _mission_type != MISSION_TYPE_NONE) { + && _mission_type != MISSION_TYPE_NONE) + || _navigator->get_vstatus()->is_vtol) { heading_sp_update(); } @@ -566,7 +567,8 @@ Mission::heading_sp_update() } /* always keep the front of the rotary wing pointing to the next waypoint */ - if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT + || _navigator->get_vstatus()->is_vtol) { _mission_item.yaw = get_bearing_to_next_waypoint( point_from_latlon[0], point_from_latlon[1], From 6eac78d67529e20c3170092fd271c11c57c70f2f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 24 Jan 2016 16:34:49 +0100 Subject: [PATCH 108/128] Sensors: Code style fix --- src/modules/sensors/sensors.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 79a2b54a63..60c4fa5e6e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -883,6 +883,7 @@ Sensors::parameters_update() /* scaling of ADC ticks to battery current */ if (param_get(_parameter_handles.battery_current_scaling, &(_parameters.battery_current_scaling)) != OK) { warnx("%s", paramerr); + } else if (_parameters.battery_current_scaling < 0.0f) { /* apply scaling according to defaults if set to default */ #if defined (CONFIG_ARCH_BOARD_PX4FMU_V4) @@ -898,7 +899,7 @@ Sensors::parameters_update() #else /* ensure a missing default leads to an unrealistic current value */ _parameters.battery_voltage_scaling = 0.00001f; -#endif +#endif } param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation)); From 9cccc0ec76ad313dadd9310637dc342b3c217b1f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 09:30:45 +0100 Subject: [PATCH 109/128] GPS: Do initial zero value publication to avoid corner cases --- src/drivers/gps/gps.cpp | 31 ++++++++++++++++++++++++++++++- 1 file changed, 30 insertions(+), 1 deletion(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 4ecf4d661e..758cb3f320 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -363,7 +363,36 @@ GPS::task_main() /* reset report */ memset(&_report_gps_pos, 0, sizeof(_report_gps_pos)); - _Helper->reset_update_rates(); + if (_mode == GPS_DRIVER_MODE_UBX) { + /* Publish initial report that we have access to a GPS, + * but set all critical state fields to indicate we have + * no valid position lock + */ + + _report_gps_pos.timestamp_time = hrt_absolute_time(); + + /* reset the timestamps for data, because we have no data yet */ + _report_gps_pos.timestamp_position = 0; + _report_gps_pos.timestamp_variance = 0; + _report_gps_pos.timestamp_velocity = 0; + + /* set a massive variance */ + _report_gps_pos.eph = 10000.0f; + _report_gps_pos.epv = 10000.0f; + _report_gps_pos.fix_type = 0; + + if (!(_pub_blocked)) { + if (_report_gps_pos_pub != nullptr) { + orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); + + } else { + _report_gps_pos_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report_gps_pos); + } + } + + /* GPS is obviously detected successfully, reset statistics */ + _Helper->reset_update_rates(); + } int helper_ret; From d32d533d11f3a51fd9e72cf4290cab9caddcfce3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 12:07:38 +0100 Subject: [PATCH 110/128] FMUv4: Disable unused ADC channels --- src/drivers/boards/px4fmu-v4/px4fmu_init.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_init.c b/src/drivers/boards/px4fmu-v4/px4fmu_init.c index 55327c1605..21e1777cbd 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_init.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_init.c @@ -219,10 +219,6 @@ __EXPORT int nsh_archinitialize(void) stm32_configgpio(GPIO_ADC1_IN3); /* BATT_CURRENT_SENS */ stm32_configgpio(GPIO_ADC1_IN4); /* VDD_5V_SENS */ - stm32_configgpio(GPIO_ADC1_IN13); /* FMU_AUX_ADC_1 */ - stm32_configgpio(GPIO_ADC1_IN14); /* FMU_AUX_ADC_2 */ - stm32_configgpio(GPIO_ADC1_IN15); /* PRESSURE_SENS */ - /* configure power supply control/sense pins */ stm32_configgpio(GPIO_PERIPH_3V3_EN); stm32_configgpio(GPIO_VDD_BRICK_VALID); From 2e8accc6ffd5e1c2f37d31b09d6968a1e4af31ba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 12:08:08 +0100 Subject: [PATCH 111/128] FMU driver: Only init pins right before using the for PWM. Prevents accidental pulses --- src/drivers/px4fmu/fmu.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 7c125d75a1..67f9bec9ad 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -161,6 +161,8 @@ private: volatile bool _initialized; bool _throttle_armed; bool _pwm_on; + bool _pwm_init; + uint8_t _pwm_channel_mask; MixerGroup *_mixers; @@ -313,6 +315,8 @@ PX4FMU::PX4FMU() : _initialized(false), _throttle_armed(false), _pwm_on(false), + _pwm_init(false), + _pwm_channel_mask(0), _mixers(nullptr), _groups_required(0), _groups_subscribed(0), @@ -418,10 +422,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3; break; @@ -432,10 +433,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xf); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xf; break; @@ -446,10 +444,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0x3f); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0x3f; break; @@ -461,10 +456,7 @@ PX4FMU::set_mode(Mode mode) _pwm_default_rate = 50; _pwm_alt_rate = 50; _pwm_alt_rate_channels = 0; - - /* XXX magic numbers */ - up_pwm_servo_init(0xff); - set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_channel_mask = 0xff; break; #endif @@ -477,6 +469,7 @@ PX4FMU::set_mode(Mode mode) /* disable servo outputs - no need to set rates */ up_pwm_servo_deinit(); + _pwm_init = false; break; @@ -873,6 +866,13 @@ PX4FMU::cycle() if (_pwm_on != pwm_on) { _pwm_on = pwm_on; + + if (!_pwm_init && _pwm_on) { + up_pwm_servo_init(_pwm_channel_mask); + set_pwm_rate(_pwm_alt_rate_channels, _pwm_default_rate, _pwm_alt_rate); + _pwm_init = true; + } + up_pwm_servo_arm(pwm_on); } } From 9d5728b96ff7020b9b78acfa1d98391b89002f8f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 12:08:26 +0100 Subject: [PATCH 112/128] FMU: Default to 900 us pulses --- src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c index 600dfef412..ef2606d07d 100644 --- a/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c +++ b/src/drivers/boards/px4fmu-v4/px4fmu_pwm_servo.c @@ -70,36 +70,36 @@ __EXPORT const struct pwm_servo_channel pwm_channels[PWM_SERVO_MAX_CHANNELS] = { .gpio = GPIO_TIM1_CH4OUT, .timer_index = 0, .timer_channel = 4, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH3OUT, .timer_index = 0, .timer_channel = 3, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH2OUT, .timer_index = 0, .timer_channel = 2, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM1_CH1OUT, .timer_index = 0, .timer_channel = 1, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM4_CH2OUT, .timer_index = 1, .timer_channel = 2, - .default_value = 1000, + .default_value = 900, }, { .gpio = GPIO_TIM4_CH3OUT, .timer_index = 1, .timer_channel = 3, - .default_value = 1000, + .default_value = 900, } }; From 28754d3f58398a29c77fbec1af028273593c7d99 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Jan 2016 16:19:10 +0100 Subject: [PATCH 113/128] ekf2: don't reset GPS position in every loop --- src/modules/ekf2/ekf2_main.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a48a52bcab..a13ea5b940 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -247,6 +247,8 @@ void Ekf2::task_main() // initialise parameter cache updateParams(); + vehicle_gps_position_s gps = {}; + while (!_task_should_exit) { int ret = px4_poll(fds, sizeof(fds) / sizeof(fds[0]), 1000); @@ -277,7 +279,6 @@ void Ekf2::task_main() bool airspeed_updated = false; sensor_combined_s sensors = {}; - vehicle_gps_position_s gps = {}; airspeed_s airspeed = {}; orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors); From 563460444a2fe0c7601a5181f96633ddf1b90b8c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Jan 2016 16:19:45 +0100 Subject: [PATCH 114/128] ekf2: get the rad to deg conversion right --- src/modules/ekf2/ekf2_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf2/ekf2_main.cpp b/src/modules/ekf2/ekf2_main.cpp index a13ea5b940..38dbaf3327 100644 --- a/src/modules/ekf2/ekf2_main.cpp +++ b/src/modules/ekf2/ekf2_main.cpp @@ -376,8 +376,8 @@ void Ekf2::task_main() lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt) - lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees - lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees + lpos.ref_lat = _ekf->_posRef.lat_rad * 180.0 / M_PI; // Reference point latitude in degrees + lpos.ref_lon = _ekf->_posRef.lon_rad * 180.0 / M_PI; // Reference point longitude in degrees lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level From 6ce5e1be49526b186cfade19d370642802c90123 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Jan 2016 16:33:25 +0100 Subject: [PATCH 115/128] posix sitl: don't exit if a command fails --- src/platforms/posix/main.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 749feaa6a3..13343367fc 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -112,10 +112,16 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) int retval = apps[command](i, (char **)arg); - if (exit_on_fail && retval) { - exit(retval); + if (retval) { + cout << "Command failed, returned " << retval << endl; + + if (exit_on_fail && retval) { + exit(retval); + } } + + } else if (command.compare("help") == 0) { list_builtins(); @@ -136,7 +142,7 @@ static void usage() { cout << "./mainapp [-d] [startup_config] -h" << std::endl; - cout << " -d - Optional flag to run the app in daemon mode and does not take listen for user input." << + cout << " -d - Optional flag to run the app in daemon mode and does not listen for user input." << std::endl; cout << " This is needed if mainapp is intended to be run as a upstart job on linux" << std::endl; cout << " - config file for starting/stopping px4 modules" << std::endl; @@ -215,6 +221,7 @@ int main(int argc, char **argv) if (infile.is_open()) { for (string line; getline(infile, line, '\n');) { + // TODO: this should be true but for that we have to check all startup files process_line(line, false); } @@ -308,7 +315,7 @@ int main(int argc, char **argv) } } - process_line(mystr, !daemon_mode); + process_line(mystr, false); mystr = ""; buf_ptr_read = buf_ptr_write; break; From 166f6e2e7a5a65a8d66bb99c8cc1ac3bbf07f238 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Jan 2016 20:42:15 +0100 Subject: [PATCH 116/128] posix sitl: bring the pxh back --- src/platforms/posix/main.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/platforms/posix/main.cpp b/src/platforms/posix/main.cpp index 13343367fc..91bb6b3074 100644 --- a/src/platforms/posix/main.cpp +++ b/src/platforms/posix/main.cpp @@ -132,10 +132,6 @@ static void run_cmd(const vector &appargs, bool exit_on_fail) cout << endl << "Invalid command: " << command << "\ntype 'help' for a list of commands" << endl; } - - if (exit_on_fail) { - print_prompt(); - } } static void usage() @@ -318,6 +314,8 @@ int main(int argc, char **argv) process_line(mystr, false); mystr = ""; buf_ptr_read = buf_ptr_write; + + print_prompt(); break; case '\033': { // arrow keys From 4cb1f8a4404d9ae6b2ecf438c1a509c359f673d8 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Mon, 25 Jan 2016 14:40:44 -0500 Subject: [PATCH 117/128] Updated matrix for euler wrap fix. --- src/lib/matrix | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/matrix b/src/lib/matrix index a92d5c1338..ce30542dba 160000 --- a/src/lib/matrix +++ b/src/lib/matrix @@ -1 +1 @@ -Subproject commit a92d5c13389df17eaae1b27ed7b3e767c845bf31 +Subproject commit ce30542dbaa3e6f5e68b742a34cda993a351e15d From 6c13cef85ed32178b4dfeec626c65e9ad04f4784 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 21:46:07 +0100 Subject: [PATCH 118/128] Lister: Add missing uint16 --- Tools/generate_listener.py | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/Tools/generate_listener.py b/Tools/generate_listener.py index be6f533450..057aa9250f 100755 --- a/Tools/generate_listener.py +++ b/Tools/generate_listener.py @@ -192,9 +192,15 @@ for index,m in enumerate(messages[1:]): elif item[0] == "int32": print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) elif item[0] == "uint32": - print("\t\t\tprintf(\"%s: %%d\\n\",container.%s);" % (item[1], item[1])) - elif item[0] == "uint8": print("\t\t\tprintf(\"%s: %%u\\n\",container.%s);" % (item[1], item[1])) + elif item[0] == "int16": + print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1])) + elif item[0] == "uint16": + print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1])) + elif item[0] == "int8": + print("\t\t\tprintf(\"%s: %%d\\n\",(int)container.%s);" % (item[1], item[1])) + elif item[0] == "uint8": + print("\t\t\tprintf(\"%s: %%u\\n\",(unsigned)container.%s);" % (item[1], item[1])) elif item[0] == "bool": print("\t\t\tprintf(\"%s: %%s\\n\",container.%s ? \"True\" : \"False\");" % (item[1], item[1])) print("\t\t\t}") From fcbd717200e086824a399388a6062aa9ae70f51a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 25 Jan 2016 22:42:53 +0100 Subject: [PATCH 119/128] Switch POSIX sitl to EKF2 per default --- cmake/configs/posix_sitl_default.cmake | 1 + posix-configs/SITL/init/rcS_jmavsim_iris | 3 +-- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cmake/configs/posix_sitl_default.cmake b/cmake/configs/posix_sitl_default.cmake index 66eca5205d..2ccdb33daa 100644 --- a/cmake/configs/posix_sitl_default.cmake +++ b/cmake/configs/posix_sitl_default.cmake @@ -34,6 +34,7 @@ set(config_module_list modules/mavlink modules/attitude_estimator_ekf modules/attitude_estimator_q + modules/ekf2 modules/ekf_att_pos_estimator modules/position_estimator_inav modules/navigator diff --git a/posix-configs/SITL/init/rcS_jmavsim_iris b/posix-configs/SITL/init/rcS_jmavsim_iris index 6fd1d59db1..2b0899f4bb 100644 --- a/posix-configs/SITL/init/rcS_jmavsim_iris +++ b/posix-configs/SITL/init/rcS_jmavsim_iris @@ -51,8 +51,7 @@ sensors start commander start land_detector start multicopter navigator start -attitude_estimator_q start -position_estimator_inav start +ekf2 start mc_pos_control start mc_att_control start mixer load /dev/pwm_output0 ../../../../ROMFS/px4fmu_common/mixers/quad_x.main.mix From 17c3aa3bac9b6b817259b1e4b37f159e47df9cfb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 12:32:26 +0100 Subject: [PATCH 120/128] MC att control: Slightly increase max yaw rate --- src/modules/mc_att_control/mc_att_control_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index fc296ccf74..a6e64d49a2 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -261,7 +261,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_MAX, 220.0f); * @max 360.0 * @group Multicopter Attitude Control */ -PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 30.0f); +PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 45.0f); /** * Max acro roll rate From e11fff3011ad8fa9acd4686bcf361907028b8cb5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 12:58:17 +0100 Subject: [PATCH 121/128] Fix tests stack space --- src/systemcmds/tests/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/tests/CMakeLists.txt b/src/systemcmds/tests/CMakeLists.txt index e9cb903477..8fc1ea5451 100644 --- a/src/systemcmds/tests/CMakeLists.txt +++ b/src/systemcmds/tests/CMakeLists.txt @@ -69,7 +69,7 @@ endif() px4_add_module( MODULE systemcmds__tests MAIN tests - STACK 60000 + STACK 8000 COMPILE_FLAGS -Wframe-larger-than=6000 -Wno-float-equal From c9b1fb154d3e6ea1bbb28401688811a59c0f6543 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 26 Jan 2016 13:19:15 +0100 Subject: [PATCH 122/128] hotfix: multirotors shouldn't use tailsitter recovery code. --- src/modules/mc_att_control/mc_att_control_main.cpp | 14 ++++++++++++-- .../vtol_att_control/vtol_att_control_params.c | 9 +++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 5461f0ec97..27901dcdff 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -199,6 +199,7 @@ private: param_t vtol_type; param_t roll_tc; param_t pitch_tc; + param_t vtol_opt_recovery_enabled; } _params_handles; /**< handles for interesting parameters */ @@ -218,6 +219,7 @@ private: math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */ float rattitude_thres; int vtol_type; /**< 0 = Tailsitter, 1 = Tiltrotor, 2 = Standard airframe */ + bool vtol_opt_recovery_enabled; } _params; TailsitterRecovery *_ts_opt_recovery; /**< Computes optimal rates for tailsitter recovery */ @@ -347,6 +349,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.mc_rate_max.zero(); _params.acro_rate_max.zero(); _params.rattitude_thres = 1.0f; + _params.vtol_opt_recovery_enabled = false; _rates_prev.zero(); _rates_sp.zero(); @@ -383,11 +386,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params_handles.vtol_type = param_find("VT_TYPE"); _params_handles.roll_tc = param_find("MC_ROLL_TC"); _params_handles.pitch_tc = param_find("MC_PITCH_TC"); + _params_handles.vtol_opt_recovery_enabled = param_find("VT_OPT_RECOV_EN"); /* fetch initial parameter values */ parameters_update(); - if (_params.vtol_type == 0) { + if (_params.vtol_type == 0 && _params.vtol_opt_recovery_enabled) { // the vehicle is a tailsitter, use optimal recovery control strategy _ts_opt_recovery = new TailsitterRecovery(); } @@ -415,8 +419,10 @@ MulticopterAttitudeControl::~MulticopterAttitudeControl() } } while (_control_task != -1); } + if (_ts_opt_recovery != nullptr) { + delete _ts_opt_recovery; + } - delete _ts_opt_recovery; mc_att_control::g_control = nullptr; } @@ -489,6 +495,10 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.vtol_type, &_params.vtol_type); + int tmp; + param_get(_params_handles.vtol_opt_recovery_enabled, &tmp); + _params.vtol_opt_recovery_enabled = (bool)tmp; + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); return OK; diff --git a/src/modules/vtol_att_control/vtol_att_control_params.c b/src/modules/vtol_att_control/vtol_att_control_params.c index fe7d8ebd82..49643ee422 100644 --- a/src/modules/vtol_att_control/vtol_att_control_params.c +++ b/src/modules/vtol_att_control/vtol_att_control_params.c @@ -203,3 +203,12 @@ PARAM_DEFINE_FLOAT(VT_ARSP_BLEND, 8.0f); * @group VTOL Attitude Control */ PARAM_DEFINE_FLOAT(VT_ARSP_TRANS, 10.0f); + +/** + * Enable optimal recovery strategy for pitch-weak tailsitters + * + * @min 0 + * @max 1 + * @group VTOL Attitude Control + */ +PARAM_DEFINE_INT32(VT_OPT_RECOV_EN, 0); From 5ea5ecf32bfd4b19d8babe94d1ecc02369df3e83 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 12:35:27 +0100 Subject: [PATCH 123/128] Limit manual yaw command properly. Fixes #3600 --- .../mc_pos_control/mc_pos_control_main.cpp | 15 +++++++++++++-- 1 file changed, 13 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 966a930c2c..f322924e34 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -185,6 +185,7 @@ private: param_t man_roll_max; param_t man_pitch_max; param_t man_yaw_max; + param_t global_yaw_max; param_t mc_att_yaw_p; param_t hold_xy_dz; param_t hold_z_dz; @@ -203,6 +204,7 @@ private: float man_roll_max; float man_pitch_max; float man_yaw_max; + float global_yaw_max; float mc_att_yaw_p; float hold_xy_dz; float hold_z_dz; @@ -432,6 +434,7 @@ MulticopterPositionControl::MulticopterPositionControl() : _params_handles.man_roll_max = param_find("MPC_MAN_R_MAX"); _params_handles.man_pitch_max = param_find("MPC_MAN_P_MAX"); _params_handles.man_yaw_max = param_find("MPC_MAN_Y_MAX"); + _params_handles.global_yaw_max = param_find("MC_YAWRATE_MAX"); _params_handles.mc_att_yaw_p = param_find("MC_YAW_P"); _params_handles.hold_xy_dz = param_find("MPC_HOLD_XY_DZ"); _params_handles.hold_z_dz = param_find("MPC_HOLD_Z_DZ"); @@ -547,9 +550,12 @@ MulticopterPositionControl::parameters_update(bool force) param_get(_params_handles.man_roll_max, &_params.man_roll_max); param_get(_params_handles.man_pitch_max, &_params.man_pitch_max); param_get(_params_handles.man_yaw_max, &_params.man_yaw_max); + param_get(_params_handles.global_yaw_max, &_params.global_yaw_max); _params.man_roll_max = math::radians(_params.man_roll_max); _params.man_pitch_max = math::radians(_params.man_pitch_max); _params.man_yaw_max = math::radians(_params.man_yaw_max); + _params.global_yaw_max = math::radians(_params.global_yaw_max); + param_get(_params_handles.mc_att_yaw_p, &v); _params.mc_att_yaw_p = v; } @@ -1766,14 +1772,19 @@ MulticopterPositionControl::task_main() /* do not move yaw while sitting on the ground */ else if (!_vehicle_status.condition_landed && !(!_control_mode.flag_control_altitude_enabled && _manual.z < 0.1f)) { - const float yaw_offset_max = _params.man_yaw_max / _params.mc_att_yaw_p; - _att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max; + /* we want to know the real constraint, and global overrides manual */ + const float yaw_rate_max = (_params.man_yaw_max < _params.global_yaw_max) ? _params.man_yaw_max : _params.global_yaw_max; + const float yaw_offset_max = yaw_rate_max / _params.mc_att_yaw_p; + + _att_sp.yaw_sp_move_rate = _manual.r * yaw_rate_max; float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt); float yaw_offs = _wrap_pi(yaw_target - _yaw); // If the yaw offset became too big for the system to track stop // shifting it + + // XXX this needs inspection - probably requires a clamp, not an if if (fabsf(yaw_offs) < yaw_offset_max) { _att_sp.yaw_body = yaw_target; } From 3928924c4332083d5d282c1df387aa29c61256a8 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Tue, 26 Jan 2016 15:44:14 +0100 Subject: [PATCH 124/128] RTL was broken by a recent change, revert Revert "for multicopter landings make sure that the copter moves" This reverts commit ad1058d3742bbfa9cbd16648aa2925fa1e618a55. --- src/modules/navigator/mission.cpp | 7 +----- src/modules/navigator/mission_block.cpp | 31 +++---------------------- 2 files changed, 4 insertions(+), 34 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index cda6aaeef3..3dd2ce36d1 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -150,6 +150,7 @@ void Mission::on_active() { check_mission_valid(); + /* check if anything has changed */ bool onboard_updated = false; orb_check(_navigator->get_onboard_mission_sub(), &onboard_updated); @@ -180,12 +181,6 @@ Mission::on_active() } else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) { altitude_sp_foh_update(); - } else if (_mission_item.nav_cmd == NAV_CMD_LAND && _waypoint_position_reached && _navigator->get_vstatus()->is_rotary_wing) { - // the copter has reached the landing spot location - // set the same landing item again but this time the vehicle will actually - // descend and land - set_mission_item_reached(); - set_mission_items(); } else { /* if waypoint position reached allow loiter on the setpoint */ if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) { diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 77362a9e2b..64512d3d3d 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -92,17 +92,9 @@ MissionBlock::is_mission_item_reached() } return true; } + case NAV_CMD_LAND: - if (!_navigator->get_vstatus()->is_rotary_wing) { - return _navigator->get_vstatus()->condition_landed; - } else { - if (_waypoint_position_reached) { - // the copter has reached the position of the landing spot - // it can can start to descend - return _navigator->get_vstatus()->condition_landed; - } - } - break; + return _navigator->get_vstatus()->condition_landed; /* TODO: count turns */ /*_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||*/ @@ -167,12 +159,6 @@ MissionBlock::is_mission_item_reached() if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius()) { _waypoint_position_reached = true; } - } else if (_mission_item.nav_cmd == NAV_CMD_LAND) { - // check if we have reached our landing spot - if (dist_xy >= 0.0f && dist_xy <= _navigator->get_acceptance_radius()) { - _waypoint_position_reached = true; - return false; - } } 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 || @@ -236,7 +222,6 @@ MissionBlock::is_mission_item_reached() return true; } } - return false; } @@ -299,17 +284,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite break; case NAV_CMD_LAND: - - if (!_waypoint_position_reached) { - // we first need to move to the landing waypoint - // use the altitude specified in the mission item - sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; - sp->type = position_setpoint_s::SETPOINT_TYPE_POSITION; - } else { - sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; - sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; - } - + sp->type = position_setpoint_s::SETPOINT_TYPE_LAND; break; case NAV_CMD_LOITER_TIME_LIMIT: From ba169ce0b519af67a6f759e550e56c79780cd356 Mon Sep 17 00:00:00 2001 From: Roman Date: Tue, 26 Jan 2016 18:18:52 +0100 Subject: [PATCH 125/128] do not do jumped takeoff if already in air --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++++- 1 file changed, 7 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 f322924e34..f4287903e7 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1360,6 +1360,12 @@ MulticopterPositionControl::task_main() if (!_control_mode.flag_control_manual_enabled && _pos_sp_triplet.current.valid && _pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF) { + // check if we are not already in air. + // if yes then we don't need a jumped takeoff anymore + if (!_takeoff_jumped && !_vehicle_status.condition_landed && fabsf(_takeoff_thrust_sp) < FLT_EPSILON ) { + _takeoff_jumped = true; + } + if (!_takeoff_jumped) { // ramp thrust setpoint up if (_vel(2) > -(_params.tko_speed / 2.0f)) { @@ -1371,6 +1377,7 @@ MulticopterPositionControl::task_main() // into an integral part and into a P part thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); + _vel_sp_prev(2) = _vel(2); _takeoff_jumped = true; reset_int_z = false; } @@ -1378,7 +1385,6 @@ MulticopterPositionControl::task_main() if (_takeoff_jumped) { _vel_sp(2) = -_params.tko_speed; - _vel_sp_prev(2) = -_params.tko_speed; } } else { From b54a0308a75c4792dfacdef76a1df7f6545ae7db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 14:52:20 +0100 Subject: [PATCH 126/128] IO: Do not allocate excessive UART buffers --- nuttx-configs/px4io-v2/nsh/defconfig | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index a9b8477948..6871908315 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -164,8 +164,8 @@ CONFIG_USART1_SERIAL_CONSOLE=y CONFIG_USART2_SERIAL_CONSOLE=n CONFIG_USART3_SERIAL_CONSOLE=n -CONFIG_USART1_TXBUFSIZE=64 -CONFIG_USART2_TXBUFSIZE=64 +CONFIG_USART1_TXBUFSIZE=32 +CONFIG_USART2_TXBUFSIZE=32 CONFIG_USART3_TXBUFSIZE=64 CONFIG_USART1_RXBUFSIZE=64 From f2af8a5a5dfb3104979a6264a8c849a3168f312e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 26 Jan 2016 14:52:39 +0100 Subject: [PATCH 127/128] IO Firmware: Reduce unnecessary buffer space --- src/modules/px4iofirmware/mixer.cpp | 2 +- src/modules/px4iofirmware/px4io.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 4ff95bd5af..bbcd39c69f 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -418,7 +418,7 @@ mixer_callback(uintptr_t handle, * not loaded faithfully. */ -static char mixer_text[256]; /* large enough for one mixer */ +static char mixer_text[200]; /* large enough for one mixer */ static unsigned mixer_text_length = 0; int diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c index 1ee50d5a7f..4715cfd063 100644 --- a/src/modules/px4iofirmware/px4io.c +++ b/src/modules/px4iofirmware/px4io.c @@ -82,7 +82,7 @@ static volatile uint8_t msg_next_out, msg_next_in; * for mixer handling. Do not allocate more than 80 bytes for * output. */ -#define NUM_MSG 2 +#define NUM_MSG 1 static char msg[NUM_MSG][40]; static void heartbeat_blink(void); From f35e3335be8092d14136c82f35f48c253e0b250a Mon Sep 17 00:00:00 2001 From: Roman Date: Wed, 27 Jan 2016 11:15:22 +0100 Subject: [PATCH 128/128] - only use takeoff strategy when in auto mode - after jumped takeoff set previous vel setpoint such that thrust setpoint is continuous --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index f4287903e7..1b8503a790 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -1377,7 +1377,7 @@ MulticopterPositionControl::task_main() // into an integral part and into a P part thrust_int(2) = _takeoff_thrust_sp - _params.vel_p(2) * fabsf(_vel(2)); thrust_int(2) = -math::constrain(thrust_int(2), _params.thr_min, _params.thr_max); - _vel_sp_prev(2) = _vel(2); + _vel_sp_prev(2) = -_params.tko_speed; _takeoff_jumped = true; reset_int_z = false; } @@ -1472,7 +1472,7 @@ MulticopterPositionControl::task_main() math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int; if (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_TAKEOFF - && !_takeoff_jumped) { + && !_takeoff_jumped && !_control_mode.flag_control_manual_enabled) { // for jumped takeoffs use special thrust setpoint calculated above thrust_sp.zero(); thrust_sp(2) = -_takeoff_thrust_sp;